///摄像头
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <librealsense2/hpp/rs_processing.hpp>
#include "example.hpp" 

#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>
#include <set>
#include <string>
#include <mutex>
#include <sstream>

#include <fstream>
#include <algorithm>
#include <cstring>

// opencv 用于图像数据读取与处理
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
// #include <opencv2/core/eigen.hpp>

// 使用Eigen的Geometry模块处理3d运动
#include <Eigen/Core>
#include <Eigen/Geometry> 
#include <Eigen/Dense>

#include <opencv2/core/eigen.hpp>
/*........................................................................*/

#include<librealsense2/rsutil.h>
///摄像头
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <librealsense2/hpp/rs_processing.hpp>
#include "../include/example.hpp" 
#include <opencv2/opencv.hpp>

/*........................................................................*/
// pcl
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/conditional_removal.h>         //条件滤波器头文件
#include <pcl/filters/passthrough.h>                 //直通滤波器头文件
#include <pcl/filters/radius_outlier_removal.h>      //半径滤波器头文件
#include <pcl/filters/statistical_outlier_removal.h> //统计滤波器头文件
#include <pcl/filters/voxel_grid.h>                  //体素滤波器头文件
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>

#include <pcl/PointIndices.h>

// boost.format 字符串处理
//#include <boost/format.hpp>

#include "Yolo.hpp"
#include "DepthProcessor.hpp"
#include "IMUProcessor.hpp"


// 语音
#include <string.h>
#include <sys/types.h>
#include <unistd.h>
#include <stdlib.h>
#include "serialport.h" 
#include<map>
#include<chrono>
using namespace std;

// 超声波
#include "../include/dyp-a05/sonar_driver.h"
#include <bitset>
#include <thread>
#include <condition_variable>
#include<functional>
// 蜂鸣器
#include <JetsonGPIO.h>
#include <signal.h>
/* Private variables ---------------------------------------------------------*/
static int addr = 0;
static int error = 0;
static int fdcom = 0;
static PortInfo_t portinfo = {9600, 8, 0, 0, 1, 0};

/*
=========================================
|   header   |  id  |  指令  |  校验和  |
-----------------------------------------
| 0x55 0xaa |  0x01  |  0x01 |   0x01   |
=========================================
*/
static unsigned char tx_buffer[TX_BUFFER_MAX] = {0x55, 0xaa, 0x01, 0x01, 0x01};

/*
=========================================
|  帧头     |  地址  |  指令  |  校验和  |
-----------------------------------------
| 0x55 0xaa |  0x01  |  0x01 |   0x01   |
=========================================
*/
static unsigned char rx_buffer[RX_BUFFER_MAX];

static double Distance[4] = {0, 0, 0, 0};
std::string port = "/dev/ultrasound";

/*初始化参数定义*/
int ini = 0;
int initial_times = 10;
bool initial = false;
double max_wrong =2.5;
double min_wrong =0.25;
double dis0[10];
double dis1[10];

bool end_this_program = false;
// 差值
double dis0_d = 0.0;
double dis1_d = 0.0;
// 差值阈值
double Thr = 0.5; 

// 播报选择
static int voice_status = 0; 
std::mutex mtx;
bool ToPlay = false;
int distt;
bool isObjectt;
// declare
void play_voice(string);

// 检查校验和
int data_check(unsigned char buf[RX_BUFFER_MAX], int datalen)
{
    int checksum = 0;

    if((buf[0]==0xFF) && datalen == 10)
    {
        for(int i=0; i<RX_BUFFER_MAX-1; i++)
        {
            checksum += buf[i];
        }
        if((checksum&0x00ff)== buf[RX_BUFFER_MAX-1])
        {
            return 1;
        }
    }
	
    printf("first : %x,校验和不对, 数据长度datalen: %d\n", buf[0],datalen);
    error = 2;
    return 0;
}

// 超声波高低八位合并
unsigned int data_merge(unsigned short dt_h, unsigned short dt_l)
{
    unsigned int tmp;
    tmp = dt_h;
    tmp <<= 8;
    tmp |= dt_l;
    return tmp;
}

