#include "./laser_manager.h"

#include "../tool/proto_tool.h"
#include "../laser/laser_parameter.pb.h"
#include "livox_driver.h"

#include <livox_sdk.h>

Laser_manager::Laser_manager()
{
	m_laser_manager_status = LASER_MANAGER_UNKNOW;
	m_laser_number = 0;
	m_laser_manager_working_flag = false;
	mp_laser_manager_thread = NULL;

	mp_laser_manager_task = NULL;
	m_laser_number = 0;
}
Laser_manager::~Laser_manager()
{
	laser_manager_uninit();
}

//初始化 雷达 管理模块。如下三选一
Error_manager Laser_manager::laser_manager_init()
{
	return  laser_manager_init_from_protobuf(LASER_PARAMETER_PATH);
}
//初始化 雷达 管理模块。从文件读取
Error_manager Laser_manager::laser_manager_init_from_protobuf(std::string prototxt_path)
{
	Laser_proto::Laser_parameter_all t_laser_parameters;

	if(!  proto_tool::read_proto_param(prototxt_path,t_laser_parameters) )
	{
		return Error_manager(LASER_MANAGER_READ_PROTOBUF_ERROR,MINOR_ERROR,"Laser_manager read_proto_param  failed");
	}

	return laser_manager_init_from_protobuf(t_laser_parameters);
}
//初始化 雷达 管理模块。从protobuf读取
Error_manager Laser_manager::laser_manager_init_from_protobuf(Laser_proto::Laser_parameter_all& laser_parameters)
{
	LOG(INFO) << " ---laser_manager_init_from_protobuf run--- "<< this;
	Error_manager t_error;
	m_laser_number = laser_parameters.laser_parameters_size();
	m_laser_manager_working_flag = false;

	//创建大疆雷达
	int laser_cout=laser_parameters.laser_parameters_size();
	m_laser_vector.resize(laser_cout);
	for(int i=0;i<laser_parameters.laser_parameters_size();++i)
	{
		//创建雷达, CreateLaser 会为雷达实例new内存
		m_laser_vector[i]=LaserRegistory::CreateLaser(laser_parameters.laser_parameters(i).type(),i,
													  laser_parameters.laser_parameters(i));
		if(m_laser_vector[i]!=NULL)
		{
			if(m_laser_vector[i]->connect_laser()!=SUCCESS)
			{
				char description[255]={0};
				sprintf(description,"Laser %d connect failed...",i);
				return Error_manager(LASER_CONNECT_FAILED,MINOR_ERROR,description);
			}
		}
	}
	//查询是否有livox雷达,初始化 Livox_driver
	for (int i = 0; i < laser_parameters.laser_parameters_size(); ++i)
	{
		std::string type = laser_parameters.laser_parameters(i).type();
		if (type.find("Livox") != type.npos)
		{
			t_error = Livox_driver::get_instance_references().Livox_driver_init() ;
			if ( t_error != Error_code::SUCCESS )
			{
				return t_error;
			}
			break;
		}
	}

	m_laser_manager_status = LASER_MANAGER_READY;

	//启动雷达管理模块的内部线程。默认wait。
	m_laser_manager_condition.reset(false, false, false);
	mp_laser_manager_thread = new std::thread(&Laser_manager::thread_work, this);

	return Error_code::SUCCESS;
}
//反初始化 雷达 管理模块。
Error_manager Laser_manager::laser_manager_uninit()
{
	LOG(INFO) << " ---laser_manager_uninit run--- "<< this;
	//关闭线程
	if (mp_laser_manager_thread)
	{
		m_laser_manager_condition.kill_all();
	}
	//回收线程的资源
	if (mp_laser_manager_thread)
	{
		mp_laser_manager_thread->join();
		delete mp_laser_manager_thread;
		mp_laser_manager_thread = NULL;
	}

	//回收雷达的内存
	for (int i = 0; i < m_laser_number; ++i)
	{

		delete(m_laser_vector[i]);
	}
	m_laser_manager_status = LASER_MANAGER_UNKNOW;
	m_laser_number = 0;
	m_laser_vector.clear();
	m_laser_manager_working_flag = false;
	//反初始化 Livox_driver
	Livox_driver::get_instance_references().Livox_driver_uninit();

	//回收下发雷达任务单
	for (map<int, Laser_task*>::iterator iter  = m_laser_task_map.begin(); iter != m_laser_task_map.end(); )
	{

		delete(iter->second);
		iter = m_laser_task_map.erase(iter);
		//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
		//for循环不用 ++iter
	}
	return Error_code::SUCCESS;
}



