#include <ros/ros.h>
#include <libusb-1.0/libusb.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "usb_hid_node");
    ros::NodeHandle nh;

    libusb_device_handle *dev_handle;
    libusb_device **devs;
    libusb_context *ctx = NULL;
    int r;
    ssize_t cnt;

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

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

    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 1;
    }

    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 1;
    }

    ROS_INFO("Interface claimed");

    ros::Rate loop_rate(2); // 设置循环频率

    unsigned char data[64];
    int transferred;
    int actual;

    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)
        if (r == 0 )
        {
            ROS_INFO("Read successful");
            // float ultrasonic_range = *(float *)(data);
            float dist = *(float *)(data);
			int16_t my = *(int16_t *)(data + 5);
			int16_t mz = *(int16_t *)(data + 7);

			ROS_INFO("data:dist =%2f %d %d %d %d\n", dist,data[0],data[1],data[2],data[3]);
            // ROS_INFO("Magnetic field: %f",ultrasonic_range);
        }
        else
        {
            ROS_ERROR("Read error");
        }

        // ros::spinOnce();
        // loop_rate.sleep();
    }

    r = libusb_release_interface(dev_handle, 0); // 释放接口
    if (r != 0)
        ROS_ERROR("Cannot release interface");

    libusb_close(dev_handle); // 关闭设备
    libusb_exit(ctx);          // 退出libusb

    return 0;
}