// 物体转16进制
map<string, char> Objects = {
		{"person", 0x51},
		{"bicycle", 0x52},
		{"motorcycle", 0x53},
		{"bus", 0x54},
		{"cat", 0x55},
		{"dog", 0x56},
		{"fire hydrant", 0x57},
		{"bottle", 0x58},
		{"cup", 0x59},
		{"chair", 0x5a},
		{"couch", 0x5b},
		{"potted plant", 0x5c},
		{"bed", 0x5d},
		{"dining table", 0x5e},
		{"toilet", 0x5f},
		{"tv", 0x60},
		{"refrigerator", 0x61},
		
        {"object", 0x6e},
    // 待增物体todo
};

// 播报函数
// 10->16进制，conver使用
char tenTo16(int i){
	char output = 'a' ;
	switch(i){
	case 1: output = 0x01; break;
	case 2: output = 0x02; break;
	case 3: output = 0x03; break;
	case 4: output = 0x04; break;
	case 5: output = 0x05; break;
	case 6: output = 0x06; break;
	case 7: output = 0x07; break;
	case 8: output = 0x08; break;
	case 9: output = 0x09; break;
	case 0: output = 0x0e; break;
	}
	return output;
}

// 待传输数组，结合距离和物体，未用
void conver(char out[], double dis, string obj){
	// todo 四舍五入一下 0.29->0.3
	int ge = -1,shi=-1,bai=-1,qian=-1, dian= -1, dian2 = -1;;
	//char out[] = {0x8b};
	//auto *beg = begin(out);
	int p = 0;
	//printf("qian: %d,bai:%d,shi:%d,ge:%d,dian:%d\n",qian,bai,shi,ge,dian);
	//将距离转为十六进制
	if(dis >= 1000){
		qian = (int)dis/1000;
		bai = (int)dis/100%10;
		shi = (int)dis/10%10;
		ge = (int)dis%10;
	}else if(dis >= 100){
		bai = (int)dis/100;
		shi = (int)dis/10%10;
		ge = (int)dis%10;
	}else if(dis >= 10){
		shi = (int)dis/10;
		ge = (int)dis%10;
	}else if(dis >=1){
		ge = (int)dis%10;
	}else{
		ge = 0;
	}
	dian = (int)(dis*10)%10;	
	dian2 = (int)(dis*100)%10;	
	//printf("qian: %d,bai:%d,shi:%d,ge:%d,dian:%d\n",qian,bai,shi,ge,dian);
	if(qian != -1){
		out[p++] = tenTo16(qian);
		out[p++] = 0x0c;
		out[p++] = tenTo16(bai);
		out[p++] = 0x0b;
		out[p++] = tenTo16(shi);
		out[p++] = 0x0a;
	}else if(bai != -1){
		out[p++] = tenTo16(bai);
		out[p++] = 0x0b;
		out[p++] = tenTo16(shi);
		out[p++] = 0x0a;
	}else if(shi != -1){
		out[p++] = tenTo16(shi);
		out[p++] = 0x0a;
	}
	// 个位数
	out[p++] = tenTo16(ge);
	// 点数
	out[p++] = 0x0d;
	// 小数点后一位
	out[p++] = tenTo16(dian);
	// 小数点后两位
	out[p++] = tenTo16(dian2);

	//printf("p:%d, out[0]:%x\n",p,out[1]);
	//将string转换成对应的十六进制
	auto iter = Objects.find(obj);
	if(iter != Objects.end()){
		out[p++] = iter->second;
	}
	
	//return out;
}

// 串口发送起始120，十六进制78
bool serial_com(char n16){
	SerialPort serialport;
	SerialPort::Port_INFO *pPort;
	// Init SerialPort 
	int COM_id = 0;
	pPort = (SerialPort::Port_INFO *)malloc(sizeof(SerialPort::Port_INFO)); 
	if(pPort == NULL) {
		fprintf(stderr, "Error malloc...  [FAIL]\n");
	}
	memset(pPort, 0, sizeof(SerialPort::Port_INFO)); 
	int result = serialport.InitPort(pPort, COM_id); 
	if(result < 0) { 
		fprintf(stderr, "Error Running SerialPort...  [FAIL]\n"); 
	}
	// fprintf(stdout, "SerialPort [%s] running...  [OK]\n", pPort->name); 
		
	int send_size = 15;
	char serial_sendmsg[send_size] = {n16};

	int msg_len = strlen(serial_sendmsg);	
	int send_nbyte = serialport.sendnPort(pPort,const_cast<char*>(serial_sendmsg),msg_len);
	if (send_nbyte > 0) {
	// 收到数据
		printf("Send: [%d] ...   [OK]\n", send_nbyte);
		return true;
	}

	return false;
}

