#include "uwb.h"
#include <cmath>
#include "Mat.h"


#define CARHEIGHT 35

namespace uwb_slam{
    Mat A;
    Mat b;
    Mat uwbPos;
    int M=3;
    //
    Uwb::Uwb(){
        readConfigFile("/home/firefly/pibot_ros/ros_ws/src/upbot_location/parameter/AnchorPos.txt");
        for(int i=0; i<3; i++)
        {

            for(int j=0; j<3; j++)
            {
                std::cout<<AnchorPos[i][j]<<" ";
            }
            std::cout<<std::endl;
            
        }
        A.Init(2,2,0);
        b.Init(2,1,0);
        uwbPos.Init(2,1,0);

        for(int i=0; i<2; i++)
        {
            A.mat[i][0] = -2*(this->AnchorPos[0][0]-this->AnchorPos[i+1][0]);
            A.mat[i][1] = -2*(this->AnchorPos[0][1]-this->AnchorPos[i+1][1]);
        
        }
            // 初始化 AnchorPos 数组后再初始化 libusb
        int r = libusb_init(&ctx);
        if (r < 0) {
            ROS_ERROR("Failed to initialize libusb");
            return;
        }

        // 获取设备列表
        ssize_t cnt = libusb_get_device_list(ctx, &devs);
        if (cnt < 0) {
            ROS_ERROR("Failed to get device list");
            libusb_exit(ctx);
            return;
        }

        // 使用指定的 vendor 和 product ID 打开设备
        dev_handle = libusb_open_device_with_vid_pid(ctx, 0x0483, 0x5750);
        if (dev_handle == NULL) {
            ROS_ERROR("Cannot open device");
            libusb_free_device_list(devs, 1);
            libusb_exit(ctx);
            return;
        }

        ROS_INFO("Device opened");

        libusb_free_device_list(devs, 1); // 释放设备列表

        if (libusb_kernel_driver_active(dev_handle, 0) == 1) {
            // 检查是否有内核驱动程序激活
            ROS_INFO("Kernel driver active");
            if (libusb_detach_kernel_driver(dev_handle, 0) == 0)
                ROS_INFO("Kernel driver detached");
            else
                ROS_WARN("Failed to detach kernel driver");
        }

        // 请求接口
        r = libusb_claim_interface(dev_handle, 0);
        if (r < 0) {
            ROS_ERROR("Cannot claim interface");
            libusb_close(dev_handle);
            libusb_exit(ctx);
            return;
        }

        ROS_INFO("Interface claimed");
    }


    Uwb::~Uwb() {
        // 释放接口
        int r = libusb_release_interface(dev_handle, 0);
        if (r != 0) {
            ROS_ERROR("Cannot release interface");
        }

        // 关闭设备
        libusb_close(dev_handle);

        // 退出 libusb
        libusb_exit(ctx);
    }

