#include "wj_716_lidar_protocol.h"
#include <iostream>
//#include "../tool/proto_tool.h"



Wj_716_lidar_protocol::Wj_716_lidar_protocol()
{
	m_wanji_lidar_protocol_status = E_UNKNOWN;

	mp_communication_data_queue = NULL;
	memset(m_scan_point_distance, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);
	memset(m_scan_point_intensity, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);
	memset(m_two_echo_point_distance, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);
	m_current_frame_number = 0;

	mp_scan_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
	mp_two_echo_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
	mp_thread_analysis = NULL;

	// 设置初始时间为昨天，以避免第一次点云更新前获取到空点云
	m_scan_cloud_time = std::chrono::system_clock::now() - std::chrono::duration<int, std::ratio<60 * 60 * 24>>(1);
	m_two_echo_cloud_time =
	std::chrono::system_clock::now() - std::chrono::duration<int, std::ratio<60 * 60 * 24>>(1);

	memset(&m_polar_coordinates_box, 0, sizeof(Point2D_tool::Polar_coordinates_box));
	memset(&m_point2D_box, 0, sizeof(Point2D_tool::Point2D_box));
	memset(&m_point2D_transform, 0, sizeof(Point2D_tool::Point2D_transform));


	mp_thread_analysis = NULL;
}

Wj_716_lidar_protocol::~Wj_716_lidar_protocol()
{
	uninit();
}

// 初始化
Error_manager Wj_716_lidar_protocol::init(Thread_safe_queue<Binary_buf *> *p_communication_data_queue,
										  Point2D_tool::Polar_coordinates_box polar_coordinates_box,
										  Point2D_tool::Point2D_box point2D_box,
										  Point2D_tool::Point2D_transform point2D_transform)
{
	if (p_communication_data_queue == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}

	mp_communication_data_queue = p_communication_data_queue;
	m_polar_coordinates_box = polar_coordinates_box;
	m_point2D_box = point2D_box;
	m_point2D_transform = point2D_transform;
	//接受线程, 默认开启循环, 在内部的wait_and_pop进行等待
	m_condition_analysis.reset(false, true, false);
	mp_thread_analysis = new std::thread(&Wj_716_lidar_protocol::thread_analysis, this);


	m_wanji_lidar_protocol_status = E_READY;

	return Error_code::SUCCESS;
}

//反初始化
Error_manager Wj_716_lidar_protocol::uninit()
{
	LOG(INFO) << " ---Wj_716_lidar_protocol uninit --- "<< this;

	if ( mp_communication_data_queue )
	{
		//终止队列，防止 wait_and_pop 阻塞线程。
		mp_communication_data_queue->termination_queue();
	}

	//关闭线程
	if (mp_thread_analysis)
	{
		m_condition_analysis.kill_all();
	}
	//回收线程的资源
	if (mp_thread_analysis)
	{
		mp_thread_analysis->join();
		delete mp_thread_analysis;
		mp_thread_analysis = NULL;
	}

	if ( mp_communication_data_queue )
	{
		//清空队列
		mp_communication_data_queue->clear_and_delete();
	}
	//队列的内存由上级管理, 这里写空就好了.
	mp_communication_data_queue = NULL;

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


//检查状态
Error_manager Wj_716_lidar_protocol::check_status()
{
	if ( m_wanji_lidar_protocol_status == E_READY )
	{
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::WJ_PROTOCOL_STATUS_ERROR, Error_level::MINOR_ERROR,
							 " Wj_716_lidar_protocol::check_status() error ");
	}
}

//判断是否为待机，如果已经准备好，则可以执行任务。
bool Wj_716_lidar_protocol::is_ready()
{
	if ( m_wanji_lidar_protocol_status == E_READY )
	{
		return true;
	}
	else
	{
		return false;
	}
}


//获取状态
Wj_716_lidar_protocol::Wanji_lidar_protocol_status Wj_716_lidar_protocol::get_status()
{
	return m_wanji_lidar_protocol_status;
}

//获取扫描周期
int Wj_716_lidar_protocol::get_scan_cycle()
{
	return WANJI_FRAME_SCAN_CYCLE_MS;
}