// 串口发送接口函数-发送单个物体
bool serial_com(string object){
	SerialPort serialport;
	SerialPort::Port_INFO *pPort;
	// Init SerialPort 
	int COM_id = 0;
	pPort = (SerialPort::Port_INFO *)malloc(sizeof(SerialPort::Port_INFO)); 
	if(pPort == NULL) {
		fprintf(stderr, "Error malloc...  [FAIL]\n");
	}
	memset(pPort, 0, sizeof(SerialPort::Port_INFO)); 
	int result = serialport.InitPort(pPort, COM_id); 
	if(result < 0) { 
		fprintf(stderr, "Error Running SerialPort...  [FAIL]\n"); 
	}
	
	// COM Send 
	// 设置参数
	// 物体
	//string object= "bed";
	// 距离
	//double Distance= 5.5;
	
	int send_size = 15;
	char serial_sendmsg[send_size] = {};

	auto iter = Objects.find(object);
	if(iter != Objects.end()){
		serial_sendmsg[0] = iter->second;
	}
	// conver(serial_sendmsg, Distance, object);

	int msg_len = strlen(serial_sendmsg);	
	int send_nbyte = serialport.sendnPort(pPort,const_cast<char*>(serial_sendmsg),msg_len);
	if (send_nbyte > 0) {
	// todo...
		fprintf(stdout, "Send: [%d] %s...  [OK]\n", send_nbyte, serial_sendmsg);
		return true;
	}
	return false;

}

// 串口发送接口函数-发送多个物体类别
bool serial_com(set<string> objects){
	SerialPort serialport;
	SerialPort::Port_INFO *pPort;
	/* Init SerialPort */
	int COM_id = 0;
	pPort = (SerialPort::Port_INFO *)malloc(sizeof(SerialPort::Port_INFO)); 
	if(pPort == NULL) {
		fprintf(stderr, "Error malloc...  [FAIL]\n");
	}
	memset(pPort, 0, sizeof(SerialPort::Port_INFO)); 
	int result = serialport.InitPort(pPort, COM_id); 
	if(result < 0) { 
		fprintf(stderr, "Error Running SerialPort...  [FAIL]\n"); 
	}
	
	int send_size = 50;
	char serial_sendmsg[send_size] = {0x02};
	// conver(serial_sendmsg, distance, object);
	int p=0;
	for(auto e: objects){
		// Objects
		auto iter = Objects.find(e);
		if(iter != Objects.end()){
			// 找到
			serial_sendmsg[p++] = iter->second;
		}
	}

	int msg_len = strlen(serial_sendmsg);	
	int send_nbyte = serialport.sendnPort(pPort,const_cast<char*>(serial_sendmsg),msg_len);
	if (send_nbyte > 0) {
	// todo...
		fprintf(stdout, "Send: [%d] %s...  [OK]\n", send_nbyte, serial_sendmsg);
		return true;
	}
	return false;

}


// 串口接收函数
void serial_receive(char response_buf[]){
	SerialPort serialport;
	SerialPort::Port_INFO *pPort;
	// Init SerialPort 
	int COM_id = 0;
	pPort = (SerialPort::Port_INFO *)malloc(sizeof(SerialPort::Port_INFO)); 
	if(pPort == NULL) {
		fprintf(stderr, "Error malloc...  [FAIL]\n");
	}
	memset(pPort, 0, sizeof(SerialPort::Port_INFO)); 
	int result = serialport.InitPort(pPort, COM_id); 
	if(result < 0) { 
		fprintf(stderr, "Error Running SerialPort...  [FAIL]\n"); 
	}

	// COM Receive 
	int receive_nbyte = 0;
	int size = COM_BUFFER_SIZE;
	// char response_buf[COM_BUFFER_SIZE] = {'\0'};
	while (1) {
		// 收取一次
		receive_nbyte = serialport.signal_recvnPort(pPort, response_buf, size);
		if(receive_nbyte > 0) {
			for(int i = 0; i < receive_nbyte; i++) {
				response_buf[i] = response_buf[i];
			}
			response_buf[receive_nbyte] = '\0';
			break;
		}
		// if (receive_nbyte == 0) break;
	}
	// fprintf(stdout, "Receive: [%d] %s...  [OK]\n", receive_nbyte, response_buf);  
}

