#ifndef RS_DEVICE_BRIDGE_HH
#define RS_DEVICE_BRIDGE_HH
/*
 * @Author: yct 18202736439@163.com
 * @Date: 2023-01-12 14:31:29
 * @LastEditors: yct 18202736439@163.com
 * @LastEditTime: 2023-01-12 15:14:54
 * @FilePath: /puai_wj_2021/velodyne_lidar/velodyne_driver/rs_device_bridge.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */

#include "../rslidar/rslidar_driver.h"
#include "velodyne_driver/velodyne_lidar_device.h"

class RS_device_bridge : public Velodyne_lidar_device
{
public:

    // 无参构造函数
    RS_device_bridge(){}
    // 析构函数
    ~RS_device_bridge(){}

    // 初始化
    Error_manager init(velodyne::velodyneLidarParams params)
    {
        return m_rs_lidar.init(params);
    }

    // 反初始化
    Error_manager uninit()
    {
        return m_rs_lidar.uninit();
    }

    // 检查雷达状态,是否正常运行
    Error_manager check_status(){return m_rs_lidar.check_status(); }

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

    int get_lidar_id() { return m_rs_lidar.get_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)
    {
        std::vector<PointT> t_cloud_vec;
        Error_manager ec = m_rs_lidar.get_new_ring_cloud_and_wait(p_mutex, t_cloud_vec, command_time, timeout_ms);
        cloud_vec.clear();
        for (size_t i = 0; i < t_cloud_vec.size(); i++)
        {
            cloud_vec.push_back(Lidar_point(t_cloud_vec[i].x, t_cloud_vec[i].y, t_cloud_vec[i].z, t_cloud_vec[i].ring, 0, 0.0f, t_cloud_vec[i].intensity, t_cloud_vec[i].timestamp));
        }
        return ec;
    }

	//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置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)
    {
        return m_rs_lidar.get_new_cloud_and_wait(p_mutex, p_cloud, command_time, timeout_ms);
    }

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

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

public:
	Velodyne_lidar_device_status get_status(){return Velodyne_lidar_device_status(m_rs_lidar.get_status());}

    void updata_status(){m_rs_lidar.update_status();}

protected:
    void execute_thread_fun(){}

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

	// 解析点云线程函数
	void parse_thread_fun(){}
protected:
    RS_lidar_device m_rs_lidar;
};

#endif