//
// 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_UNKNOWN;

	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;
}
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_UNKNOWN)
	{
		return Error_manager(Error_code::WJ_MANAGER_INIT_ERROR, 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));
//		LOG(WARNING) << wanji_parameters.wj_lidar(i).DebugString();
		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_max = wanji_parameters.regions(i).maxy();
		// changed by yct, 传入终端index,仅普爱使用
		t_error = p_region_worker->init(t_point2d_box, &m_cloud_collection_mutex, mp_cloud_collection, i);
		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_UNKNOWN;

	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_BUSY;
		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);

	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)
{
	//关闭子线程
	m_execute_condition.notify_all(false);
	//确保内部线程已经停下
	while (m_execute_condition.is_working())
	{

	}

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

	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 状态不变

	return Error_code::SUCCESS;
}

//检查雷达状态,是否正常运行
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();

				//重新收集最近的点云, 不允许阻塞
				// added by yct, 测试雷达功能
//				int get_cloud_count=0;
				for (auto iter = m_wanji_lidar_device_map.begin(); iter != m_wanji_lidar_device_map.end(); ++iter)
				{
					t_error = iter->second->get_last_cloud(mp_cloud_collection,t_command_time);
					if ( t_error != Error_code::SUCCESS )
					{
						t_result.compare_and_cover_error(t_error);
//						LOG(WARNING) << "cloud timeout: "<<get_cloud_count;
					}
//                    get_cloud_count++;
				}

				if ( t_result == SUCCESS && mp_cloud_collection->size()>0 )
				{
					m_cloud_updata_time = std::chrono::system_clock::now();
					//成功则唤醒预测算法
					start_auto_detect();
				}
				else
				{
					mp_cloud_collection->clear();
					//失败则关闭预测算法
					stop_auto_detect();
//					LOG(WARNING) << t_result.to_string();
				}
			}
		}
        t_result.error_manager_clear_all();
	}
	LOG(INFO) << " Wanji_manager::collect_cloud_thread_fun() end "<< this;
	return;
}

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

	return Error_code::SUCCESS;
}
//关闭自动预测的算法线程
Error_manager Wanji_manager::stop_auto_detect()
{
	for (auto iter = m_region_worker_map.begin(); iter != m_region_worker_map.end(); ++iter)
	{
		iter->second->stop_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;
	Common_data::Car_wheel_information t_car_wheel_information;

	//雷达管理的独立线程，负责控制管理所有的雷达。
	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();

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

			//万集内部全自动运行, 只需要根据时间去获取想要的就行了.
			//目前楚天是唯一的预测算法, 如果后续有多个出入口,则使用任务单的终端id来获取指定的.
			if ( m_region_worker_map.size() > 0 && m_region_worker_map.find(mp_wanji_manager_task->get_terminal_id()) != m_region_worker_map.end())
			{
				t_error = m_region_worker_map[mp_wanji_manager_task->get_terminal_id()]->get_current_wheel_information(&t_car_wheel_information, mp_wanji_manager_task->get_command_time());
				if ( t_error == Error_code::SUCCESS )
				{
					//添加车轮的预测结果
					mp_wanji_manager_task->set_car_wheel_information(t_car_wheel_information);
					//正常完成任务
					end_task();
				}
				else
				{
					//如果在指令时间1秒后都没有成功获取结果, 返回错误原因
					if ( std::chrono::system_clock::now() - m_region_worker_map[mp_wanji_manager_task->get_terminal_id()]->get_detect_updata_time() < std::chrono::milliseconds(WANJI_MANAGER_EXECUTE_TIMEOUT_MS)  )
					{
						//没有超时, 那么就等1ms, 然后重新循环
						std::this_thread::sleep_for(std::chrono::milliseconds(1));
						std::this_thread::yield();
					}
					else
					{
						//因为故障,而提前结束任务.
						mp_wanji_manager_task->compare_and_cover_task_error_manager(t_error);
						end_task();
					}

				}
			}

		}
	}
	LOG(INFO) << " Wanji_manager::execute_thread_fun() end "<< this;
	return;
}