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

#ifndef NNXX_TESTS_WANJI_MANAGER_H
#define NNXX_TESTS_WANJI_MANAGER_H

#include "../tool/thread_condition.h"
#include "../tool/singleton.h"
#include "../tool/point_tool.h"
#include "../error_code/error_code.h"
#include <thread>
#include <map>

#include "../wanji_lidar/wanji_716N_device.h"
#include "../wanji_lidar/wanji_lidar_device.h"
#include "../wanji_lidar/region_worker.h"
#include "../wanji_lidar/wanji_manager_task.h"
#include "../wanji_lidar/wj_lidar_conf.pb.h"

// 1 716, 2 716N
#define LIDAR_TYPE 2

struct Timestamped_cloud_wj
{
	public:
		Timestamped_cloud_wj(wj::CalibParameter calib)
		{
			m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
			m_time_collected = std::chrono::system_clock::now();
			m_extrinsic.CopyFrom(calib);
			updateMatrix();
		}
		Timestamped_cloud_wj(wj::CalibParameter calib, pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
		{
			m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
			m_cloud->operator+=(*input_cloud);
			m_time_collected = std::chrono::system_clock::now();
			m_extrinsic.CopyFrom(calib);
			updateMatrix();
		}
		~Timestamped_cloud_wj(){}

		// 设置点云，覆盖
		void setCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
		{
			m_cloud->clear();
			m_cloud->operator+=(*input_cloud);
			m_time_collected = std::chrono::system_clock::now();
		}
		// 添加点云至现有点云中
		void addCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
		{
			m_cloud->operator+=(*input_cloud);
			m_time_collected = std::chrono::system_clock::now();
		}
		void clearCloud()
		{
			m_cloud->clear();
			m_time_collected = std::chrono::system_clock::now();
		}
		// 更新外参矩阵
		void updateMatrix()
		{
			Eigen::AngleAxisd rot_x = Eigen::AngleAxisd(m_extrinsic.r()*M_PI/180.0, Eigen::Vector3d::UnitX());
			Eigen::AngleAxisd rot_y = Eigen::AngleAxisd(m_extrinsic.p()*M_PI/180.0, Eigen::Vector3d::UnitY());
			Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(m_extrinsic.y()*M_PI/180.0, Eigen::Vector3d::UnitZ());
			Eigen::Vector3d trans(m_extrinsic.cx(), m_extrinsic.cy(), m_extrinsic.cz());
			m_extrin_matrix << rot_x.toRotationMatrix() * rot_y.toRotationMatrix() * rot_z.toRotationMatrix(), trans, 0.0, 0.0, 0.0, 1.0;
		}
		// 使用外参返回变换后点云
		pcl::PointCloud<pcl::PointXYZ>::Ptr transCloud()
		{
			pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZ>);
			if(m_cloud->size() > 0)
			{
				for (size_t i = 0; i < m_cloud->size(); i++)
				{
					Eigen::Vector4d t_point(m_cloud->points[i].x, m_cloud->points[i].y, m_cloud->points[i].z, 1.0);
					t_point.x() = m_extrin_matrix(0,0) * t_point.x() +  m_extrin_matrix(0,1) * t_point.y() +  m_extrin_matrix(0,2) * t_point.z() + m_extrin_matrix(0,3) * t_point.w(); 
					t_point.y() = m_extrin_matrix(1,0) * t_point.x() +  m_extrin_matrix(1,1) * t_point.y() +  m_extrin_matrix(1,2) * t_point.z() + m_extrin_matrix(1,3) * t_point.w(); 
					t_point.z() = m_extrin_matrix(2,0) * t_point.x() +  m_extrin_matrix(2,1) * t_point.y() +  m_extrin_matrix(2,2) * t_point.z() + m_extrin_matrix(2,3) * t_point.w();  
					t_point.w() = m_extrin_matrix(3,0) * t_point.x() +  m_extrin_matrix(3,1) * t_point.y() +  m_extrin_matrix(3,2) * t_point.z() + m_extrin_matrix(3,3) * t_point.w(); 
					pcl::PointXYZ t_pcl_point;
					t_pcl_point.x = t_point.x() / t_point.w();
					t_pcl_point.y = t_point.y() / t_point.w();
					t_pcl_point.z = t_point.z() / t_point.w();
					t_cloud->push_back(t_pcl_point);
				}
			}
			// t_cloud->operator+=(*m_cloud);

			return t_cloud;			
		}

		// 时间戳
		std::chrono::system_clock::time_point m_time_collected;
		// 点云
		pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;
		// 外参数据
		wj::CalibParameter m_extrinsic;
		// 外参齐次矩阵
		Eigen::Matrix4d m_extrin_matrix;
};

class Wanji_manager:public Singleton<Wanji_manager>
{
public:
	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
	friend class Singleton<Wanji_manager>;

