#include <iostream>
#include <stdio.h>
#include "lidar.h"
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <opencv2/opencv.hpp>
#include "ranging.h"
#include "fusion.h"
#include <string>
#include <pthread.h>
#include <mutex>
#include <sys/time.h>
/*#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>*/
#include "opencv2/imgcodecs/imgcodecs.hpp"
#include <ctime>

#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)

Ranging r(9, 640, 480);
std::queue<std::vector<Mat>> frameInfo;
std::queue<FrameData> laser_queue;
std::mutex g_mutex;

void *ranging(void *args)		// ranging线程
{
	while (true)
	{
		std::cout<<" ************ enter ranging *********** "<<std::endl;
		std::vector<Mat> result = r.get_range();
		g_mutex.lock();
		for (uchar i = 0; i < frameInfo.size(); i++)		// 只保存当前最新的图片帧信息
		frameInfo.pop();
		frameInfo.push(result);
		g_mutex.unlock();
	}
}

int main(int argc, char **argv)
{
	uint8_t laser_queue_size = 2;		// 雷达帧队列大小参数
	LD14_LiPkg *pkg = new LD14_LiPkg;
	CmdInterfaceLinux cmd_port(9);
	std::string port_name = "/dev/ttyUSB0";
	if (argc > 1)
		port_name = argv[1];
// 雷达读取回调函数，一直在执行雷达读取和处理数据
	cmd_port.SetReadCallback([&pkg](const char *byte, size_t len) {
		if (pkg->Parse((uint8_t*)byte, len))
		{
			pkg->AssemblePacket();
		} });
	if (!cmd_port.Open(port_name))
		return -1;

	ros::init(argc, argv, "publish");
	ros::NodeHandle nh; /* create a ROS Node */
	ros::Publisher uplidar_pub = nh.advertise<sensor_msgs::LaserScan>("up_scan", 1);
	//ros::Publisher downlidar_pub = nh.advertise<sensor_msgs::LaserScan>("down_scan", 1);
	sensor_msgs::LaserScan upscan;
	// sensor_msgs::LaserScan downscan;


	/*image_transport::ImageTransport it(nh);
        image_transport::Publisher img_pub = it.advertise("camera/image", 1);*/
	pthread_t tids[1];		// 执行ranging线程
	int ret = pthread_create(&tids[0], NULL, ranging, NULL);
	if (ret != 0)
	{
		std::cout << "Multithreading error !" << std::endl;
		return -2;
	}

	LCFusion lcFusion(&r);
	namedWindow("camera");

	while (ros::ok() && !cmd_port.IsExited())
	{
		int64 t = getTickCount();
		if (pkg->IsFrameReady())		// 若一帧雷达处理好，执行后续操作
		{
			FrameData laserData_queue = pkg->GetFrameData();		// 得到雷达数据
			laserData_queue.timestamp = ros::Time::now();
			laser_queue.push(laserData_queue);
			while(laser_queue.size() > laser_queue_size)		// 只保存两帧雷达数据
			{
				laser_queue.pop();
			}
		}
		else 
			continue;
		if (laser_queue.size() == laser_queue_size)			// 若队列有两帧数据，执行后续操作
		{
			FrameData laserData = laser_queue.front();
			laser_queue.pop();
			upscan.header.stamp = laserData.timestamp;
			// upscan.header.stamp = ros::Time::now();
			// downscan.header.stamp = upscan.header.stamp;

			// uint16_t times_stamp = pkg->GetTimestamp();
			// FrameData laserData = pkg->GetFrameData();
			char state = -1;

			float angle_min = ANGLE_TO_RADIAN(laserData.angle_min);			// 角度弧度转换
			float angle_max = ANGLE_TO_RADIAN(laserData.angle_max);
			uint32_t len = laserData.len;
			float angle_inc = (angle_max - angle_min) / (len - 1.);			// 角度增量
			lcFusion.filter_laser(laserData, angle_min, angle_inc);			// 视角同步，处理视角内雷达数据
			if (!frameInfo.empty())
			{
				g_mutex.lock();
				std::vector<Mat> camInfo = frameInfo.front();				// 读取图像帧数据
				frameInfo.pop();
				g_mutex.unlock();
				double fusion_old,fusion_now;
				fusion_old = ros::Time::now().toSec();
				state = lcFusion.fusion(camInfo);							// 融合
				fusion_now = ros::Time::now().toSec();
				cout << "data_fusion: " << fusion_now - fusion_old << endl;
				if (state != -2)
				{
					lcFusion.set_laser_data(upscan);						// 设置雷达发布数据
					uplidar_pub.publish(upscan);							// 发布
				}
				// 调试使用，将雷达点映射到图像中显示
				if (!lcFusion.projectedPoints.empty())
				{
					for (int i = 0; i < lcFusion.projectedPoints.size(); i++)
					{
						Point2f p = lcFusion.projectedPoints[i];
						circle(camInfo[0], p, 3, Scalar(0, 255, 0), 1, 8);
					}
				}
				 /* sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", camInfo[0]).toImageMsg();
    				img_pub.publish(msg);*/
				if (!camInfo[0].empty())
				{
					cv::Mat pic;
					cv::pyrDown(camInfo[0],pic);
					imshow("camera", pic);
				}
				t = getTickCount() - t;
				char fps[50];
				std::cout << "fps:  " << int(1 / (t / getTickFrequency())) << std::endl;
				if (waitKey(1) == 81)
					break;
			}
			//lcFusion.set_laser_data(downscan, laserData);
			//downlidar_pub.publish(downscan);
		}
	}
	std::cout << "ROS or lidar error !" << std::endl;
	delete pkg;
	cmd_port.Close();
	return 0;
}