// 为了剔除类别中不属于检测出的类别
std::vector<std::string> currentLabel= {
	"person", "bicycle", "motorcycle", "bus", "cat", "dog", "fire hydrant", 
	"bottle", "cup", "chair", "couch", "potted plant", "bed", 
	"dining table", "toilet", "tv", "refrigerator",
};

// 检查label是否在currentLabel中
bool labelCheck(std::string label){
	// 使用 std::find() 在 vector 中查找目标字符串
    auto it = std::find(currentLabel.begin(), currentLabel.end(), label);

    if(it != currentLabel.end()) {
        return 1;
    }else{
        return 0;
    }
}

// 超声波检测线程函数
void ultrasound_play(){
	fdcom = uart_init(port.c_str());
	std::cout << fdcom << std::endl;
	if(fdcom > 0)
    {
        uart_set(fdcom, &portinfo);
        printf("[板卡%d通信参数]: %d, 8, 0, 0, 1, 0.\n",addr,9600);
    }
    while (true)
    {
        if(error)
        {
            Distance[0] = 0;
            Distance[1] = 0;
            Distance[2] = 0;
            Distance[3] = 0;
           // std::cout<< "串口打开错误！"<<std::endl;
			uart_deinit(fdcom);
			fdcom = 0;
			// 尝试恢复
			while(fdcom <= 0 ){
        		fdcom = uart_init(port.c_str());
        		printf("超声波串口打开错误,正在恢复..........\n");
    		}
			error = 0;
			if(fdcom > 0)
    		{
        		uart_set(fdcom, &portinfo);
        		printf("fdcom:%d重启成功！！！！！！！！！！\n", fdcom);
    		}
            continue;							
        }
        else
        {
            uart_txd(fdcom, tx_buffer, TX_BUFFER_MAX);
            // 接收数据
            int datalen = uart_rxd(fdcom, rx_buffer, RX_BUFFER_MAX, portinfo.baudrate);
            if(data_check(rx_buffer, datalen))
            {
                Distance[0] = double(data_merge(rx_buffer[1], rx_buffer[2]))/1000;// 前一个为high，后一个为low
                Distance[1] = double(data_merge(rx_buffer[3], rx_buffer[4]))/1000;
                Distance[2] = double(data_merge(rx_buffer[5], rx_buffer[6]))/1000;
                Distance[3] = double(data_merge(rx_buffer[7], rx_buffer[8]))/1000;
                // std::cout<<"Distance[0]:"<<Distance[0]<<"米 "<<"Distance[1]:"<<Distance[1]<<"米 "<<"Distance[2]:"<<Distance[2]<<"米 "<<"Distance[3]:"<<Distance[3]<<"米 "<<std::endl;

                if(((Distance[0] <= Thr)&&(Distance[0]>min_wrong)) || ((Distance[1] <= Thr)&&(Distance[1]>min_wrong))){
					// 定义全局变量
					{
						std::cout<<"Distance[0]:"<<Distance[0]<<"米 "<<"Distance[1]:"<<Distance[1]<<"米 "<<"Distance[2]:"<<Distance[2]<<"米 "<<"Distance[3]:"<<Distance[3]<<"米 "<<std::endl;
						ToPlay = true;
						voice_status = 3;
					}
					continue;

                }
				else ToPlay = false;
            }
        }
		
    }
	uart_deinit(fdcom);
}


// 要播报的物体类别
// 播报前赋值，播报完清空
set<string> classes = {}; 

