#include "LivoxLaser.h"
#include "livox_driver.h"


RegisterLaser(Livox)



CLivoxLaser::CLivoxLaser(int id, Laser_proto::laser_parameter laser_param)
:Laser_base(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 = -1;

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

			//以后再写.
			;
		}
	}
}

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

//雷达链接设备，为3个线程添加线程执行函数。
Error_manager CLivoxLaser::connect_laser()
{
	return Laser_base::connect_laser();
}
//雷达断开链接，释放3个线程
Error_manager CLivoxLaser::disconnect_laser()
{
	return Laser_base::disconnect_laser();
}
//对外的接口函数，负责接受并处理任务单，
//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
{
	LOG(INFO) << " ---CLivoxLaser::execute_task run  ---"<< this;
	Error_manager t_error;
	Error_manager t_result;

	//检查指针
	if (p_laser_task == NULL) {
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "Laser_base::execute_task failed, POINTER_IS_NULL");
	}
	//检查任务类型，
	if (p_laser_task->get_task_type() != LASER_BASE_SCAN_TASK)
	{
		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "laser task type error ！= LASER_BASE_SCAN_TASK");
	}



	//检查雷达状态
	t_error = check_status();
	if ( t_error != SUCCESS )
	{
		t_result.compare_and_cover_error(t_error);
	}
	else
	{
		//接受任务，并将任务的状态改为TASK_SIGNED已签收
		mp_laser_task = (Laser_task *) p_laser_task;
		mp_laser_task->set_task_statu(TASK_SIGNED);

		//检查消息内容是否正确，
		//检查三维点云指针
		if (mp_laser_task->get_task_point_cloud().get() == NULL)
		{
			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
										 Error_level::MINOR_ERROR,
										 "execute_task mp_task_point_cloud is null");
			t_result.compare_and_cover_error(t_error);
		}
		else if (mp_laser_task->get_task_cloud_lock() == NULL)
		{
			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
										Error_level::MINOR_ERROR,
										"execute_task mp_task_cloud_lock is null");
			t_result.compare_and_cover_error(t_error);
		}
		else
		{
			//扫描最大帧数,从任务单导入.
			m_frame_maxnum = mp_laser_task->get_task_frame_maxnum();

			//设置保存文件的路径
			t_error = set_open_save_path(mp_laser_task->get_task_save_path(), mp_laser_task->get_task_save_flag());
			if (t_error != Error_code::SUCCESS)
			{
				//文件保存文件的路径的设置 允许失败。继续后面的动作
				t_result.compare_and_cover_error(t_error);
			}
			//启动雷达扫描
			t_error = start_scan();
			if (t_error != Error_code::SUCCESS)
			{
				t_result.compare_and_cover_error(t_error);
			}
			else
			{
				//将任务的状态改为 TASK_WORKING 处理中
				mp_laser_task->set_task_statu(TASK_WORKING);
			}
		}

		//return 之前要填充任务单里面的错误码.
		if (t_result != Error_code::SUCCESS)
		{
			//忽略轻微故障
			if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
			{
				//将任务的状态改为 TASK_ERROR 结束错误
				mp_laser_task->set_task_statu(TASK_ERROR);
			}
			//返回错误码
			mp_laser_task->compare_and_cover_task_error_manager(t_result);

		}
	}

	return t_result;
}

//检查雷达状态,是否正常运行
Error_manager CLivoxLaser::check_status()
{
	if ( get_laser_statu() == LASER_READY && m_livox_device.device_state == K_DEVICE_STATE_CONNECT )
	{
		return Error_code::SUCCESS;
	}
	else if ( get_laser_statu() == LASER_BUSY && m_livox_device.device_state == K_DEVICE_STATE_SAMPLING )
	{
		return Error_manager(Error_code::lIVOX_STATUS_BUSY, Error_level::MINOR_ERROR,
							 " CLivoxLaser::check_status error ");
	}
	else
	{
		return Error_manager(Error_code::lIVOX_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " CLivoxLaser::check_status error ");
	}
	return Error_code::SUCCESS;
}
//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
Error_manager CLivoxLaser::start_scan()
{
	LOG(INFO) << " ---CLivoxLaser::start_scan() ---:"<<this;
	if ( is_ready() )
	{
		//清空livox子类的队列，
		m_queue_livox_data.clear_and_delete();
		m_frame_count = 0;
		Livox_driver::get_instance_references().Livox_driver_start_sample(m_livox_device.handle);
		return Laser_base::start_scan();
	}
	else
	{
		return Error_manager(Error_code::LIVOX_START_FAILE, Error_level::MINOR_ERROR,
							 "Laser_base::start_scan() is not ready ");
	}
	return Error_code::SUCCESS;
}
//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
Error_manager CLivoxLaser::stop_scan()
{
	//防止多次调用
	if ( m_laser_scan_flag )
	{
		LOG(INFO) << " ---CLivoxLaser::stop_scan() ---:"<<this;
		Livox_driver::get_instance_references().Livox_driver_stop_sample(m_livox_device.handle);
		return Laser_base::stop_scan();
	}
	return Error_code::SUCCESS;

}
//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
Error_manager CLivoxLaser::end_task()
{
	stop_scan();
	m_frame_count = 0;
	m_queue_livox_data.clear_and_delete();
	return Laser_base::end_task();
}

