#include "rslidar_driver.h"

// 万集雷达封装类构造函数
RS_lidar_device::RS_lidar_device()
{
	m_rs_lidar_device_status = E_UNKNOWN;
    mb_exit_process = false;
}

// 万集雷达封装类析构函数
RS_lidar_device::~RS_lidar_device()
{
	uninit();
}

// 初始化
Error_manager RS_lidar_device::init(velodyne::velodyneLidarParams params)
{
	m_params = params;
	Error_manager t_error;
	LOG(INFO) << " RS_lidar_device::init " << this;
	m_rs_lidar_device_status = E_UNKNOWN;
	m_rs_protocol_status = E_UNKNOWN;
	m_lidar_id = params.lidar_id();

    // 设置雷达内参矩阵
	Eigen::AngleAxisd rot_x = Eigen::AngleAxisd(params.calib().r()*M_PI/180.0, Eigen::Vector3d::UnitX());
	Eigen::AngleAxisd rot_y = Eigen::AngleAxisd(params.calib().p()*M_PI/180.0, Eigen::Vector3d::UnitY());
	Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(params.calib().y()*M_PI/180.0, Eigen::Vector3d::UnitZ());
	Eigen::Vector3d trans(params.calib().cx(), params.calib().cy(), params.calib().cz());
	m_calib_matrix << rot_x.toRotationMatrix() * rot_y.toRotationMatrix() * rot_z.toRotationMatrix(), trans, 0.0, 0.0, 0.0, 1.0;
	// LOG(INFO) <<"lidar "<<m_lidar_id<<",,,\n"<< rot_x.toRotationMatrix()<<"------------------\n"<<rot_y.toRotationMatrix()<<"-----------------\n"<<rot_z.toRotationMatrix()<<"---------------\n"<<m_calib_matrix;

    m_lidar_params.input_type = InputType::ONLINE_LIDAR;
    m_lidar_params.input_param.msop_port = params.port();         ///< Set the lidar msop port number, the default is 6699
    m_lidar_params.input_param.difop_port = params.difop();        ///< Set the lidar difop port number, the default is 7788
    // m_lidar_params.input_param.difop_port = 7788;        ///< Set the lidar difop port number, the default is 7788
    m_lidar_params.decoder_param.min_distance = params.min_range();
    m_lidar_params.decoder_param.max_distance = params.max_range();
    m_lidar_params.decoder_param.start_angle = params.min_angle();
    m_lidar_params.decoder_param.end_angle = params.max_angle();
    m_lidar_params.decoder_param.dense_points = true;
    // if(params.model() == "HELIOS16")
    m_lidar_params.lidar_type = LidarType::RSHELIOS_16P; ///< Set the lidar type. Make sure this type is correct
    // 驱动初始化
    m_lidar_driver.regPointCloudCallback(std::bind(&RS_lidar_device::driverGetPointCloudFromCallerCallback, this), std::bind(&RS_lidar_device::driverReturnPointCloudToCallerCallback, this, std::placeholders::_1)); ///< Register the point cloud callback functions
    m_lidar_driver.regExceptionCallback(std::bind(&RS_lidar_device::exceptionCallback, this, std::placeholders::_1));                                                              ///< Register the exception callback function
    if (!m_lidar_driver.init(m_lidar_params))                                                                                     ///< Call the init function
    {
        LOG(WARNING) << "Driver Initialize Error...";
        t_error = Error_manager(Error_code::VELODYNE_LIDAR_UNINITIALIZED, Error_level::MAJOR_ERROR,
							 (std::string("RS_lidar_device::init driver error ")+std::to_string(params.port())).c_str());
        return t_error;
    }
    mp_cloud_handle_thread = new std::thread(std::bind(&RS_lidar_device::processCloud, this));

    m_lidar_driver.start();

    m_rs_protocol_status = E_READY;

	//初始化雷达通信模块
	std::string t_ip = params.ip();
	int t_port = params.port();

	m_rs_lidar_device_status = E_READY;
    LOG(INFO) << "rslidar device "<< m_lidar_params.input_param.msop_port<<" initialized.\n";
    m_lidar_params.print();

	return t_error;
}

//反初始化
Error_manager RS_lidar_device::uninit()
{
    mb_exit_process = true;
    m_lidar_driver.stop();

    if(mp_cloud_handle_thread != nullptr)
    {
        if(mp_cloud_handle_thread->joinable())
            mp_cloud_handle_thread->join();
        delete(mp_cloud_handle_thread);
        mp_cloud_handle_thread = nullptr;
    }

	if (m_rs_lidar_device_status != E_FAULT)
	{
		m_rs_lidar_device_status = E_UNKNOWN;
	}
	return Error_code::SUCCESS;
}

