#include "upbot_vision/ultrasonic.h"

UltrasonicNode::UltrasonicNode(){
    r = libusb_init(&ctx); // 初始化libusb
    if (r < 0)
    {
        ROS_ERROR("Failed to initialize libusb");
        return ;
    }

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

    dev_handle = libusb_open_device_with_vid_pid(ctx, 0x048d, 0x5750); // 使用指定的vendor和product ID打开设备
    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");
    ultrasonic_pub = nh.advertise<pibot_msgs::avoid>("ultrasonic_data", 10);
}
UltrasonicNode::~UltrasonicNode(){
    r = libusb_release_interface(dev_handle, 0); // 释放接口
    if (r != 0)
        ROS_ERROR("Cannot release interface");
    libusb_close(dev_handle); // 关闭设备
    libusb_exit(ctx); 

}
void UltrasonicNode::readUltrasonic() {
    unsigned char data[64];
    int transferred;

    while (ros::ok())
    {
        r = libusb_interrupt_transfer(dev_handle, (0x01 | LIBUSB_ENDPOINT_IN), data, 64, &transferred, 1000); // 执行批量传输
        // std::cout<<r<<std::endl;
        // if (r == 0 && transferred == 64)
        pibot_msgs::avoid msg;
        if (r == 0 )
        {
            range = *(float *)(data);
            // uint8_t bool_data = *(uint8_t *)(data + sizeof(float));
            // bool l1 = bool_data & (1 << 0);
            // bool l2 = bool_data & (1 << 1);
            // bool r1 = bool_data & (1 << 2);
            // bool r2 = bool_data & (1 << 3);

            uint8_t r1 = *(uint8_t*)(data + 4);
            uint8_t r2 = *(uint8_t*)(data + 5);
            uint8_t l1 = *(uint8_t*)(data + 6);
            uint8_t l2 = *(uint8_t*)(data + 7);
            msg.ultrasonic_range = range;
            msg.l1 = l1;
            msg.l2 = l2;
            msg.r2 = r1;
            msg.r1 = r2;
            ultrasonic_pub.publish(msg);
            ROS_INFO("Received ultrasonic data: %f mm, l1: %d, l2: %d, r1: %d, r2: %d", msg.ultrasonic_range, msg.l1, msg.l2, msg.r1, msg.r2);
            // ROS_INFO("Magnetic field: %f",ultrasonic_range);
        }
        else
        {
            ROS_ERROR("Read error");
        }

    }
}
int main(int argc, char** argv) {
    ros::init(argc, argv, "ultrasonic_node");
    UltrasonicNode ultrasonic_node;
    ros::Rate rate(10); // 10 Hz

    while (ros::ok()) {
        ultrasonic_node.readUltrasonic();
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}
