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


#define CARHEIGHT 20
#define DISINDEX 3
#define AOAINDEX 15
#define DATALENGTH 27

namespace uwb_slam{
    Uwb::Uwb(){
    
    }

    void Uwb::Run() {
        
        while(1){
        this->Serread();

        }


    }


    void Uwb::Serread(){
        try {
        boost::asio::io_service io;
        boost::asio::serial_port serial(io, "/dev/ttyUSB1"); // 替换成你的串口设备路径

        serial.set_option(boost::asio::serial_port_base::baud_rate(115200)); // 设置波特率
        serial.set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位
        serial.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); // 设置校验位
        serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位

        uint8_t tmpdata[DATALENGTH];
        // std::cerr << "befor read" << std::endl;
        size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, sizeof(tmpdata))); // 读取串口数据
        // std::cerr << "after read" << std::endl;
        this->uwb_data_.uwb_t_ = ros::Time::now();
        
        for (int i=0;i< bytesRead;i++)
        {
            std::cout << std::hex<<static_cast<int>(tmpdata[i]) << " ";
        }
        std::cout << std::endl;

        memcpy(&this->d[0], &tmpdata[DISINDEX], sizeof(d[0]));
        memcpy(&this->d[1], &tmpdata[DISINDEX + sizeof(d[0])], sizeof(d[0]));
        memcpy(&this->d[2], &tmpdata[DISINDEX + sizeof(d[0]) * 2], sizeof(d[0]));
        memcpy(&this->aoa[0], &tmpdata[AOAINDEX], sizeof(aoa[0]));
        memcpy(&this->aoa[1], &tmpdata[AOAINDEX + sizeof(aoa[0])], sizeof(aoa[0]));
        memcpy(&this->aoa[2], &tmpdata[AOAINDEX + sizeof(aoa[0]) * 2], sizeof(aoa[0]));

        //std::cout << "d:" << d[0] << " " << d[1] << " " << d[2] << std::endl;

        // if(abs(d[0]) > 2000 || abs(d[1]) > 2000 || abs(d[2]) > 2000) {
        //     return;
        // }
        // for(int i=0; i<3; i++)
        // {
        //     this->d[i] = sqrt(this->d[i] * this->d[i] - (AnchorPos[i][2] - CARHEIGHT) * (AnchorPos[i][2] - CARHEIGHT));
        // }
        
        std::cout<<"d: "<<this->d[0]<<" aoa: "<<this->aoa[0]<<std::endl;
        d[0] = ((((-1.4229e-07 * d[0]) + 1.8784e-04) * d[0]) + 0.9112) * d[0] + 2.4464;
        // d[1] = ((((-2.3004e-07 * d[1]) + 3.2942e-04) * d[1]) + 0.8385) * d[1] + 6.2770;
        // d[2] = ((((-2.0616e-07 * d[2]) + 3.3886e-04) * d[2]) + 0.8231) * d[2] + 8.1566;   
    } catch (const std::exception& ex) {
        std::cerr << "Exception: " << ex.what() << std::endl;
    }



    }
    
 
    void fusion()
    {

    }



    


};

// bool Uwb::checknewdata()
// {
//     std::unique_lock<std::mutex> lock(mMutexUwb);
//     return !v_buffer_imu_odom_pose_data_.empty();
// }

// void Uwb::Run() {
//     while(1)
//     {
//         if(checknewdata())
//         {
//             {
//                 std::unique_lock<std::mutex> lock(mMutexUwb);
//                 Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front();
//                 v_buffer_imu_odom_pose_data_.pop();
//             }



//         }
//     }



// }

// void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){
//     std::unique_lock<std::mutex> lock(mMutexUwb);
//     v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data);
// }