	//万集管理任务超时时间1000ms,
#define WANJI_MANAGER_EXECUTE_TIMEOUT_MS	1000
//万集配置参数的默认路径
#define WANJI_MANAGER_PARAMETER_PATH		"../setting/wanji_manager.prototxt"
public:
	//雷达管理的状态
	enum Wanji_manager_status
	{
		E_UNKNOWN              	= 0,    //未知
		E_READY               	= 1,    //准备，待机
		E_BUSY					= 2, 	//工作正忙

		E_ISSUED_SCAN			= 3, 	//下发任务, 获取点云
		E_WAIT_SCAN				= 4,	//等待任务, 扫描点云

		E_ISSUED_DETECT			= 5, 	//下发任务, 算法预测
		E_WAIT_DETECT			= 6, 	//等待任务, 算法预测

		E_FAULT					= 10,	//故障
	};
private:
	Wanji_manager();
public:

	Wanji_manager(const Wanji_manager& other)= delete;
	Wanji_manager& operator =(const Wanji_manager& other)= delete;
	~Wanji_manager();
public://API functions
	//初始化 雷达 管理模块。如下三选一
	Error_manager wanji_manager_init(int terminal_id);
	//初始化 雷达 管理模块。从文件读取
	Error_manager wanji_manager_init_from_protobuf(std::string prototxt_path, int terminal_id);
	//初始化 雷达 管理模块。从protobuf读取
	Error_manager wanji_manager_init_from_protobuf(wj::wjManagerParams& wanji_parameters, int terminal_id);
	//反初始化 雷达 管理模块。
	Error_manager wanji_manager_uninit();


	//对外的接口函数，负责接受并处理任务单，
	Error_manager execute_task(Task_Base* p_wanji_manager_task);
	//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
	Error_manager end_task();
	//取消任务单，由发送方提前取消任务单
	Error_manager cancel_task(Task_Base* p_wanji_manager_task);

	//检查雷达状态,是否正常运行
	Error_manager check_status();
	//判断是否为待机，如果已经准备好，则可以执行任务。
	bool is_ready();

	// 根据区域与激光id生成唯一string作为key
	std::string generate_region_lidar_key(int region_id, int lidar_id) { return std::to_string(region_id) + std::string("-") + std::to_string(lidar_id); }

public://get or set member variable
	Wanji_manager_status get_status();

#if LIDAR_TYPE == 1
	std::map<int, Wanji_lidar_device*> & get_wanji_lidar_device_map();
#else
	std::map<int, Wanji_716N_lidar_device*> & get_wanji_lidar_device_map();
#endif

	std::map<int, Region_worker *> & get_region_worker_map();
protected://member functions
	//自动收集点云的线程函数
	void collect_cloud_thread_fun();
	//更新点云，自行计算
	Error_manager update_region_cloud();

	//开始自动预测的算法线程
	Error_manager start_auto_detect();
	//关闭自动预测的算法线程
	Error_manager stop_auto_detect();



	//执行外界任务的执行函数
	void execute_thread_fun();


protected://member variable

	int m_terminal_id;
	//状态
	Wanji_manager_status						m_wanji_manager_status;			//万集管理模块的状态

#if LIDAR_TYPE == 1
	std::map<int, Wanji_lidar_device *> 		m_wanji_lidar_device_map; 		// 万集雷达实例指针数组, 内存由本类管理
#else
	std::map<int, Wanji_716N_lidar_device *> 		m_wanji_lidar_device_map;
#endif
	
	std::map<int, Region_worker *> 				m_region_worker_map;  			// 区域功能实例指针数组, 内存由本类管理


	//万集雷达的自动功能, 定时收集所有点云, 然后通知detect去计算车轮信息.
	std::mutex	 								m_cloud_collection_mutex;       // 点云更新互斥锁
	std::map<std::string, std::shared_ptr<Timestamped_cloud_wj>> 		m_region_laser_to_cloud_collection_map;			//扫描点的点云集合, 内存由本类管理

	// std::mutex	 								m_cloud_collection_mutex;       // 点云更新互斥锁
	// pcl::PointCloud<pcl::PointXYZ>::Ptr 		mp_cloud_collection;			//扫描点的点云集合, 内存由本类管理
	std::chrono::system_clock::time_point		m_cloud_update_time;			//扫描点的更新时间.
	std::thread*        						mp_collect_cloud_thread;   		//收集点云的线程指针，内存由本类管理
	Thread_condition							m_collect_cloud_condition;		//收集点云的条件变量

	//任务执行线程
	std::thread*        						mp_execute_thread;   			//执行的线程指针，内存由本类管理
	Thread_condition							m_execute_condition;			//执行的条件变量

	Wanji_manager_task*							mp_wanji_manager_task;			//万集管理模块的任务单的指针，内存由发送方管理。


private:

};


#endif //NNXX_TESTS_WANJI_MANAGER_H