//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
bool CLivoxLaser::is_ready()
{
	//livox雷达设备的状态，livox sdk后台线程的状态
	if ( m_laser_statu == LASER_READY && m_livox_device.device_state == K_DEVICE_STATE_CONNECT  )
	{
		true;
	}
	else
	{
		false;
	}
}

//判断是否扫描完成
bool CLivoxLaser::is_scan_complete()
{
	//雷达的采集帧数判断，直接比较 扫描当前帧数 扫描最大帧数
	return m_frame_count >= m_frame_maxnum;
}

//往数据缓存里面添加雷达数据,
Error_manager CLivoxLaser::push_livox_data(Binary_buf* p_binary_buf)
{
	if(m_queue_livox_data.push(p_binary_buf))
	{
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::lIVOX_CANNOT_PUSH_DATA, Error_level::MINOR_ERROR,
							 " CLivoxLaser::push_livox_data error ");
	}
	return Error_code::SUCCESS;
}

//雷达帧数计数增加
Error_manager CLivoxLaser::add_livox_frame(int add_count)
{
	m_frame_count += add_count;
	return Error_code::SUCCESS;
}











void CLivoxLaser::set_livox_device(uint8_t handle, DeviceState device_state , DeviceInfo info  )
{
	m_livox_device.handle = handle;
	m_livox_device.device_state = device_state;
	m_livox_device.info = info;
}

void CLivoxLaser::set_handle(uint8_t handle)
{
	m_livox_device.handle = handle;

}

void CLivoxLaser::set_device_state(DeviceState device_state)
{
	m_livox_device.device_state = device_state;
}
CLivoxLaser::DeviceState CLivoxLaser::get_device_state()
{
	return m_livox_device.device_state;
}


void CLivoxLaser::set_device_info(DeviceInfo info)
{
	m_livox_device.info = info;

}








//接受二进制消息的功能函数，并将其存入 m_queue_laser_data, 每次只转化一个CBinaryData，
// 纯虚函数，必须由子类重载，
Error_manager CLivoxLaser::receive_buf_to_queue()
{
	Binary_buf* tp_livox_buf = NULL;
	if (m_queue_livox_data.try_pop(tp_livox_buf))
	{
		//tp_livox_buf此时是有内存的, 是 livox_data_callback 里面new出来的
		//Binary_buf 内存的管理权限    从 m_queue_livox_data 直接进入到 m_queue_laser_data,
		//数据最终在 Laser_base::thread_transform()  transform_buf_to_points  释放.
		if(m_queue_laser_data.push(tp_livox_buf))
		{
			return Error_code::SUCCESS;
		}
		else
		{
			//这里要销毁 tp_livox_buf 的内存
			delete(tp_livox_buf);
			return Error_manager(Error_code::LASER_QUEUE_ERROR, Error_level::MINOR_ERROR,
								 " CLivoxLaser::receive_buf_to_queue() error ");
		}
	}
	else
	{
		return NO_DATA;// m_queue_livox_data 没有数据并不意味着程序就出错了,
	}
	return Error_code::SUCCESS;
}
//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
// 纯虚函数，必须由子类重载，
Error_manager CLivoxLaser::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的二进制数据格式就是三维点坐标。直接强行转化指针类型即可
		LivoxRawPoint *tp_Livox_data = (LivoxRawPoint *)tp_binaty_buf->get_buf();
		//计算这一帧数据有多少个三维点。
		int t_count = tp_binaty_buf->get_length() / (sizeof(LivoxRawPoint));
		//转变三维点格式，并存入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));
			}

			//LivoxRawPoint 转为 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 NO_DATA;// m_queue_laser_data 没有数据并不意味着程序就出错了,
	}
	return Error_code::SUCCESS;
}