//
// Created by huli on 2020/9/9.
//

#include "wanji_manager.h"
#include "../tool/proto_tool.h"

Wanji_manager::Wanji_manager()
{
	m_wanji_manager_status = E_UNKNOW;

	mp_cloud_collection = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
	m_cloud_updata_time = std::chrono::system_clock::now() - std::chrono::hours(1);
	mp_collect_cloud_thread = NULL;

	mp_execute_thread = NULL;
	mp_wanji_manager_task = NULL;
	mp_task_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
}
Wanji_manager::~Wanji_manager()
{
	wanji_manager_uninit();
}


//初始化 雷达 管理模块。如下三选一
Error_manager Wanji_manager::wanji_manager_init()
{
	return  wanji_manager_init_from_protobuf(WANJI_MANAGER_PARAMETER_PATH);
}
//初始化 雷达 管理模块。从文件读取
Error_manager Wanji_manager::wanji_manager_init_from_protobuf(std::string prototxt_path)
{
	wj::wjManagerParams t_wanji_parameters;

	if(!  proto_tool::read_proto_param(prototxt_path,t_wanji_parameters) )
	{
		return Error_manager(WJ_MANAGER_READ_PROTOBUF_ERROR,MINOR_ERROR,"Wanji_manager read_proto_param  failed");
	}

	return wanji_manager_init_from_protobuf(t_wanji_parameters);
}
//初始化 雷达 管理模块。从protobuf读取
Error_manager Wanji_manager::wanji_manager_init_from_protobuf(wj::wjManagerParams& wanji_parameters)
{
	LOG(INFO) << " ---wanji_manager_init_from_protobuf run--- "<< this;
	Error_manager t_error;
	if ( m_wanji_manager_status != E_UNKNOW)
	{
		return Error_manager(Error_code::WJ_MANAGER_INIT_REPEAT, Error_level::MINOR_ERROR,
							 " Wanji_manager::wanji_manager_init_from_protobuf init repeat error ");
	}

	//创建雷达设备
	int t_wanji_lidar_size = wanji_parameters.wj_lidar_size();
	for(int i=0;i<t_wanji_lidar_size;++i)
	{
		Wanji_lidar_device* p_wanji_lidar_device(new Wanji_lidar_device);
		t_error = p_wanji_lidar_device->init(wanji_parameters.wj_lidar(i));
		if ( t_error != Error_code::SUCCESS )
		{
			delete(p_wanji_lidar_device);
			return t_error;
		}
		m_wanji_lidar_device_map[i] = (p_wanji_lidar_device);
	}

	//创建预测算法
	int t_region_workers_size = wanji_parameters.regions_size();
	for(int i=0;i<t_region_workers_size;++i)
	{
		Region_worker* p_region_worker(new Region_worker);
		Point2D_tool::Point2D_box t_point2d_box;
		t_point2d_box.x_min = wanji_parameters.regions(i).minx();
		t_point2d_box.x_max = wanji_parameters.regions(i).maxx();
		t_point2d_box.y_min = wanji_parameters.regions(i).miny();
		t_point2d_box.y_min = wanji_parameters.regions(i).maxy();
		t_error = p_region_worker->init(t_point2d_box, &m_cloud_collection_mutex, mp_cloud_collection);
		if ( t_error != Error_code::SUCCESS )
		{
			delete(p_region_worker);
			return t_error;
		}
		m_region_worker_map[i] = p_region_worker;
	}



	//启动 收集点云的线程。默认 。
	m_collect_cloud_condition.reset(false, false, false);
	mp_collect_cloud_thread = new std::thread(&Wanji_manager::collect_cloud_thread_fun, this);

	//启动 执行的线程。默认 。
	m_execute_condition.reset(false, false, false);
	mp_execute_thread = new std::thread(&Wanji_manager::execute_thread_fun, this);

	m_wanji_manager_status = E_READY;

	return Error_code::SUCCESS;
}
//反初始化 雷达 管理模块。
Error_manager Wanji_manager::wanji_manager_uninit()
{
	LOG(INFO) << " ---wanji_manager_uninit run--- "<< this;
	//关闭线程
	if (mp_collect_cloud_thread)
	{
		m_collect_cloud_condition.kill_all();
	}
	if (mp_execute_thread)
	{
		m_execute_condition.kill_all();
	}
	//回收线程的资源
	if (mp_collect_cloud_thread)
	{
		mp_collect_cloud_thread->join();
		delete mp_collect_cloud_thread;
		mp_collect_cloud_thread = NULL;
	}
	if (mp_execute_thread)
	{
		mp_execute_thread->join();
		delete mp_execute_thread;
		mp_execute_thread = NULL;
	}
	//回收雷达设备
	for (auto iter = m_wanji_lidar_device_map.begin(); iter != m_wanji_lidar_device_map.end(); ++iter)
	{
		delete(iter->second);
	}
	m_wanji_lidar_device_map.clear();

	for (auto iter = m_region_worker_map.begin(); iter != m_region_worker_map.end(); ++iter)
	{
		delete(iter->second);
	}
	m_region_worker_map.clear();

	m_wanji_manager_status = E_UNKNOW;

	//回收下发雷达任务单
	wanji_lidar_scan_task_map_clear_and_delete();

	return Error_code::SUCCESS;
}


