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


#define CARHEIGHT 35

namespace uwb_slam{

    Uwb::Uwb(){

        // 初始化 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->usb_hid_read();
        }


    }
    void Uwb::usb_hid_read()
    {
        unsigned char tmp_char[64];
        int transferred;
        tmp_char[0] = 0x01;
	    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;
            }
            else
            {
                std::cout<<r<<std::endl;
                ROS_ERROR("Read error");
            }
            ros::spinOnce();
        }
    }


};

