#ifndef RS_LIDAR_ENCAPSULATION_HH
#define RS_LIDAR_ENCAPSULATION_HH

#include <iostream>
#include <thread>
#include <mutex>
#include <chrono>
#include <functional>

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

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <glog/logging.h>

#include <rs_driver/api/lidar_driver.hpp>
#ifdef ENABLE_PCL_POINTCLOUD
#include <rs_driver/msg/pcl_point_cloud_msg.hpp>
#else
#include <rs_driver/msg/point_cloud_msg.hpp>
#endif
#include "../velodyne_lidar/velodyne_config.pb.h"

using namespace robosense::lidar;

typedef PointXYZIRT PointT;
typedef PointCloudT<PointT> PointCloudMsg;


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

		E_BUSY					= 3, 	//工作正忙

		E_FAULT					=10,	//故障
	};

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

	// ************************* rslidar driver callbacks *******************************
	// @brief point cloud callback function. The caller should register it to the lidar driver.
	//        Via this fucntion, the driver gets an free/unused point cloud message from the caller.
	// @param msg  The free/unused point cloud message.
	//
	std::shared_ptr<PointCloudMsg> driverGetPointCloudFromCallerCallback(void)
	{
		// std::cout<<"get cloud from caller callback"<<std::endl;
		// Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver,
		//       so please DO NOT do time-consuming task here.
		std::shared_ptr<PointCloudMsg> msg = free_cloud_queue.pop();
		if (msg.get() != NULL)
		{
			return msg;
		}
		return std::make_shared<PointCloudMsg>();
	}

	//
	// @brief point cloud callback function. The caller should register it to the lidar driver.
	//        Via this function, the driver gets/returns a stuffed point cloud message to the caller.
	// @param msg  The stuffed point cloud message.
	//
	void driverReturnPointCloudToCallerCallback(std::shared_ptr<PointCloudMsg> msg)
	{
		// std::cout<<"return point callback"<<std::endl;
		// Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver,
		//       so please DO NOT do time-consuming task here. Instead, process it in caller's own thread. (see processCloud() below)
		// stuffed_cloud_queue.push(msg);
		stuffed_cloud_queue.push(msg);
	}

	//
	// @brief exception callback function. The caller should register it to the lidar driver.
	//        Via this function, the driver inform the caller that something happens.
	// @param code The error code to represent the error/warning/information
	//
	void exceptionCallback(const Error &code)
	{
		// std::cout<<"exception callback"<<std::endl;
		// Note: This callback function runs in the packet-receving and packet-parsing/point-cloud_constructing thread of the driver,
		//       so please DO NOT do time-consuming task here.
		if(ERRCODE_MSOPTIMEOUT == code.error_code)
		{
			m_rs_lidar_device_status = E_DISCONNECT;
		}else if(ERRCODE_SUCCESS == code.error_code)
		{
			m_rs_lidar_device_status = E_READY;
			m_rs_protocol_status = E_READY;
			// RS_WARNING << code.toString() << RS_REND;
		}else if(code.error_code_type == ErrCodeType::WARNING_CODE || code.error_code_type == ErrCodeType::ERROR_CODE)
		{
			m_rs_lidar_device_status = E_FAULT;
			m_rs_protocol_status = E_FAULT;
			// RS_WARNING << code.toString() << RS_REND;
		}
		RS_WARNING << code.toString() << RS_REND;
	}

	void processCloud(void)
	{
		while (!mb_exit_process)
		{
			usleep(1);
			std::shared_ptr<PointCloudMsg> msg = stuffed_cloud_queue.popWait();
			if (msg.get() == NULL)
			{
				continue;
			}

			m_capture_time = std::chrono::steady_clock::now();
			// RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND;
			std::lock_guard<std::mutex> lck(m_cloud_mutex);
			m_ring_cloud.clear();
			for (size_t i = 0; i < msg->points.size(); i++)
			{
				Eigen::Vector4d point{msg->points[i].x, msg->points[i].y, msg->points[i].z, 1};
				point = m_calib_matrix * point;
				point.x() = point.x() / point.w();
				point.y() = point.y() / point.w();
				point.z() = point.z() / point.w();
				msg->points[i].x = point.x();
				msg->points[i].y = point.y();
				msg->points[i].z = point.z();
				m_ring_cloud.push_back(msg->points[i]);
			}
			free_cloud_queue.push(msg);
		}
	}

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

	//检查雷达状态,是否正常运行
	Error_manager check_status();

	//判断是否为待机，如果已经准备好，则可以执行任务。
	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<PointT> &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);

	RS_lidar_device_status get_status();

	void update_status();

protected:
	//状态
	RS_lidar_device_status m_rs_lidar_device_status;	//设备状态
	RS_lidar_device_status m_rs_protocol_status;
	// 雷达编号
	int m_lidar_id;
	// 雷达配置
	velodyne::velodyneLidarParams m_params;
	RSDriverParam m_lidar_params;
	LidarDriver<PointCloudMsg> m_lidar_driver;               ///< Declare the driver object

	// 空点云缓存
	SyncQueue<std::shared_ptr<PointCloudMsg>> free_cloud_queue;
	SyncQueue<std::shared_ptr<PointCloudMsg>> stuffed_cloud_queue;

	bool mb_exit_process;
	std::thread* mp_cloud_handle_thread;
	std::mutex m_cloud_mutex;
	// 完整可获取点云
	std::vector<PointT> m_ring_cloud;
	// 获取点云时间
	std::chrono::steady_clock::time_point m_capture_time;

	// 点云内参齐次矩阵
	Eigen::Matrix4d m_calib_matrix;

};


#endif // !RS_LIDAR_ENCAPSULATION_HH