//检查雷达状态,是否正常运行
Error_manager RS_lidar_device::check_status()
{
	update_status();
	if (m_rs_lidar_device_status == E_READY)
	{
		return Error_code::SUCCESS;
	}
	else if (m_rs_lidar_device_status == E_BUSY)
	{
		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
							 "  RS_lidar_device::check_status() error ");
	}
	else
	{
		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " RS_lidar_device::check_status() error ");
	}
	return Error_code::SUCCESS;
}

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

Error_manager RS_lidar_device::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)
{
	if(p_mutex == nullptr)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	//判断是否超时
	while (std::chrono::steady_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
	{
		//获取指令时间之后的点云, 如果没有就会循环
		if (m_capture_time > command_time)
		{
			std::unique_lock<std::mutex> lck(*p_mutex);
			{
				cloud_vec.clear();
				std::lock_guard<std::mutex> lck(m_cloud_mutex);
				if (m_ring_cloud.size() <= 0)
					return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
				for (size_t i = 0; i < m_ring_cloud.size(); i++)
				{
					cloud_vec.assign(m_ring_cloud.begin(), m_ring_cloud.end());
				}
			}
			return SUCCESS;
		}
		//等1ms
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}
	//超时退出就报错
	return Error_manager(Error_code::VELODYNE_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
						 " RS_lidar_device::get_new_ring_cloud_and_wait error ");
}

//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
Error_manager RS_lidar_device::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)
{
	if (p_mutex == NULL || p_cloud.get() == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	//判断是否超时
	while (std::chrono::steady_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
	{
		//获取指令时间之后的点云, 如果没有就会循环
		if (m_capture_time > command_time)
		{
			std::unique_lock<std::mutex> lck(*p_mutex);
			{
				std::lock_guard<std::mutex> lck(m_cloud_mutex);
				if (m_ring_cloud.size() <= 0)
					return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
				for (size_t i = 0; i < m_ring_cloud.size(); i++)
				{
					p_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
				}
			}
			return SUCCESS;
		}
		//else 等待下一次数据更新

		//等1ms
		std::this_thread::sleep_for(std::chrono::milliseconds(1));
	}
	//超时退出就报错
	return Error_manager(Error_code::VELODYNE_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
						 " RS_lidar_device::get_new_cloud_and_wait error ");
}

//外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
Error_manager RS_lidar_device::get_current_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
													   std::chrono::steady_clock::time_point command_time)
{
	if (p_mutex == NULL || p_cloud.get() == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	//获取指令时间之后的点云, 如果没有就会报错, 不会等待
	if (m_capture_time > command_time)
	{
		std::unique_lock<std::mutex> lck(*p_mutex);
		{
			std::lock_guard<std::mutex> lck(m_cloud_mutex);
			if (m_ring_cloud.size() <= 0)
				return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
			for (size_t i = 0; i < m_ring_cloud.size(); i++)
			{
				p_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
			}
		}
		return SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " RS_lidar_device::get_current_cloud error ");
	}
}

//外部调用获取最新的点云, 如果没有就会报错, 不会等待
Error_manager RS_lidar_device::get_last_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
													std::chrono::steady_clock::time_point command_time)
{
	if (p_mutex == NULL || p_cloud.get() == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	if (m_capture_time > command_time - std::chrono::milliseconds(int(60000.0/(m_params.rpm()*0.5))))
	{
		std::unique_lock<std::mutex> lck(*p_mutex);
		{
			std::lock_guard<std::mutex> lck(m_cloud_mutex);
			if (m_ring_cloud.size() <= 0)
				return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
			for (size_t i = 0; i < m_ring_cloud.size(); i++)
			{
				p_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
			}
		}
		return SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " RS_lidar_device::get_current_cloud error ");
	}
}

//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
//注:函数内部不加锁, 由外部统一加锁.
Error_manager RS_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
													std::chrono::steady_clock::time_point command_time)
{
	if (p_cloud.get() == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	if (m_capture_time > command_time - std::chrono::milliseconds(int(60000.0/(m_params.rpm()*0.5))))
	{
		std::lock_guard<std::mutex> lck(m_cloud_mutex);
		if (m_ring_cloud.size() <= 0)
			return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
		for (size_t i = 0; i < m_ring_cloud.size(); i++)
		{
			p_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
		}
		return SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
							 " RS_lidar_device::get_current_cloud error ");
	}
}

RS_lidar_device::RS_lidar_device_status RS_lidar_device::get_status()
{
	update_status();
	return m_rs_lidar_device_status;
}

void RS_lidar_device::update_status()
{
	if (m_rs_protocol_status == E_FAULT)
	{
		m_rs_lidar_device_status = E_FAULT;
	}
	else if(m_rs_protocol_status == E_BUSY)
    {
        m_rs_lidar_device_status = E_BUSY;
    }
	else if (m_rs_protocol_status == E_READY)
	{
		m_rs_lidar_device_status = E_READY;
	}
	else
	{
//	    LOG(WARNING)<<"unknown????? "<<m_rs_socket_status<<", "<<m_rs_protocol_status;
		m_rs_lidar_device_status = E_UNKNOWN;
	}
}