#include "LivoxMid100Laser.h"
#include <glog/logging.h>
RegisterLaser(LivoxMid100)


CLivoxMid100Laser::CLivoxMid100Laser(int id, Laser_proto::laser_parameter laser_param)
:CLivoxLaser(id, laser_param)
{
	//设备livox扫描最大帧数
    m_frame_maxnum = laser_param.frame_num();
	//判断参数类型，
    if (laser_param.type() == "LivoxMid100")
    {
        std::string sn = laser_param.sn();
        std::string sn1 = sn, sn2 = sn, sn3 = sn;
        sn1 += "1";
        sn2 += "2";
        sn3 += "3";
        g_sn_laser.insert(std::make_pair(sn1, this));
        g_sn_laser.insert(std::make_pair(sn2, this));
        g_sn_laser.insert(std::make_pair(sn3, this));
		//初始化livox
		InitLivox();
    }
}
CLivoxMid100Laser::~CLivoxMid100Laser()
{

}

//雷达链接设备，为3个线程添加线程执行函数。
Error_manager CLivoxMid100Laser::connect_laser()
{
	Error_manager t_error;
	//设置点云变换矩阵
	double matrix[12]={0};
	matrix[0]=m_laser_param.mat_r00();
	matrix[1]=m_laser_param.mat_r01();
	matrix[2]=m_laser_param.mat_r02();
	matrix[3]=m_laser_param.mat_r03();

	matrix[4]=m_laser_param.mat_r10();
	matrix[5]=m_laser_param.mat_r11();
	matrix[6]=m_laser_param.mat_r12();
	matrix[7]=m_laser_param.mat_r13();

	matrix[8]=m_laser_param.mat_r20();
	matrix[9]=m_laser_param.mat_r21();
	matrix[10]=m_laser_param.mat_r22();
	matrix[11]=m_laser_param.mat_r23();
	t_error = set_laser_matrix(matrix, 12);
	if ( t_error != Error_code::SUCCESS )
	{
		return t_error;
	}
	return CLivoxLaser::connect_laser();
}
//雷达断开链接，释放3个线程
Error_manager CLivoxMid100Laser::disconnect_laser()
{
	return CLivoxLaser::disconnect_laser();
}



//对外的接口函数，负责接受并处理任务单，
//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
Error_manager CLivoxMid100Laser::execute_task(Task_Base* p_laser_task)
{
	LOG(INFO) << " CLivoxMid100Laser::execute_task start  "<< 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::porform_task failed, POINTER_IS_NULL");
	}
	//检查任务类型，
	if (p_laser_task->get_task_type() != LASER_TASK)
	{
		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "laser task type error ！= LASER_TASK");
	}
    //检查当前状态
    t_error=check_laser();
	if(t_error!=SUCCESS)
    {
	    return t_error;
    }
	//接受任务，并将任务的状态改为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() == NULL)
	{
		t_result.error_manager_reset(Error_code::POINTER_IS_NULL,
									 Error_level::MINOR_ERROR,
									 "execute_task mp_task_point_cloud is null");
		//将任务的状态改为 TASK_OVER 结束任务
		mp_laser_task->set_task_statu(TASK_OVER);
		//返回错误码
		mp_laser_task->set_task_error_manager(t_result);
		return t_result;
	}
	else
	{
		m_frame_maxnum=mp_laser_task->get_task_frame_maxnum();


		//设置保存文件的路径
		std::string save_path = mp_laser_task->get_save_path();
		t_error = set_open_save_path(save_path);
		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);

			//将任务的状态改为 TASK_OVER 结束任务
			mp_laser_task->set_task_statu(TASK_OVER);
			//返回错误码
			mp_laser_task->set_task_error_manager(t_result);
			return t_result;
		}
		else
		{
			//将任务的状态改为 TASK_WORKING 处理中
			mp_laser_task->set_task_statu(TASK_WORKING);
		}
	}
	//返回错误码
	if (t_result != Error_code::SUCCESS)
	{
		mp_laser_task->set_task_error_manager(t_result);
	}
	return t_result;
}

//检查雷达状态,是否正常运行
Error_manager CLivoxMid100Laser::check_laser()
{
	return CLivoxLaser::check_laser();
}

//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
Error_manager CLivoxMid100Laser::start_scan()
{
	LOG(INFO) << " livoxMid100 start :" << m_laser_param.sn();
	//清空livox子类的队列，
	m_queue_livox_data.clear_and_delete();
	g_count[m_handle1] = 0;
	g_count[m_handle2] = 0;
	g_count[m_handle3] = 0;
	return CLivoxLaser::start_scan();
}
//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
Error_manager CLivoxMid100Laser::stop_scan()
{
	LOG(INFO)<<" livoxmid100 stoped !!!"<<m_laser_param.sn();
	return CLivoxLaser::stop_scan();
}
//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
Error_manager CLivoxMid100Laser::end_task()
{
	return CLivoxLaser::end_task();
}
//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
bool CLivoxMid100Laser::is_ready()
{
	//3个livox雷达设备的状态，livox sdk后台线程的状态
	bool cond1=g_devices[m_handle1].device_state == kDeviceStateConnect ||
			   g_devices[m_handle1].device_state == kDeviceStateSampling;
	bool cond2=g_devices[m_handle2].device_state == kDeviceStateConnect ||
			   g_devices[m_handle2].device_state == kDeviceStateSampling;
	bool cond3=g_devices[m_handle3].device_state == kDeviceStateConnect ||
			   g_devices[m_handle3].device_state == kDeviceStateSampling;

	if ( cond1 && cond2 && cond3 && m_laser_statu == LASER_READY )
	{
		true;
	}
	else
	{
		false;
	}
}


void CLivoxMid100Laser::UpdataHandle()
{
	std::string sn = m_laser_param.sn();
	std::string sn1 = sn, sn2 = sn, sn3 = sn;
	sn1 += "1";
	sn2 += "2";
	sn3 += "3";
	if (g_sn_handle.find(sn1) != g_sn_handle.end())
	{
		m_handle1 = g_sn_handle[sn1];
	}
	if (g_sn_handle.find(sn2) != g_sn_handle.end())
	{
		m_handle2 = g_sn_handle[sn2];
	}
	if (g_sn_handle.find(sn3) != g_sn_handle.end())
	{
		m_handle3 = g_sn_handle[sn3];
	}
}
bool CLivoxMid100Laser::IsScanComplete()
{
	//雷达的采集帧数判断，直接比较任务单里面的帧数最大值
	if(mp_laser_task==NULL)
	{
		return false;
	}
	else
	{
		int frame_count=mp_laser_task->get_task_frame_maxnum();
		return g_count[m_handle1] >= frame_count && g_count[m_handle2] >= frame_count
			   && g_count[m_handle2] >= frame_count;
	}
}