//对外的接口函数，负责接受并处理任务单，
//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
Error_manager Laser_manager::execute_task(Task_Base* p_laser_task)
{
	LOG(INFO) << " ---Laser_manager::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_manager::execute_task failed, POINTER_IS_NULL");
	}
	//检查任务类型，
	if (p_laser_task->get_task_type() != LASER_MANGER_SCAN_TASK)
	{
		return Error_manager(Error_code::LASER_MANAGER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "laser task type error ！= LASER_MANGER_SCAN_TASK ");
	}

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

		//检查消息内容是否正确，
		//检查三维点云指针
		if (mp_laser_manager_task->get_task_point_cloud_map() == 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_manager_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_laser_manager_status = LASER_MANAGER_ISSUED_TASK;
			m_laser_manager_working_flag = true;
			m_laser_manager_condition.notify_all(true);
			//通知 thread_work 子线程启动。

			//将任务的状态改为 TASK_WORKING 处理中
			mp_laser_manager_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_manager_task->set_task_statu(TASK_ERROR);
			}
			//返回错误码
			mp_laser_manager_task->compare_and_cover_task_error_manager(t_result);
		}
	}

	return t_result;

}

//检查雷达状态,是否正常运行
Error_manager Laser_manager::check_status()
{
	if ( m_laser_manager_status == LASER_MANAGER_READY )
	{
		return Error_code::SUCCESS;
	}
	else if ( m_laser_manager_status == LASER_MANAGER_ISSUED_TASK || m_laser_manager_status == LASER_MANAGER_WAIT_REPLY )
	{
		return Error_manager(Error_code::LASER_MANAGER_STATUS_BUSY, Error_level::MINOR_ERROR,
							 " Laser_manager::check_status error ");
	}
	else
	{
		return Error_manager(Error_code::LASER_MANAGER_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " Laser_manager::check_status error ");
	}
	return Error_code::SUCCESS;
}
//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
Error_manager Laser_manager::end_task()
{
	LOG(INFO) << " ---Laser_manager::end_task run---"<< this;

	//关闭子线程
	m_laser_manager_working_flag=false;
	m_laser_manager_condition.notify_all(false);


	//释放下发的任务单
	laser_task_map_clear_and_delete();

	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
	if(mp_laser_manager_task !=NULL)
	{
		//判断任务单的错误等级，
		if ( mp_laser_manager_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
		{
			if ( m_laser_manager_status == LASER_MANAGER_ISSUED_TASK  || m_laser_manager_status == LASER_MANAGER_WAIT_REPLY )
			{
				//故障等级为 NORMAL NEGLIGIBLE_ERROR MINOR_ERROR，则恢复待机，可以接受下一次的任务单
				m_laser_manager_status = LASER_MANAGER_READY;
			}
			//else 状态不变

			//强制改为TASK_OVER，不管它当前在做什么。
			mp_laser_manager_task->set_task_statu(TASK_OVER);
		}
		else
		{
			//故障等级为 MAJOR_ERROR CRITICAL_ERROR，则雷达故障，不可以再接受任务单。
			m_laser_manager_status = LASER_MANAGER_FAULT;

			//强制改为 TASK_ERROR，不管它当前在做什么。
			mp_laser_manager_task->set_task_statu(TASK_ERROR);
		}
	}
//	std::cout << "------Laser_manager::end_task()------."<<mp_laser_manager_task->get_task_error_manager().to_string() << std::endl;
	return Error_code::SUCCESS;
}

//取消任务单，由发送方提前取消任务单
Error_manager Laser_manager::cancel_task()
{
	//需要先取消子任务.
	map<int, Laser_task*>::iterator map_iter;
	for (  map_iter = m_laser_task_map.begin(); map_iter != m_laser_task_map.end(); ++map_iter)
	{
			m_laser_vector[map_iter->first]->cancel_task();
			map_iter->second->set_task_statu(TASK_DEAD);
	}

	end_task();
	//强制改为 TASK_DEAD，不管它当前在做什么。
	mp_laser_manager_task->set_task_statu(TASK_DEAD);

	return Error_code::SUCCESS;
}


//释放下发的任务单
Error_manager Laser_manager::laser_task_map_clear_and_delete()
{
	for ( map<int, Laser_task*>::iterator map_iter2 = m_laser_task_map.begin(); map_iter2 != m_laser_task_map.end(); )
	{
		//销毁下发的任务单。
		delete(map_iter2->second);
		map_iter2 = m_laser_task_map.erase(map_iter2);
		//注：erase 删除当前map_iter2之后返回下一个节点，当前的map_iter2无效化，
		//for循环不用 ++map_iter2
	}
	return Error_code::SUCCESS;
}

//判断是否为待机，如果已经准备好，则可以执行任务。
bool Laser_manager::is_ready()
{
	return m_laser_manager_status == LASER_MANAGER_READY;
}

Laser_manager::Laser_manager_status Laser_manager::get_laser_manager_status()
{
	return m_laser_manager_status;
}

std::vector<Laser_base*> & Laser_manager::get_laser_vector()
{
	return m_laser_vector;
}


//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
Error_manager Laser_manager::issued_task(int laser_index)
{
	Error_manager t_error;

	if ( laser_index < 0 || laser_index >= m_laser_number )
	{
		return Error_manager(Error_code::LASER_MANAGER_LASER_INDEX_ERRPR, Error_level::MINOR_ERROR,
							 " issued_task laser_index  error ");
	}

	//查重
	map<int, Laser_task*>::iterator map_iter = m_laser_task_map.find(laser_index);
	if ( map_iter != m_laser_task_map.end() )
	{
		//如果重复，则不做任何处理,视为无效行为.
		//返回轻微错误.仅仅提示作用.
		return Error_manager(Error_code::LASER_MANAGER_LASER_INDEX_REPEAT, Error_level::NEGLIGIBLE_ERROR,
							" issued_task  laser_index repeart ");
	}
	else
	{
		//创建雷达扫描任务,
		// 这里为下发雷达任务分配内存，后续要记得回收。
		Laser_task* t_laser_task=new Laser_task();
		mp_laser_manager_task->trans_to_laser_task(t_laser_task,laser_index,m_laser_vector[laser_index],
												   TASK_CREATED,std::chrono::milliseconds(7000));
		//保存雷达扫描任务 到 m_laser_task_map
		m_laser_task_map[laser_index] = t_laser_task;

		//发送任务单给雷达
		t_error = Task_command_manager::get_instance_references().execute_task(t_laser_task);
	}



	if(t_error!=SUCCESS)
	{
		;
		//这里的故障处理很复杂，以后再写。
	}

	return t_error;

}

//故障处理,下发任务的故障处理.
Error_manager Laser_manager::troubleshoot_for_issued_task(Error_manager error, int laser_index)
{
	if (error == LASER_MANAGER_LASER_INDEX_ERRPR || error == LASER_MANAGER_LASER_INDEX_REPEAT   )
	{
	    //雷达索引错误,降级为可忽略的故障,然后返回轻微故障.
		error.set_error_level_down(NEGLIGIBLE_ERROR);
		// 此时没有创建子任务,后面等待任务返回时会忽略掉.
		return error;
	}
	if (error == POINTER_IS_NULL || error == LIVOX_TASK_TYPE_ERROR   )
	{
		//雷达对象接受任务,发现任务单下发错误.应该是整个任务通信模块错误了.将主任务错误码升级
		error.set_error_level_up(CRITICAL_ERROR);
		return error;
	}
	//其他情况雷达对象会签收任务,之后出错会填充到任务单里面的错误码.
	//即使接收方的状态不对,也会签收任务,然后把错误填充到任务单里面的错误码.
	//后续在等待答复时,会统一处理子任务单所有的错误.


	return error;
}

//mp_laser_manager_thread 线程执行函数，
//thread_work 内部线程负责分发扫描任务给下面的雷达，并且回收汇总雷达的数据
void Laser_manager::thread_work()
{
	LOG(INFO) << " mp_laser_manager_thread start "<< this;
	Error_manager t_error;
	Error_manager t_result;

	//雷达管理的独立线程，负责控制管理所有的雷达。
	while (m_laser_manager_condition.is_alive())
	{
		m_laser_manager_condition.wait();
		if ( m_laser_manager_condition.is_alive() )
		{
			std::this_thread::yield();
			//重新循环必须清除错误码.
			t_error.error_manager_clear_all();
			t_result.error_manager_clear_all();

			if ( mp_laser_manager_task == NULL )
			{
				m_laser_manager_status = LASER_MANAGER_FAULT;
				m_laser_manager_working_flag = false;
				m_laser_manager_condition.notify_all(false);
			}
			//第一步
			//下发任务, 雷达管理模块给子雷达下发扫描任务.
			else if(m_laser_manager_status == LASER_MANAGER_ISSUED_TASK)
			{
				//分发扫描任务给下面的雷达
				if ( mp_laser_manager_task->get_select_all_laser_flag() )
				{
					//下发任务给所有的雷达
					for (int i = 0; i < m_laser_number; ++i)
					{
						//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
						t_error = issued_task(i);
						if ( t_error != SUCCESS)
						{
							//下发子任务的故障处理
							t_error = troubleshoot_for_issued_task(t_error, i);
							t_result.compare_and_cover_error(t_error);
						}
					}
				}
				else
				{
					//下发任务给选中的雷达
					std::vector<int> & t_select_laser_id_vector = mp_laser_manager_task->get_select_laser_id_vector();
					for (int i = 0; i < t_select_laser_id_vector.size(); ++i)
					{
						//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
						t_error = issued_task(t_select_laser_id_vector[i]);
						if ( t_error != SUCCESS)
						{
							//下发子任务的故障处理
							t_error = troubleshoot_for_issued_task(t_error, i);
							t_result.compare_and_cover_error(t_error);
						}
					}
				}

				//故障汇总, 只处理2级以上的故障
				if(t_result.get_error_level() >= MINOR_ERROR)
				{
					m_laser_manager_status = LASER_MANAGER_FAULT;
					mp_laser_manager_task->set_task_statu(TASK_ERROR);
					//因为故障,而提前结束任务.
					mp_laser_manager_task->compare_and_cover_task_error_manager(t_result);
					end_task();
				}
				else
				{
					//下发任务之后，修改状态为等待答复。
					m_laser_manager_status = LASER_MANAGER_WAIT_REPLY;
				}
			}
			//第二步
			//等待答复, 注意不能写成死循环阻塞,
			//子任务没完成就直接通过, 然后重新进入  while (m_laser_manager_condition.is_alive()),
			// 这样线程仍然被 m_laser_manager_condition 控制
			else if(m_laser_manager_status == LASER_MANAGER_WAIT_REPLY)
			{
				map<int, Laser_task*>::iterator map_iter;
				for (  map_iter = m_laser_task_map.begin(); map_iter != m_laser_task_map.end(); ++map_iter)
				{

					if ( map_iter->second->is_task_end() == false)
					{
						if ( map_iter->second->is_over_time() )
						{
							//超时处理。取消任务。
							m_laser_vector[map_iter->first]->cancel_task();
							map_iter->second->set_task_statu(TASK_DEAD);
							t_error.error_manager_reset(Error_code::LASER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
														" LASER_TASK is_over_time ");
							map_iter->second->set_task_error_manager(t_error);

							continue;
							//本次子任务超时死亡.直接进行下一个
						}
						else
						{
							//如果任务还在执行, 那就等待任务继续完成，等1ms
							//std::this_thread::sleep_for(std::chrono::milliseconds(1));
							std::this_thread::yield();
							break;
							//注意了:这里break之后,map_iter不会指向 m_laser_task_map.end()
							//这就表示仍然有子任务单没有结束.那就继续等待,,,直到所有任务完成或者超时.
						}

					}
					//else 任务完成就判断下一个
				}

				//如果下发的任务全部结束，那么汇总所有的结果, 并销毁下发的任务单。
				if(map_iter == m_laser_task_map.end() )
				{
					for ( map<int, Laser_task*>::iterator map_iter2 = m_laser_task_map.begin(); map_iter2 != m_laser_task_map.end(); )
					{
						//数据汇总，
						//由于 pcl::PointCloud<pcl::PointXYZ>::Ptr mp_task_point_cloud; 是所有任务单共用的，所以不用处理了。自动就在一起。

						//错误汇总
						t_result.compare_and_cover_error(map_iter2->second->get_task_error_manager());

						//销毁下发的任务单。
						delete(map_iter2->second);
						map_iter2 = m_laser_task_map.erase(map_iter2);
						//注：erase 删除当前map_iter2之后返回下一个节点，当前的map_iter2无效化，
						//for循环不用 ++map_iter2
					}

					m_laser_manager_working_flag = false;
					mp_laser_manager_task->compare_and_cover_task_error_manager(t_result);
					//正常结束任务
					end_task();
				}
				//else 直接通过, 然后重新进入  while (m_laser_manager_condition.is_alive())
			}
		}
	}
	LOG(INFO) << " mp_laser_manager_thread end :"<<this;
	return;
}