//对外的接口函数，负责接受并处理任务单，
Error_manager Wanji_manager::execute_task(Task_Base* p_wanji_manager_task)
{
	LOG(INFO) << " ---Wanji_manager::execute_task run---  "<< this;
	Error_manager t_error;
	Error_manager t_result;

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

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

//启动雷达管理模块，的核心工作线程
		m_wanji_manager_status = E_ISSUED_SCAN;
		m_execute_condition.notify_all(true);

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

	return t_result;
}

//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
Error_manager Wanji_manager::end_task()
{
	m_execute_condition.notify_all(false);

	//释放下发的任务单
	wanji_lidar_scan_task_map_clear_and_delete();
	{
		std::unique_lock<std::mutex> t_lock(m_task_cloud_mutex);
		mp_task_cloud->clear();
	}

	if ( m_wanji_manager_status == E_BUSY ||
	m_wanji_manager_status == E_ISSUED_SCAN ||
	m_wanji_manager_status == E_WAIT_SCAN ||
	m_wanji_manager_status == E_ISSUED_DETECT ||
	m_wanji_manager_status == E_WAIT_DETECT )
	{
		m_wanji_manager_status = E_READY;
	}
	//else 状态不变

	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
	if(mp_wanji_manager_task !=NULL)
	{
		//判断任务单的错误等级，
		if ( mp_wanji_manager_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
		{
			//强制改为TASK_OVER，不管它当前在做什么。
			mp_wanji_manager_task->set_task_statu(TASK_OVER);
		}
		else
		{
			//强制改为 TASK_ERROR，不管它当前在做什么。
			mp_wanji_manager_task->set_task_statu(TASK_ERROR);
		}
	}

	return Error_code::SUCCESS;
}
//取消任务单，由发送方提前取消任务单
Error_manager Wanji_manager::cancel_task(Task_Base* p_wanji_manager_task)
{
}

//检查雷达状态,是否正常运行
Error_manager Wanji_manager::check_status()
{
	if ( m_wanji_manager_status == E_READY )
	{
		return Error_code::SUCCESS;
	}
	else if ( m_wanji_manager_status == E_BUSY ||
			  m_wanji_manager_status == E_ISSUED_SCAN ||
			  m_wanji_manager_status == E_WAIT_SCAN ||
			  m_wanji_manager_status == E_ISSUED_DETECT ||
			  m_wanji_manager_status == E_WAIT_DETECT )
	{
		return Error_manager(Error_code::WJ_MANAGER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
							 " Laser_manager::check_status error ");
	}
	else
	{
		return Error_manager(Error_code::WJ_MANAGER_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " Laser_manager::check_status error ");
	}
	return Error_code::SUCCESS;
}
//判断是否为待机，如果已经准备好，则可以执行任务。
bool Wanji_manager::is_ready()
{
	return  m_wanji_manager_status == E_READY;
}

Wanji_manager::Wanji_manager_status Wanji_manager::get_status()
{
	return m_wanji_manager_status;
}
std::map<int, Wanji_lidar_device*> & Wanji_manager::get_wanji_lidar_device_map()
{
	return m_wanji_lidar_device_map;
}
std::map<int, Region_worker *> & Wanji_manager::get_region_worker_map()
{
	return m_region_worker_map;
}






//自动收集点云的线程函数
void Wanji_manager::collect_cloud_thread_fun()
{
	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() start "<< this;
	Error_manager t_error;
	Error_manager t_result;

	while (m_collect_cloud_condition.is_alive())
	{
		//暂时固定为一个扫描周期, 就循环一次
		//后期可以根据每个小雷达的刷新情况, 实时更改总点云.....
		m_collect_cloud_condition.wait_for_millisecond(WANJI_716_SCAN_CYCLE_MS);
		if ( m_collect_cloud_condition.is_alive() )
		{
			std::chrono::system_clock::time_point t_command_time = std::chrono::system_clock::now();
			{
				std::unique_lock<std::mutex> t_lock(m_cloud_collection_mutex);
				mp_cloud_collection->clear();
			}

			//重新收集当前的点云, 不允许阻塞
			for (auto iter = m_wanji_lidar_device_map.begin(); iter != m_wanji_lidar_device_map.end(); ++iter)
			{
				t_error = iter->second->get_current_cloud(&m_cloud_collection_mutex,
												mp_cloud_collection,t_command_time);
				if ( t_error != Error_code::SUCCESS )
				{
					t_result.compare_and_cover_error(t_error);
				}
			}

			if ( t_result == SUCCESS && mp_cloud_collection->size()>0 )
			{
				{
					std::unique_lock<std::mutex> t_lock(m_cloud_collection_mutex);
					m_cloud_updata_time = std::chrono::system_clock::now();
				}
				wake_auto_detect();
			}


//			std::cout << " huli test :::: " << " mp_cloud_collection->size() = " << mp_cloud_collection->size() << std::endl;

			//huli
		}
	}
	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() end "<< this;
	return;
}

//唤醒自动预测的算法线程
Error_manager Wanji_manager::wake_auto_detect()
{
	for (auto iter = m_region_worker_map.begin(); iter != m_region_worker_map.end(); ++iter)
	{
	    iter->second->wake_auto_detect();
	}

	return Error_code::SUCCESS;
}

//执行外界任务的执行函数
void Wanji_manager::execute_thread_fun()
{
	LOG(INFO) << " Wanji_manager::execute_thread_fun() start "<< this;
	Error_manager t_error;
	Error_manager t_result;

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

			if ( mp_wanji_manager_task == NULL )
			{
				//忽略任务, 直接停止线程
				m_wanji_manager_status = E_READY;
				m_execute_condition.notify_all(false);
			}

			switch ( m_wanji_manager_status )
			{
			    case E_ISSUED_SCAN:
			    {
					//第一步
					//下发任务, 雷达管理模块给子雷达下发扫描任务.
					//分发扫描任务给下面的雷达
					//下发任务给所有的雷达
					for (auto iter = m_wanji_lidar_device_map.begin(); iter != m_wanji_lidar_device_map.end(); ++iter)
					{
						//下发扫描任务
						t_error = issued_scan_task(iter->first);
						if ( t_error != SUCCESS)
						{
							//下发子任务的故障处理汇总
							t_result.compare_and_cover_error(t_error);
						}
					}

					//故障处理, 只处理2级以上的故障, 一级故障继续执行后面的
					if(t_result.get_error_level() >= MINOR_ERROR)
					{
						m_wanji_manager_status = E_READY;
						mp_wanji_manager_task->set_task_statu(TASK_ERROR);
						//因为故障,而提前结束任务.
						mp_wanji_manager_task->compare_and_cover_task_error_manager(t_result);
						end_task();
					}
					else
					{
						//下发任务之后，修改状态为等待答复。
						m_wanji_manager_status = E_WAIT_SCAN;
					}
			        break;
			    }
			    case E_WAIT_SCAN:
			    {
					//第二步
					//等待答复, 注意不能写成死循环阻塞,
					//子任务没完成就直接通过, 然后重新进入  while (m_laser_manager_condition.is_alive()),
					// 这样线程仍然被 m_laser_manager_condition 控制
					//循环检查任务的完成状态
					std::map<int, Wanji_lidar_scan_task*>::iterator map_iter;
					for (  map_iter = m_wanji_lidar_scan_task_map.begin(); map_iter != m_wanji_lidar_scan_task_map.end(); ++map_iter)
					{
						if ( map_iter->second->is_task_end() == false)
						{
							if ( map_iter->second->is_over_time() )
							{
								//超时处理。取消任务。
								m_wanji_lidar_device_map[map_iter->first]->cancel_task(map_iter->second);
								map_iter->second->set_task_statu(TASK_DEAD);
								t_error.error_manager_reset(Error_code::WANJI_LIDAR_DEVICE_TASK_OVER_TIME, Error_level::MINOR_ERROR,
															" Wanji_lidar_scan_task is_over_time ");
								map_iter->second->set_task_error_manager(t_error);

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

					//如果下发的任务全部结束，那么汇总所有的结果, 并销毁下发的任务单。
					if(map_iter == m_wanji_lidar_scan_task_map.end() )
					{
						for (auto iter = m_wanji_lidar_scan_task_map.begin(); iter != m_wanji_lidar_scan_task_map.end(); )
						{
							//数据汇总，
							//由于 cloud; 是所有任务单共用的，所以不用处理了。自动就在一起。

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

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

						//故障处理, 只处理2级以上的故障, 一级故障继续执行后面的
						if(t_result.get_error_level() >= MINOR_ERROR)
						{
							m_wanji_manager_status = E_READY;
							mp_wanji_manager_task->set_task_statu(TASK_ERROR);
							//因为故障,而提前结束任务.
							mp_wanji_manager_task->compare_and_cover_task_error_manager(t_result);
							end_task();
						}
						else
						{
							//下发任务之后，修改状态为等待答复。
							m_wanji_manager_status = E_ISSUED_DETECT;
						}
					}
					//else 直接通过, 然后重新进入  while (m_laser_manager_condition.is_alive())
			        break;
			    }
				case E_ISSUED_DETECT:
				{
					m_wanji_manager_status = E_WAIT_DETECT;
					break;
				}
				case E_WAIT_DETECT:
				{
					{
						std::unique_lock<std::mutex> t_lock(m_task_cloud_mutex);
						std::cout << " huli test :::: " << " mp_task_cloud->size() = " << mp_task_cloud->size() << std::endl;
					}

					mp_wanji_manager_task->compare_and_cover_task_error_manager(t_result);
					//正常结束任务
					end_task();
					break;
				}
			    default:
			    {

			        break;
			    }
			}



			//第3步, 下发预测算法任务
			//huli
		}
	}
	LOG(INFO) << " Wanji_manager::execute_thread_fun() end "<< this;
	return;
}

//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
Error_manager Wanji_manager::issued_scan_task(int lidar_index)
{
	//判断是否存在
	std::map<int, Wanji_lidar_device*>::iterator map_iter1 = m_wanji_lidar_device_map.find(lidar_index);
	if ( map_iter1 == m_wanji_lidar_device_map.end() )
	{
		return Error_manager(Error_code::WJ_MANAGER_LASER_INDEX_ERRPR, Error_level::NEGLIGIBLE_ERROR,
							 " issued_scan_task lidar_index  error ");
	}

	//查重
	std::map<int, Wanji_lidar_scan_task*>::iterator map_iter2 = m_wanji_lidar_scan_task_map.find(lidar_index);
	if ( map_iter2 != m_wanji_lidar_scan_task_map.end() )
	{
		//如果重复，则不做任何处理,视为无效行为.
		//返回轻微错误.仅仅提示作用.
		return Error_manager(Error_code::WJ_MANAGER_LASER_INDEX_REPEAT, Error_level::NEGLIGIBLE_ERROR,
							 " issued_scan_task  lidar_index repeart ");
	}

	//创建雷达扫描任务,
	// 这里为下发雷达任务分配内存，后续要记得回收。
	Wanji_lidar_scan_task* t_wanji_lidar_scan_task=new Wanji_lidar_scan_task();
	t_wanji_lidar_scan_task->task_init(TASK_CREATED, "",
									   m_wanji_lidar_device_map[lidar_index],
									   std::chrono::milliseconds(WANJI_716_SCAN_CYCLE_MS),
									   mp_wanji_manager_task->get_command_time(),
									   &m_task_cloud_mutex,
									   mp_task_cloud	);


	//保存雷达扫描任务 到 m_laser_task_map
	m_wanji_lidar_scan_task_map[lidar_index] = t_wanji_lidar_scan_task;

	//发送任务单给雷达
	return m_wanji_lidar_device_map[lidar_index]->execute_task(t_wanji_lidar_scan_task);

}


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