// IMUReader.cpp
#include <iostream>
#include <string>
#include "IMUReader.h"

// 构造函数
IMUReader::IMUReader() {
    isRunning = true;
    // 在构造函数中进行初始化工作
    IMUInit("COM0");
    // 创建一个新线程运行数据读取循环
    dataThread = std::thread(&IMUReader::IMUReadDataThread, this);
}

// 构造函数
IMUReader::IMUReader(std::string ComNumber) {
    isRunning = true;
    // 在构造函数中进行初始化工作
    IMUInit(ComNumber);
    // 创建一个新线程运行数据读取循环
    dataThread = std::thread(&IMUReader::IMUReadDataThread, this);
}

// 析构函数
IMUReader::~IMUReader() {
    // 在析构函数中通过改变 isExist 变量为 false 退出循环
    isRunning = false;
    // 等待数据读取线程结束
    if (dataThread.joinable()) {
        dataThread.join();
    }
    // realease serial object.
    if(serial != NULL){
        delete serial;
    }
}

// 获取 IMU 数据状态
int IMUReader::IMUReadData() 
{
    uint8_t tmpdata[24];

    // 读取串口数据
    size_t bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 

    // check frame head
    if(tmpdata[0] != 0xfc){
        return 0;
    }
    // check frame type
    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
    if(tmpdata[0] != 0x40){
        return 0;
    }

    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
    // check imu data len
    if(tmpdata[0] != 0x38){
        return 0;
    }

    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 1)); 
    bytesRead = boost::asio::read(*serial, boost::asio::buffer(tmpdata, 24)); 
    
   
    // data.vGyro.x = *(float *)(tmpdata+0);
    // data.vGyro.y = *(float *)(tmpdata+4);
    // data.vGyro.z  = *(float *)(tmpdata+8);
    // data.vAcc.x = *(float *)(tmpdata+12);
    // data.vAcc.y = *(float *)(tmpdata+16);
    // data.vAcc.z = *(float *)(tmpdata+20);
    // data.timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now().time_since_epoch() ).count();

    IMUData newData;
    newData.vGyro.x = *(float *)(tmpdata+0);
    newData.vGyro.y = *(float *)(tmpdata+4);
    newData.vGyro.z  = *(float *)(tmpdata+8);
    newData.vAcc.x = *(float *)(tmpdata+12);
    newData.vAcc.y = *(float *)(tmpdata+16);
    newData.vAcc.z = *(float *)(tmpdata+20);
    newData.timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(
        std::chrono::system_clock::now().time_since_epoch()).count();
    
    std::lock_guard<std::mutex> lock(dataMutex);
    imubuffer.push_back(newData);
   
   
    

    // printf("gyro = %f,%f,%f, Acc = %f,%f,%f\n", data.gyroX,data.gyroY,data.gyroZ, 
    //                                             data.accelX, data.accelY, data.accelZ);
    // 返回状态，这里简单地返回 0 表示成功
    return 1;
}



// 获取 IMU 数据状态
int IMUReader::ReadData(IMUData& data) {
 
    data.vAcc.x= this->data.vAcc.x;
    data.vAcc.y = this->data.vAcc.y;
    data.vAcc.z = this->data.vAcc.z;
    data.vGyro.x= this->data.vGyro.x;
    data.vGyro.y = this->data.vGyro.y;
    data.vGyro.z = this->data.vGyro.z;
    data.timestamp = this->data.timestamp;

    

  
    return 0;
}

// 初始化 IMU
void IMUReader::IMUInit(std::string ComNumber) {
    // 这里可以进行具体的初始化工作
    std::cout << "IMU Initialization..." << ComNumber << std::endl;
    serial = new boost::asio::serial_port(io, ComNumber);

    if(serial == NULL){
        return;
    }
    serial->set_option(boost::asio::serial_port_base::baud_rate(921600)); // 设置波特率
    serial->set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位
}

// 读取 IMU 数据的循环
void IMUReader::IMUReadDataThread() {
    while (isRunning) {
        int status = IMUReadData();
        // 处理获取的数据，这里简单地
        // Add delay fun.
    }
}