//获取扫描点云
Error_manager Wj_716_lidar_protocol::get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out)
{
	if ( p_cloud_out.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 " Wj_716_lidar_protocol::get_scan_points POINTER IS NULL ");
	}
	std::unique_lock<std::mutex> lck(m_scan_mutex);
	//将扫描点云追加到p_cloud_out后面, 不要清除p_cloud_out原有的数据
	(*p_cloud_out)+=(*mp_scan_points);

	return Error_code::SUCCESS;
}


//获取二次回波点云
Error_manager Wj_716_lidar_protocol::get_two_echo_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out)
{
	if ( p_cloud_out.get() == NULL )
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 " Wj_716_lidar_protocol::get_two_echo_points POINTER IS NULL ");
	}

	std::unique_lock<std::mutex> lck(m_two_echo_mutex);
	//将扫描点云追加到p_cloud_out后面, 不要清除p_cloud_out原有的数据
	(*p_cloud_out)+=(*mp_two_echo_points);

	return Error_code::SUCCESS;
}


/**
 * 获取更新扫描点云时间
 * */
std::chrono::system_clock::time_point Wj_716_lidar_protocol::get_scan_time()
{
	std::unique_lock<std::mutex> lck(m_scan_mutex);
	return m_scan_cloud_time;
}

/**
 * 获取更新二次回波时间
 * */
std::chrono::system_clock::time_point Wj_716_lidar_protocol::get_two_echo_time()
{
	std::unique_lock<std::mutex> lck(m_two_echo_mutex);
	return m_two_echo_cloud_time;
}


//解析线程函数
void Wj_716_lidar_protocol::thread_analysis()
{
	LOG(INFO) << " ---Wj_716_lidar_protocol::thread_analysis start ---" << this;

	Error_manager t_error;
	//接受雷达消息，包括连接,重连和接受数据
	while (m_condition_analysis.is_alive())
	{
		m_condition_analysis.wait();
		if (m_condition_analysis.is_alive())
		{
			std::this_thread::yield();

			if (mp_communication_data_queue == NULL)
			{
				m_wanji_lidar_protocol_status = E_FAULT;
			}
			else
			{
				Binary_buf *tp_binary_buf = NULL;
				bool is_pop = mp_communication_data_queue->wait_and_pop(tp_binary_buf);
				if (is_pop == true && tp_binary_buf != NULL)
				{
					//检查数据头和长度,
					if (check_head_and_length(tp_binary_buf))
					{
						// 消息完整性检查, 检查末尾的校验码,
						if (check_xor(tp_binary_buf))
						{
							//至此, 我们确认消息已经被正常的接受, 后续错误就不可

							// 万集通信协议, 解析数据, 转化为点云
							t_error = data_protocol_analysis(tp_binary_buf);
							//暂时不管错误
						}
						//else 错误不管, 当做无效消息处理
					}
					//else 错误不管, 当做无效消息处理
					delete(tp_binary_buf);
				}
			}
		}
	}
	LOG(INFO) << " -Wj_716_lidar_protocol::thread_analysis end :" << this;
	return;
}

//检查数据头和长度, 注:返回失败只是表示消息格式不对不是需要处理的消息, 忽略就好了
bool Wj_716_lidar_protocol::check_head_and_length(Binary_buf *p_binary_buf)
{
	//检查总长度, 716固定为898byte
	if (p_binary_buf->get_length() != WANJI_FRAME_LENGTH_716)
	{
		return false;
	}
	char *tp_data = p_binary_buf->get_buf();
	//检查数据头, 716的前4个字节固定为0x02020202
	if (tp_data[0] == 0x02 && tp_data[1] == 0x02 && tp_data[2] == 0x02 && tp_data[3] == 0x02)
	{
		//4~7字节合并为32位的int, 是数据消息里面的长度值, 不包括数据头(4byte) 长度(4byte) 尾部校验码(1byte)
		unsigned int l_u32reallen = (tp_data[4] << 24) |
									(tp_data[5] << 16) |
									(tp_data[6] << 8) |
									(tp_data[7] << 0);
		if (l_u32reallen + 9 == WANJI_FRAME_LENGTH_716)
		{
			return true;
		}
		else
		{
			return false;
		}
	}
	else
	{
		return false;
	}
}

