#include "LivoxHorizon.h"
#include "livox_driver.h"

RegisterLaser(Horizon)



CHorizonLaser::CHorizonLaser(int id, Laser_proto::laser_parameter laser_param)
:CLivoxLaser(id, laser_param)
{
	Error_manager t_error;
	m_frame_count = 0;
	m_frame_maxnum = laser_param.frame_num();//初始化从外部参数导入,实际运行前从任务单导入.

	//默认handle为-1,切记不能初始化为0.因为0表示0号雷达,是有效的.....
	m_livox_device.handle = 255;

	//判断参数类型，
	if(laser_param.type()=="Horizon")
	{
		//填充雷达设备的广播码
		t_error = Livox_driver::get_instance_references().Livox_insert_sn_laser(laser_param.sn(), this);
		//此时不进行驱动层的初始化,在所有雷达创建完成之后,在laser_manager统一初始化.
		if ( t_error != SUCCESS)
		{
			//如果sn码不规范,或者sn码重复都会引发错误,
			//故障处理,
			// 方案一:回退.取消创建雷达的操作.释放雷达内存.
			//方案二:允许创建雷达,但是状态改为故障.禁用后续的功能.

			//以后再写.
			;
		}
	}
}

CHorizonLaser::~CHorizonLaser()
{
	//清空队列
	m_queue_livox_data.clear_and_delete();
}

//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
// 纯虚函数，必须由子类重载，
Error_manager CHorizonLaser::transform_buf_to_points()
{
	Error_manager t_error;
	Binary_buf* tp_binaty_buf=NULL;
	//从队列中取出缓存。 tp_binaty_buf的内存 从链表取出时，就自带内存了。需要手动释放。
	if(m_queue_laser_data.try_pop(tp_binaty_buf))
	{
		//livox的二进制数据格式就是三维点坐标。直接强行转化指针类型即可
		LivoxExtendRawPoint *tp_Livox_data = (LivoxExtendRawPoint *)tp_binaty_buf->get_buf();
		//计算这一帧数据有多少个三维点。
		int t_count = tp_binaty_buf->get_length() / (sizeof(LivoxExtendRawPoint));
		//转变三维点格式，并存入vector。
		for (int i = 0; i < t_count; ++i)
		{
			//三维点的坐标变换，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
			CPoint3D point_source;
			point_source.x = tp_Livox_data[i].x;
			point_source.y = tp_Livox_data[i].y;
			point_source.z = tp_Livox_data[i].z;
			CPoint3D point_destination = transform_by_matrix(point_source);

			//保存雷达扫描 三维点云的最终结果。
			if(m_save_flag)
			{
				char buf[64] = {0};
				sprintf(buf, "%f %f %f\n", point_destination.x, point_destination.y, point_destination.z);
				m_points_log_tool.write(buf, strlen(buf));
			}

			//LivoxExtendRawPoint 转为 pcl::PointXYZ
			//int32_t 转 double。不要信号强度
			pcl::PointXYZ  pointXYZ;
			pointXYZ.x = point_destination.x/1000.0;
			pointXYZ.y = point_destination.y/1000.0;
			pointXYZ.z = point_destination.z/1000.0;
			//注:单位毫米转为米, 最终数据单位统一为米.....
			t_error =  mp_laser_task->task_push_point(&pointXYZ);
			if ( t_error != Error_code::SUCCESS )
			{
				delete tp_binaty_buf;
				return t_error;
			}
		}
		//统计扫描点个数。
		m_points_count += t_count;

		delete tp_binaty_buf;
	}
	else
	{
		return NODATA;// m_queue_laser_data 没有数据并不意味着程序就出错了,
	}
	return Error_code::SUCCESS;
}