// 播报全部类别, 新开一个线程等待串口的指令
void play_all_class(){
	// 接收到202就发送全部物体类别
	char resp[COM_BUFFER_SIZE];
	std::string resp_s;
	while(1){
		usleep(200000);
		serial_receive(resp);
		resp_s = resp;

		if(resp_s == "202"){
			if(classes.empty()){
				// std::cout<< "wbw111"<< std::endl;
				if(!serial_com(0x73)){
					std::cout<< "115发送失败,没有物体不播报"<< std::endl;
				}
			}else{
				voice_status = 4;
				// 任意传入，不会播报传入的物体
				play_voice("cup");
			}
		}
	}	
}

// 语音播报 play_voice(label)
void play_voice(string object){
	// 上锁
	std::lock_guard<std::mutex> lock(mtx);
	// 发送开始的200
	if(!serial_com(0x78)){
		std::cout<< "200发送失败,此次不播报"<< std::endl;
	}
	switch(voice_status){
		case 1: {
			// 发送距离和障碍物
			if(serial_com(object)){
				std::cout << "finish_object_label" << std::endl;
			}
			break;
		}
		case 2:{
			// 发送距离
			if(serial_com("object")){
				std::cout << "finish_object" << std::endl;
			}
			break;
		}
		case 3:{
			// 超声波检测,传入无用的一个字符，语音不做处理
			if(serial_com(0x0f)){
				std::cout << "finish_ultrasound" << std::endl;
			}
			break;
		}
		case 4:{
			// 全部类别播报
			if(serial_com(classes)){
				std::cout<< classes.size() << std::endl;
				std::cout << "finish_all_classes" << std::endl;
				classes.clear();
			}
			break;
		}
		case 0:{
			std::cout << "仍在初始化阶段" << std::endl;
		}
	}
}

// 蜂鸣器播报
void signalHandler(int s) { end_this_program = true; }

void Bee()// 启动
{
	int output_pin = 13;
    GPIO::setmode(GPIO::BCM);
    //signal(SIGINT, signalHandler);
    GPIO::setup(output_pin, GPIO::OUT, GPIO::LOW);
	std::cout << "Bee" << std::endl;
	while(!end_this_program){
		if(ToPlay || (isObjectt && distt <= 0.5))
		{
			// set pin as an output pin with optional initial state of HIGH
			GPIO::output(output_pin, GPIO::HIGH);
			usleep(100000);// 300000
			GPIO::output(output_pin, GPIO::LOW);
			usleep(100000);
		}else if(isObjectt && distt > 0.5){
			GPIO::output(output_pin, GPIO::HIGH);
			usleep(100000);// 300000
			GPIO::output(output_pin, GPIO::LOW);
			usleep(300000);
		}
		else GPIO::output(output_pin, GPIO::LOW);
	}
	GPIO::cleanup();
}