// 消息完整性检查, 检查末尾的校验码, 注:返回失败只是表示消息格式不对不是需要处理的消息, 忽略就好了
bool Wj_716_lidar_protocol::check_xor(Binary_buf *p_binary_buf)
{
	int i = 0;
	char check = 0;
	//校验码的运算, 就是把中间所有的数据异或, 不包括数据头(4byte) 长度(4byte) 尾部校验码(1byte)
	char *tp_data = p_binary_buf->get_buf() + 8;
	int t_length = p_binary_buf->get_length() - 9;

	for (i = 0; i < t_length; i++)
	{
		check ^= *tp_data++;
		//后++, tp_data在最后 还会右移一个
	}
	if (check == *tp_data)
	{
		return true;
	}
	else
	{
		return false;
	}
}

// 万集通信协议, 解析数据, 转化为点云
Error_manager Wj_716_lidar_protocol::data_protocol_analysis(Binary_buf *p_binary_buf)
{
	Error_manager t_error;

	char *data = p_binary_buf->get_buf();
	int len = p_binary_buf->get_length();

	//消息指令类型判断, data[8] == 0x73 为数据类型, data[9] == 0x52||0x53 为传输方向(发送或接受)
	if ((data[8] == 0x73 && data[9] == 0x52) ||
		(data[8] == 0x73 && data[9] == 0x53)) //command type:0x73(s) 0x52/0X53(R/S)
	{
		//716雷达扫描的数据量很大, 一次完整的扫描信息分成了6条消息分开发送,
		//数据的包号l_n32PackageNo, 实际为1~6, 无限循环, 循环完毕后,6条加起来就是一个完整的雷达扫描数据
		//奇数包发送前面的405个点的数据, 偶数包发送后面406个点的数据
		int l_n32PackageNo = (data[50] << 8) + data[51];
		//数据的帧号l_u32FrameNo, 实际为0~2^32, 相同帧号的6条消息表示这6条是雷达一次扫描的完整消息,
		unsigned int l_u32FrameNo = (data[46] << 24) + (data[47] << 16) + (data[48] << 8) + data[49];

		//1~2包为扫描点到雷达的距离, 405+406 = 811,
		if (l_n32PackageNo == 1)
		{
			//重置当前的帧号, 清空数据, 防止出错后复用上一次的数据.
			m_current_frame_number = l_u32FrameNo;
			memset(m_scan_point_intensity, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);
			memset(m_scan_point_distance, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);
			memset(m_two_echo_point_distance, 0, WANJI_SCAN_POINT_NUMBER_TOTAL);

			for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_FORMER; ++i)
			{
				m_scan_point_distance[i] =
				(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
				m_scan_point_distance[i] /= 1000.0;//毫米->米
				//有效距离范围0~25, 超出的点强制改为NAN(无效的浮点数据)
				if (m_scan_point_distance[i] >= 25 || m_scan_point_distance[i] == 0)
				{
					m_scan_point_distance[i] = NAN;
				}
			}
		}
		else if (l_n32PackageNo == 2)
		{
			//检查帧号的一致性
			if (m_current_frame_number == l_u32FrameNo)
			{
				for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_LATTER; ++i)
				{
					m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] =
					(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
					m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] /= 1000.0;//毫米->米
					//有效距离范围0~25, 超出的点强制改为NAN(无效的浮点数据)
					if (m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] >= 25 ||
						m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] == 0)
					{
						m_scan_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] = NAN;
					}
				}
				//至此就得到了扫描点到雷达的距离, 如有需要, 可以再此使用


				//数据加锁的范围
				{
					std::unique_lock<std::mutex> lck(m_scan_mutex);
					mp_scan_points->clear();
					// 把点到雷达的距离, 转化为标准坐标下的点云
					t_error = polar_coordinates_transform_cloud(m_scan_point_distance,
																WANJI_SCAN_POINT_NUMBER_TOTAL, mp_scan_points);
					m_scan_cloud_time = std::chrono::system_clock::now();
				}
			}
			else
			{
				return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
									 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
			}
		}
			//1~2包为扫描点的光强, 405+406 = 811,
		else if (l_n32PackageNo == 3)
		{
			//检查帧号的一致性
			if (m_current_frame_number == l_u32FrameNo)
			{
				for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_FORMER; ++i)
				{
					m_scan_point_intensity[i] =
					(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
				}
			}
			else
			{
				return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
									 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
			}
		}
		else if (l_n32PackageNo == 4)
		{
			//检查帧号的一致性
			if (m_current_frame_number == l_u32FrameNo)
			{
				for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_LATTER; ++i)
				{
					m_scan_point_intensity[WANJI_SCAN_POINT_NUMBER_FORMER + i] =
					(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
				}
				//至此就得到了扫描点的光强, 如有需要, 可以再此使用

			}
			else
			{
				return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
									 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
			}
		}
		else if (l_n32PackageNo == 5)
		{
			//检查帧号的一致性
			if (m_current_frame_number == l_u32FrameNo)
			{
				for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_FORMER; ++i)
				{
					m_two_echo_point_distance[i] =
					(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
					m_two_echo_point_distance[i] /= 1000.0;//毫米->米
					//有效距离范围0~25, 超出的点强制改为NAN(无效的浮点数据)
					if (m_two_echo_point_distance[i] >= 25 || m_two_echo_point_distance[i] == 0)
					{
						m_two_echo_point_distance[i] = NAN;
					}
				}
			}
			else
			{
				return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
									 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
			}
		}
		else if (l_n32PackageNo == 6)
		{
			//检查帧号的一致性
			if (m_current_frame_number == l_u32FrameNo)
			{
				for (int i = 0; i < WANJI_SCAN_POINT_NUMBER_LATTER; ++i)
				{
					m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] =
					(((unsigned char) data[85 + i * 2]) << 8) + ((unsigned char) data[86 + i * 2]);
					m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] /= 1000.0;//毫米->米
					//有效距离范围0~25, 超出的点强制改为NAN(无效的浮点数据)
					if (m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] >= 25 ||
						m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] == 0)
					{
						m_two_echo_point_distance[WANJI_SCAN_POINT_NUMBER_FORMER + i] = NAN;
					}
				}
				//至此就得到了二次反射的扫描点到雷达的距离, 如有需要, 可以再此使用


				//数据加锁的范围
				{
					std::unique_lock<std::mutex> lck(m_two_echo_mutex);
					mp_two_echo_points->clear();
					// 把点到雷达的距离, 转化为标准坐标下的点云
					t_error = polar_coordinates_transform_cloud(m_two_echo_point_distance,
																WANJI_SCAN_POINT_NUMBER_TOTAL, mp_two_echo_points);
					m_two_echo_cloud_time = std::chrono::system_clock::now();
				}
			}
			else
			{
				return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
									 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
			}
		}
		return Error_manager(Error_code::SUCCESS);
	}
	else
	{
		return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED, Error_level::NEGLIGIBLE_ERROR,
							 " Wj_716_lidar_protocol::data_protocol_analysis  parse failed  ");
	}
	return Error_code::SUCCESS;
}