    void Uwb::Run() {
        
        while(1){
        // this->Serread();
            this->usb_hid_read();

        }


    }
       void Uwb::usb_hid_read()
    {
        unsigned char tmp_char[64];
        int transferred;
        tmp_char[0] = 0x01;
        //std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
	    r = libusb_interrupt_transfer(dev_handle, (0x01 | LIBUSB_ENDPOINT_OUT), tmp_char, 64, &transferred, 1000);
        if (r == 0){
		    printf("Set Success.\n");	
	    }else{
		    printf("Set Error.\n");
	    }
        while (ros::ok())
        {
            // 执行批量传输
            r = libusb_interrupt_transfer(dev_handle, (0x01 | LIBUSB_ENDPOINT_IN), tmp_char, 64, &transferred, 1000);
            //std::cout<<r<<std::endl;
            if (r == 0)
            {
                
                this->uwb_data_.uwb_t_ = ros::Time::now();
                memcpy(&this->d[0], &tmp_char[3], sizeof(uint16_t));
                memcpy(&this->d[1], &tmp_char[5], sizeof(uint16_t));
                memcpy(&this->d[2], &tmp_char[7], sizeof(uint16_t));
                std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
                if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) {
                    continue;
                }
                else
                {
                    // for(int i=0; i<3; i++)
                    // {
                    //     this->d[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * (AnchorPos[i][2] - CARHEIGHT));
                    // }
                    // d[0] = ((((4.9083e-07 * d[0]) - 4.6166e-04) * d[0]) + 1.0789) * d[0] + 5.4539;
                    // d[1] = ((((-4.1679e-07 * d[1]) + 5.0999e-04) * d[1]) + 0.7930) * d[1] + 29.8296;
                    // d[2] = ((((2.3514e-07 * d[2]) - 1.8277e-04) * d[2]) + 0.9935) * d[2] + 9.8852;
                    std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
                    Mat k(M,1,0);
                    for (int i = 0; i < M; ++i) {
                        double sum_squared = 0.0;
                        for (int j = 0; j < M; ++j) {
                            sum_squared += pow(AnchorPos[i][j], 2);
                        }
                        k.mat[i][0] = sum_squared;
                    }
                    // 构造矩阵 A
                    Mat A(M,4,0);
                    for (int i = 0; i < M; ++i) {
                        for (int j = 0; j < 3; ++j) {
                            A.mat[i][j] = -2 * AnchorPos[i][j];
                        }
                        A.mat[i][3] = 1.0;
                    }
                    // 计算 y
                    Mat y(M,1,0);
                    for (int i = 0; i < M; ++i) {
                        y.mat[i][0] = pow(d[i], 2) - k.mat[i][0];
                    }
                    Mat AT=~A;
                    uwbPos=(AT*A)%AT*y;
                    this->uwb_data_.x_ = uwbPos.mat[0][0];
                    this->uwb_data_.y_ = uwbPos.mat[1][0];
                    std::cout << "uwbPos:" << this->uwb_data_.x_ << " " << this->uwb_data_.y_ << std::endl;
                }                  
            }
            else
            {
                std::cout<<r<<std::endl;
                ROS_ERROR("Read error");
            }
            ros::spinOnce();
            // loop_rate.sleep();
        }
    }

    void Uwb::Serread(){
        
        try {
        boost::asio::io_service io;
        boost::asio::serial_port serial(io, "/dev/uwb"); // 替换成你的串口设备路径

        serial.set_option(boost::asio::serial_port_base::baud_rate(115200)); // 设置波特率
        serial.set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位
        serial.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); // 设置校验位
        serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位

        uint8_t tmpdata[9];
        // std::cerr << "befor read" << std::endl;
        size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, 9)); // 读取串口数据
        // std::cerr << "after read" << std::endl;
        this->uwb_data_.uwb_t_ = ros::Time::now();
        memcpy(&this->d[0], &tmpdata[3], sizeof(uint16_t));
        memcpy(&this->d[1], &tmpdata[5], sizeof(uint16_t));
        memcpy(&this->d[2], &tmpdata[7], sizeof(uint16_t));
        // std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
        if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) {
            return;
        }
        for(int i=0; i<3; i++)
        {
            this->d[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * (AnchorPos[i][2] - CARHEIGHT));
        }
        d[0] = ((((4.9083e-07 * d[0]) - 4.6166e-04) * d[0]) + 1.0789) * d[0] + 5.4539;
        d[1] = ((((-4.1679e-07 * d[1]) + 5.0999e-04) * d[1]) + 0.7930) * d[1] + 29.8296;
        d[2] = ((((2.3514e-07 * d[2]) - 1.8277e-04) * d[2]) + 0.9935) * d[2] + 9.8852;
        std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;
       
/*2D
        for(int i=0; i<2; i++)
        {
            b.mat[i][0] = (this->d[0]*this->d[0]-this->d[i+1]*this->d[i+1])\
                        -(this->AnchorPos[0][0]*this->AnchorPos[0][0]-this->AnchorPos[i+1][0]*this->AnchorPos[i+1][0])\
                        -(this->AnchorPos[0][1]*this->AnchorPos[0][1]-this->AnchorPos[i+1][1]*this->AnchorPos[i+1][1]);
        }
*/
// 3D       
            // double d[3]={179,517,379};
            Mat k(M,1,0);
            for (int i = 0; i < M; ++i) {
                double sum_squared = 0.0;
                for (int j = 0; j < M; ++j) {
                    sum_squared += pow(AnchorPos[i][j], 2);
                }
                k.mat[i][0] = sum_squared;
                // std::cout << k.mat[i][0]<<"," ;
            }
                // std::cout << std::endl;
            // 构造矩阵 A
            Mat A(M,4,0);
            // std::cout<< "A" << std::endl;
            for (int i = 0; i < M; ++i) {
                for (int j = 0; j < 3; ++j) {
                    A.mat[i][j] = -2 * AnchorPos[i][j];
                    // std::cout<< A.mat[i][j] <<",";
                }
                A.mat[i][3] = 1.0;
                // std::cout<< A.mat[i][3] <<",";
                // std::cout << std::endl;
            }
            // 计算 y
            Mat y(M,1,0);
            // std::cout<< "y" << std::endl;
            for (int i = 0; i < M; ++i) {
                y.mat[i][0] = pow(d[i], 2) - k.mat[i][0];
                // std::cout<< y.mat[i][0] <<",";
            }
                // std::cout << std::endl;
        Mat AT=~A;
        uwbPos=(AT*A)%AT*y;
        this->uwb_data_.x_ = uwbPos.mat[0][0];
        this->uwb_data_.y_ = uwbPos.mat[1][0];
        std::cout << "uwbPos:" << this->uwb_data_.x_ << " " << this->uwb_data_.y_ << std::endl;
    } catch (const std::exception& ex) {
        std::cerr << "Exception: " << ex.what() << std::endl;
    }


    }


};