int main(){
	rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);
	// 声明 RealSense 管道，封装实际设备和传感器
	rs2::pipeline cameraPipe, imuPipe;
	// 使用推荐的默认配置开始流式传输
	// 默认视频配置包含深度流和色彩流
	// 如果设备能够流式传输 IMU 数据，则默认启用陀螺仪和加速计
	rs2::config cameraCfg, imuCfg;
	// IMU数据流设置帧率会报错，或是帧率设置过大？
	imuCfg.enable_stream(rs2_stream::RS2_STREAM_ACCEL, rs2_format::RS2_FORMAT_MOTION_XYZ32F);
	imuCfg.enable_stream(rs2_stream::RS2_STREAM_GYRO, rs2_format::RS2_FORMAT_MOTION_XYZ32F);
	// Declare object that handles camera pose calculations
	IMUProcessor imuProcessor;
	rs2::pipeline_profile imuProfile = imuPipe.start(imuCfg, [&](rs2::frame frame)
	{
        // Cast the frame that arrived to motion frame
        auto motion = frame.as<rs2::motion_frame>();
        // If casting succeeded and the arrived frame is from gyro stream
        if (motion && motion.get_profile().stream_type() == RS2_STREAM_GYRO && motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F)
        {
            // Get the timestamp of the current frame
            double ts = motion.get_timestamp();
            // Get gyro measures
            rs2_vector gyroData = motion.get_motion_data();
            // Call function that computes the angle of motion based on the retrieved measures
            imuProcessor.process_gyro(gyroData, ts);
        }
        // If casting succeeded and the arrived frame is from accelerometer stream
        if (motion && motion.get_profile().stream_type() == RS2_STREAM_ACCEL && motion.get_profile().format() == RS2_FORMAT_MOTION_XYZ32F)
        {
            // Get accelerometer measures
            rs2_vector accelData = motion.get_motion_data();
            // Call function that computes the angle of motion based on the retrieved measures
            imuProcessor.process_accel(accelData);
        }
    });

	cameraCfg.enable_stream(rs2_stream::RS2_STREAM_COLOR, 640, 480, rs2_format::RS2_FORMAT_BGR8, 30);
	cameraCfg.enable_stream(rs2_stream::RS2_STREAM_DEPTH, 640, 480, rs2_format::RS2_FORMAT_Z16, 30);

	rs2::pipeline_profile cameraProfile = cameraPipe.start(cameraCfg);

	// 创建一个对齐模块
	rs2::align align2Depth(RS2_STREAM_DEPTH);
	// 对掩码进行腐蚀，定义腐蚀核
	cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));

	// 创建yolov8
	Yolo yoloV8("/home/jetson/tmp/obstacle-detection-new1/workspace/best.transd.engine");
	// 创建点云处理器
	string cfgPath = "";
	cv::Mat mK(4, 4, CV_32F);
	DepthProcessor depthProcessor(cfgPath, mK);
	depthProcessor.fx = 388.588;
	depthProcessor.fy = 388.588;
	depthProcessor.cx = 322.771;
	depthProcessor.cy = 236.947;

	bool isObject = false, isLabel = false;
	std::string label = "";
	auto startTime = std::chrono::high_resolution_clock::now();
	int count = 1;

	// 初始化时必须让摄像头看到地面
	rs2::frameset allFrames = cameraPipe.wait_for_frames();
    float3 theta = imuProcessor.get_theta();
    rs2::frameset alignedFrames = align2Depth.process(allFrames);
    rs2::depth_frame depthFrame = alignedFrames.get_depth_frame();
    rs2::video_frame colorFrame = alignedFrames.get_color_frame();

	//获取深度图的长宽
	int widthD = depthFrame.get_width();
	int heightD = depthFrame.get_height();
	cv::Mat depthMat(cv::Size(widthD, heightD), CV_16U, (void*)depthFrame.get_data(), cv::Mat::AUTO_STEP);
	
	// 将彩色图像转换为cv::Mat类型
	int widthC = colorFrame.get_width();
	int heightC = colorFrame.get_height();
	cv::Mat colorMat(cv::Size(widthC, heightC), CV_8UC3, (void*)colorFrame.get_data(), cv::Mat::AUTO_STEP);

	// 定义变换矩阵（平移和旋转）
	Eigen::Affine3f transform = Eigen::Affine3f::Identity();
	transform.translation() << 0.0, 0.0, 0.0;  // 平移
	transform.rotate(Eigen::AngleAxisf(-theta.x, Eigen::Vector3f::UnitZ()) * 
					Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
					Eigen::AngleAxisf(theta.z + PI_FL/2, Eigen::Vector3f::UnitX()));

	int init_count = 0;
	float tmp_height = 0;
	// 关闭蜂鸣器
	int output_pin_ = 13;
    GPIO::setmode(GPIO::BCM);
    //signal(SIGINT, signalHandler);
    GPIO::setup(output_pin_, GPIO::OUT, GPIO::LOW);
	GPIO::output(output_pin_, GPIO::LOW);

	//相机初始化 10次
	while(true){
		if(!depthProcessor.camHighInit(depthMat, transform)){
			std::cout << "初始化失败" << std::endl;
		}
		else{
			tmp_height += depthProcessor.camHigh;
			init_count ++;
			if(init_count == 10)
				break;
		}
		theta = imuProcessor.get_theta();

		transform = Eigen::Affine3f::Identity();
		transform.translation() << 0.0, 0.0, 0.0;

		transform.rotate(Eigen::AngleAxisf(-theta.x, Eigen::Vector3f::UnitZ()) * 
					Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
					Eigen::AngleAxisf(theta.z + PI_FL/2, Eigen::Vector3f::UnitX()));
		
		allFrames = cameraPipe.wait_for_frames();
		alignedFrames = align2Depth.process(allFrames);
		depthFrame = alignedFrames.get_depth_frame();
		depthMat = cv::Mat(cv::Size(widthD, heightD), CV_16U, (void*)depthFrame.get_data(), cv::Mat::AUTO_STEP);
	}
	depthProcessor.camHigh = tmp_height / init_count;
	
	std::cout << "相机初始化:" << depthProcessor.camHigh << std::endl;

	// 超声波模块参数初始化
	for(int i = 0 ; i < initial_times; i++)// 先初始化数组里的数为0
    {
        dis0[i] = 0.0;
        dis1[i] = 0.0;
    }

	// 创建超声波线程
	std::thread ultrasonicThread(ultrasound_play);
	ultrasonicThread.detach();
	// 创建蜂鸣器线程
	std::thread BeeThread(Bee);
	BeeThread.detach();
	// // classes
	std::thread playAllClassThread(play_all_class);
	playAllClassThread.detach();

	// 相机主循环

	while(true) {

		//等待来自摄像机的下一组图像
		rs2::frameset allFrames = cameraPipe.wait_for_frames();
		float3 theta = imuProcessor.get_theta();

		// 对齐深度图像和彩色图像
		rs2::frameset alignedFrames = align2Depth.process(allFrames);
		// 获取对齐后的深度图像和彩色图像
		rs2::depth_frame depthFrame = alignedFrames.get_depth_frame();
		rs2::video_frame colorFrame = alignedFrames.get_color_frame();
		
		// 检查是否成功获取数据 
		if (depthFrame && colorFrame) {
			auto start = std::chrono::high_resolution_clock::now();

			// 将深度图像转换为cv::Mat类型
			int widthD = depthFrame.get_width();
			int heightD = depthFrame.get_height();
			cv::Mat depthMat(cv::Size(widthD, heightD), CV_16U, (void*)depthFrame.get_data(), cv::Mat::AUTO_STEP);
			
			// 将彩色图像转换为cv::Mat类型
			int widthC = colorFrame.get_width();
			int heightC = colorFrame.get_height();

			cv::Mat colorMat(cv::Size(widthC, heightC), CV_8UC3, (void*)colorFrame.get_data(), cv::Mat::AUTO_STEP);

			// 定义变换矩阵（平移和旋转）
			Eigen::Affine3f transform = Eigen::Affine3f::Identity();
			transform.translation() << 0.0, 0.0, 0.0;  // 平移
			transform.rotate(Eigen::AngleAxisf(-theta.x, Eigen::Vector3f::UnitZ()) * 
						Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
						Eigen::AngleAxisf(theta.z + PI_FL/2, Eigen::Vector3f::UnitX()));
				
			isObject = false;
			// 在深度图中检测最近障碍物,返回是否存在障碍物，0不存在，1存在
			try {
				isObject = depthProcessor.obstacleDetect(depthMat, transform);
				if(isObject)
				{
					isObjectt = true;
					distt = depthProcessor.dist;
				}
				else isObjectt = false;
			}
			catch (const std::exception& e) {
				std::cerr << "Caught exception: " << e.what() << std::endl;
				isObject = false;
			}
			
			// YOLOv8分割+检测
			BoxArray objs = yoloV8.detect(colorMat);
			
			if(isObject) {
				cv::rectangle(colorMat, cv::Point(depthProcessor.min_x, depthProcessor.min_y),
							cv::Point(depthProcessor.max_x, depthProcessor.max_y),
							cv::Scalar(0, 0, 255), 1);
				cv::String text = cv::format("dis:%.2f", depthProcessor.dist);
				cv::putText(colorMat, text, cv::Point(depthProcessor.min_x, depthProcessor.min_y+10),
						0, 0.7, cv::Scalar(0,0,255), 1, 8);
			}
			
			float tmp_depth = 100;
			if(!classes.empty()){
				classes.clear();
			}
			// 输出识别框信息
			for(Box &obj : objs) {
				cout <<  yoloV8.cocoLabels[obj.classLabel] << "  ";
			}
			cout << endl;
			for(Box &obj : objs) {
				float depth = 0;
				int i = 0;
				cv::Mat mask(obj.seg->height, obj.seg->width, CV_8U, obj.seg->data);
				int objWidth = obj.right - obj.left;
				int objHeight = obj.bottom - obj.top;
				cv::resize(mask, mask, cv::Size(objWidth, objHeight));
				// 进行腐蚀操作
				cv::erode(mask, mask, kernel);

				// 将掩码绘制到RGB图像上
				//cv::Rect roi(obj.left, obj.top, obj_width, obj_height);
				//cv::Mat maskColor;
				//cv::cvtColor(mask, maskColor, cv::COLOR_GRAY2BGR);
				//maskColor.copyTo(colorMat(roi));
				
				for(int x = 0; x < objWidth; x++){
					for(int y = 0 ;y < objHeight; y++){
						if(mask.at<uchar>(y, x) > 10){
							uint16_t d = depthMat.at<uint16_t>(obj.top + y, obj.left + x);
							if(d > 300) {
								depth += depthMat.at<uint16_t>(obj.top + y, obj.left + x);
								i++;
							}
						}
					}
				}
				depth = depth/(i*1000);

				if(isObject){
					if (depthProcessor.max_x > obj.left && depthProcessor.min_x < obj.right 
			&& depthProcessor.max_y > obj.top && depthProcessor.min_y < obj.bottom){
						double rate = depthProcessor.IOU(depthProcessor.max_x, depthProcessor.min_x, depthProcessor.max_y, depthProcessor.min_y,
							obj.right, obj.left, obj.bottom, obj.top);
						if(rate >= 0.3){
							isLabel = true;

							if(tmp_depth > depth){
								tmp_depth = depth;
								label = yoloV8.cocoLabels[obj.classLabel];
							}
							else	isLabel = false;
							if(!labelCheck(label)){
								// 如果标签不在所需要的标签中
								isLabel = false;
							}

						}

					}
				}
				// if(/*isObject && */isLabel){
				// 	// if(abs(depthProcessor.dist - depth) > 0.2){
				// 	// 	isLabel = false;
						
				// 	// 	std::cout << " depthProcessor.dist = " << depthProcessor.dist << " depth =" << depth << std::endl;
				// 	// }
						
				// 	// else 
				// 	if(tmp_depth > depth){
				// 		tmp_depth = depth;
				// 		std::cout << "something near" <<std::endl;
				// 	}
						
				// }
				// 要播报的全部类别
				classes.insert(yoloV8.cocoLabels[obj.classLabel]);

				cv::String depthText = cv::format("d:%.2f", depth);
				cv::putText(colorMat, depthText, cv::Point(obj.left, obj.top+10), 0, 0.7, cv::Scalar(0,255,0), 1, 8);
				
			}
			
			auto endTime = std::chrono::high_resolution_clock::now();
			auto duration = std::chrono::duration_cast<std::chrono::seconds>(endTime - startTime);
			// 语音播报,间隔大于等于秒在进行下一次
			if(duration.count() >= 7 || count == 1){
				if(ToPlay){
					// 超声波播报
					std::cout << std::endl << "************************有障碍物;dis0_d: " << dis0_d << "|| dis1_d: " << dis1_d << std::endl;
					// play_voice("object");
					// Bee(300000);
					// 回置全局变量
					// ToPlay = false;
					voice_status = 0;
					startTime = std::chrono::high_resolution_clock::now();
					count++;
				}else{
					// 相机播报
					if(isObject && isLabel){
						isLabel = false;
						std::cout << std::endl;
						std::cout << "前方" << depthProcessor.dist << "米存在" << label << std::endl;
						{
							voice_status = 1;
						}
						play_voice(label);
						voice_status = 0;
						startTime = std::chrono::high_resolution_clock::now();
						count++;
					}else if(isObject){
						std::cout << std::endl;
						std::cout << "前方" << depthProcessor.dist << "米有障碍物"<< std::endl;
						{
							voice_status = 2;
						}
						play_voice(label);
						voice_status = 0;
						startTime = std::chrono::high_resolution_clock::now();
						count++;
					}
				}
			}

			isLabel = false;
			// 回置count
			if(count == 100){
				count = 2;
			}

			cv::imshow("RGB Image", colorMat);
			if (cv::waitKey(5) == 27) break;
			auto end = std::chrono::high_resolution_clock::now();
			auto duration_test = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
    		std::cout << "Time taken: " << duration_test << " microseconds" << std::endl;
		}
		
	}
	
	return 0;
}