#include <iostream>
#include <stdio.h>
#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include "upbot_vision/ranging.h"
#include <string>
#include <pthread.h>
#include <mutex>
#include <sys/time.h>
#include "opencv2/imgcodecs/imgcodecs.hpp"
#include <ctime>
#include <queue>
#include <cstdlib>
#include "pibot_msgs/dis_info.h"
#include "pibot_msgs/dis_info_array.h"

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

Ranging r(640, 480);

std::queue<std::vector<cv::Mat>> frameInfo;
std::mutex g_mutex;

void *ranging(void *args)		// ranging线程
{
	ros::Publisher dis_pub_;
	ros::NodeHandle nh; 
	// <>内为自定义ros消息类型 存储障碍物信息包括与障碍物的距离、障碍物宽度、高度和与障碍物之间的夹角
    dis_pub_ = nh.advertise<pibot_msgs::dis_info_array>("ceju_info",10);
	while (true)
	{
		/*进入测距并发布障碍物信息*/;
		std::vector<cv::Mat> result = r.get_range();
		cv ::Mat info = result[2];
		pibot_msgs::dis_info_array dis_array;
		if(!info.empty())
		{
			for(int i=0;i<info.rows;i++)
			{
				// std::cout<<"1"<<std::endl;
				if(info.at<float>(i,0)>0&&info.at<float>(i,0)<200)
				{
					pibot_msgs::dis_info data;
					data.distance = info.at<float>(i,0);
					data.width = info.at<float>(i,1);
					data.height = info.at<float>(i,2);
					data.angle = info.at<float>(i,3);
					dis_array.dis.push_back(data);
					// std::cout<<data.distance<<std::endl;
				}	
			}
			
        dis_pub_.publish(dis_array);
		}
		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)
{
	ros::init(argc, argv, "stereo");
	ros::NodeHandle nh; 
	pthread_t tids[1];		// 执行ranging线程
	int ret = pthread_create(&tids[0], NULL, ranging, NULL);
	pthread_join(tids[0],NULL);

	
	return 0;
}
