#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(){
        
        
            // 初始化 AnchorPos 数组后再初始化 libusb
        int r = libusb_init(NULL);
        if (r < 0) {
            ROS_ERROR("Failed to initialize libusb");
            return;
        }

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

        // 使用指定的 vendor 和 product ID 打开设备
        dev_handle = libusb_open_device_with_vid_pid(NULL, 0x0483, 0x5750);
        if (dev_handle == NULL) {
            ROS_ERROR("Cannot open device");
            libusb_free_device_list(devs, 1);
            libusb_exit(NULL);
            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(NULL);
            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(NULL);
    }

    void Uwb::Run() {
        // std::string filename = "/home/firefly/pibot_ros/ros_ws/src/upbot_mapping/data.txt";
        // outfile.open(filename);
        // if(outfile.is_open()) {
        //     std::cout << "start saving data" << std::endl;
        //     outfile << "x,y"<<"\n";
        // } else {
        //     std::cout<<"file can not open"<<std::endl;
        // }
        while(1){
        // this->Serread();
            this->usb_hid_read();

           // std::cout<<"s"<<std::endl;
           // std::cout<<this->x<<std::endl;
        /*if(t_tmp!=imu_odom_.imu_data_.imu_t_){
            imu_odom_queue_.push(imu_odom_);
            t_tmp=imu_odom_.imu_data_.imu_t_;
        }*/
        }

        // outfile.close();

    }
       void Uwb::usb_hid_read()
    {
        unsigned char tmp_char[64];
        int transferred;
        
        tmp_char[0] = 0x00;
	    r = libusb_interrupt_transfer(dev_handle, (0x01 | LIBUSB_ENDPOINT_OUT), tmp_char, 64, &transferred, 1000);
	    if (r == 0){
		// 解析GeekIMU的磁力计信息
		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();
                        this->d[0] = *(int16_t *)(tmp_char + 3);
                        this->d[1] = *(int16_t *)(tmp_char + 5);
                        this->d[2] = *(int16_t *)(tmp_char + 7);


                    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];
                        float o,p;
                        o = uwbPos.mat[0][0];
                        p = uwbPos.mat[1][0];
                        if(uwbPos.mat[0][0]==0&&uwbPos.mat[1][0]==0){
                            return;
                        }
                        filter.update(o,p);
                        this->fx = o;
                        this->fy = p;
                        std::cout << "uwbPos:" << this->uwb_data_.x_ << " " << this->uwb_data_.y_ << std::endl;
                    }	
                
            }else{
                printf("Read Error\n");
            }
            // outfile << uwb_data_.x_ <<","<< uwb_data_.y_ <<"\n";
              
        }	
            // outfile.close();

            // // 睡眠延时100ms
            // usleep(1000 * 10);

        }
        


};
