#include <ros/ros.h>
#include <mutex>
#include <boost/asio.hpp>
#include <iostream>
#include <string>
#include <cstdint>
#include "type.h"
#include <queue>
#include <chrono>
#include <fstream>
#include <libusb-1.0/libusb.h>
#include "UWBFilter.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();
       

 
    public:
        int pre_seq = -1;
        int cur_seq = -1;
        int AnchorNum = 3;
        //int AnchorPos[3][3]={320, -160, 85,\
         //                      -320,-160, 85,\
         //                       320, 240, 85};
        int AnchorPos[3][3]={560, -80, 85,\
                                 -80,-80, 85,\
                                 560, 320, 85};//基站坐标，序号+三维坐标
        // int AnchorPos[3][3]={-240, 400, 113,\
        //                         240, 400, 113,\
        //                         -400, -240, 113};//基站坐标，序号+三维坐标
        uint8_t tmpdata[13];
        int d[3];
     
    // std::queue<Imu_odom_pose_data> v_buffer_imu_odom_pose_data_;
   
       std::ofstream outfile;
        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;
        int a;
        ssize_t cnt;
        UWBFilter filter;
        float fx,fy;
    
    };

};






#endif
