#include <ros/ros.h>
#include <mutex>
#include <boost/asio.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <cstdint>
#include "type.h"
#include <queue>
#include <chrono>
#include <libusb-1.0/libusb.h>
#ifndef UWB_H
#define UWB_H

#define PI acos(-1)
namespace uwb_slam{

    class Uwb
    {
    public:
        Uwb();
        ~Uwb();
        void Run();
        bool checknewdata();
        void feed_imu_odom_pose_data();
        void Serread();
        void usb_hid_read();
        
        void readConfigFile(const std::string& filename) {
        std::ifstream file(filename);
        if (!file.is_open()) {
            std::cerr << "无法打开配置文件: " << filename << std::endl;
            return;
        }
        
        std::string line;
        int row = 0;
        while (std::getline(file, line) && row < AnchorNum) {
            std::istringstream iss(line);
            int x, y, z;
            char delimiter;
            if (iss >> x >> delimiter>> y >> delimiter>>z) {
                AnchorPos[row][0] = x;
                AnchorPos[row][1] = y;
                AnchorPos[row][2] = z;
                row++;
            } else {
                std::cerr << "解析配置文件时出错: " << line << std::endl;
            }
        }

        file.close();
    }
       

 
    public:
        int pre_seq = -1;
        int cur_seq = -1;
        int AnchorNum = 3;
        float AnchorPos[3][3];
        //int AnchorPos[3][3]={320, -295, 50,\
        //                        320,300, 50,\
        //                        -200, 300, 50};
        // int AnchorPos[3][3]={320, -160, 85,\
        //                          -320,-160, 85,\
        //                          320, 240, 85};//基站坐标，序号+三维坐标

        uint8_t tmpdata[13];
        int d[3];
     
    // std::queue<Imu_odom_pose_data> v_buffer_imu_odom_pose_data_;
   
       
        Uwb_data uwb_data_;
        // ros_merge_test::RawImu sub_imu_;
        // std::queue<Imu_odom_pose_data > imu_odom_queue_;
        // std::queue<Uwb_data> uwb_data_queue_;
        std::mutex mMutexUwb;
        libusb_device_handle *dev_handle;
        libusb_device **devs;
        libusb_context *ctx = NULL;
        int r;
        ssize_t cnt;
    
    };

};




#endif
