#ifndef VELODYNE_LIDAR_ENCAPSULATION_HH
#define VELODYNE_LIDAR_ENCAPSULATION_HH

#include "input.h"
#include "rawdata.h"
#include "velodyne_lidar_scan_task.h"
#include "../velodyne_config.pb.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <thread>

#include "../../error_code/error_code.h"
#include "../../tool/binary_buf.h"
#include "../../tool/thread_safe_queue.h"
#include "../../tool/thread_condition.h"

class Velodyne_lidar_device
{
public:
	//万集设备身状态
	enum Velodyne_lidar_device_status
	{
		E_UNKNOWN              	= 0,    //未知
		E_READY	             	= 1,    //正常待机
		E_DISCONNECT			= 2,	//断连

		E_BUSY					= 3, 	//工作正忙

		E_FAULT					=10,	//故障
	};

public:
    // 无参构造函数
    Velodyne_lidar_device();
    // 析构函数
    ~Velodyne_lidar_device();

	// 初始化
	Error_manager init(velodyne::velodyneLidarParams params);
	//反初始化
	Error_manager uninit();

	//对外的接口函数，负责接受并处理任务单，
	Error_manager execute_task(Task_Base* p_wanji_lidar_scan_task);

	//检查雷达状态,是否正常运行
	Error_manager check_status();
	//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
	Error_manager end_task();
	//取消任务单，由发送方提前取消任务单
	Error_manager cancel_task(Task_Base* p_wanji_lidar_scan_task);

	//判断是否为待机，如果已经准备好，则可以执行任务。
	bool is_ready();

	int get_lidar_id() { return m_lidar_id; }

	// 外部调用获取最新带环信息点云，通常用于标定
	Error_manager get_new_ring_cloud_and_wait(std::mutex* p_mutex, std::vector<Lidar_point> &cloud_vec,
							std::chrono::steady_clock::time_point command_time, int timeout_ms=1500);

	//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
	Error_manager get_new_cloud_and_wait(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
							std::chrono::steady_clock::time_point command_time, int timeout_ms=1500);

	//外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
	// 注意！！！当前最新点云未成环，或许缺失数据
	Error_manager get_current_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
									std::chrono::steady_clock::time_point command_time);
	//外部调用获取最新的点云, 如果没有就会报错, 不会等待
	Error_manager get_last_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
							std::chrono::steady_clock::time_point command_time);

	//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
	//注:函数内部不加锁, 由外部统一加锁.
	Error_manager get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
								 std::chrono::steady_clock::time_point command_time);
public:
	Velodyne_lidar_device_status get_status();
protected:

	// 根据model名与转速，获得吞吐量信息，设置每圈packet个数
	void dev_info_config(std::string model, int rpm);

	void updata_status();
	//mp_laser_manager_thread 线程执行函数，
	//thread_work 内部线程负责获取点云结果
	void execute_thread_fun();

	// 获取点云线程函数
	void capture_thread_fun();

	// 解析点云线程函数
	void parse_thread_fun();

protected:
	//状态
	Velodyne_lidar_device_status m_velodyne_lidar_device_status;	//万集设备状态
	Velodyne_lidar_device_status m_velodyne_socket_status;
	Velodyne_lidar_device_status m_velodyne_protocol_status;
	// 雷达编号
	int m_lidar_id;
	// 雷达配置
	velodyne::velodyneLidarParams m_params;

	//子模块
	velodyne_driver::InputSocket m_input_socket;					   //负责雷达通信, 接受雷达数据
	velodyne_rawdata::RawData						m_protocol;        // velodyne雷达协议

	//中间缓存
	Thread_safe_queue<Binary_buf*>				m_communication_data_queue;	//通信数据队列

	// 数据缓存
	unsigned char m_buf[5000];
	// 点云互斥锁
	std::mutex m_cloud_mutex;
	// 每圈packet个数
	int m_num_packet_per_scan;
	// packet获取计数
	int m_num_packet_inside;
	// 点云缓存
	std::vector<Lidar_point> m_cloud_buf;
	// 完整可获取点云
	std::vector<Lidar_point> m_ring_cloud;

	// 最新数据获取时间
	std::chrono::steady_clock::time_point m_capture_time;
	//点云获取线程
	std::thread *mp_capture_thread;		  // 获取点云数据线程指针，内存由本类管理
	Thread_condition m_capture_condition; // 获取点云数据条件变量

	// 最新解析环点云时间
	std::chrono::steady_clock::time_point m_parse_ring_time;
	// 解析点云时间
	std::chrono::steady_clock::time_point m_parse_time;
	//点云解析线程
	std::thread *mp_parse_thread; // 解析点云线程指针，内存由本类管理
	Thread_condition m_parse_condition; // 解析点云条件变量
	// 点云内参齐次矩阵
	Eigen::Matrix4d m_calib_matrix;

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

	Velodyne_lidar_scan_task *  					mp_velodyne_lidar_scan_task;   //雷达设备任务单的指针，内存由发送方管理。

};

#endif // !VELODYNE_LIDAR_ENCAPSULATION_HH