// 把点到雷达的距离, 转化为标准坐标下的点云
Error_manager Wj_716_lidar_protocol::polar_coordinates_transform_cloud(float *p_point_distance, int number,
																	   pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out)
{
	if (p_point_distance == NULL || p_cloud_out.get() == NULL)
	{
		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
							 "  POINTER IS NULL ");
	}
	else
	{
		pcl::PointXYZ point;
		for (int i = 0; i < number; ++i)
		{
			float angle = WANJI_SCAN_POINT_ANGLE_MIN + i * WANJI_SCAN_POINT_ANGLE_INCREMENT;
			//判断极坐标点是否在限制范围
			if (Point2D_tool::limit_with_polar_coordinates_box(p_point_distance[i], angle,
															   &m_polar_coordinates_box))
			{
				//极坐标 -> 平面坐标
				float x = p_point_distance[i] * cos(angle);
				float y = p_point_distance[i] * sin(angle);

				//判断平面坐标点是否在限制范围
				if (Point2D_tool::limit_with_point2D_box(x, y, &m_point2D_box))
				{
					//平面坐标的转换, 可以进行旋转和平移, 不能缩放
					point.x = x * m_point2D_transform.m00 + y * m_point2D_transform.m01 + m_point2D_transform.m02;
					point.y = x * m_point2D_transform.m10 + y * m_point2D_transform.m11 + m_point2D_transform.m12;
					//添加到最终点云
					p_cloud_out->push_back(point);
				}
				//else直接忽略
			}
			//else直接忽略
		}

	}
	return Error_code::SUCCESS;
}