// 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::IMUReadUartData() 
{
    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)); 

    // 从 tmpdata 中提取数据
    data.gyroX   = *(float *)(tmpdata+0);
    data.gyroY   = *(float *)(tmpdata+4);
    data.gyroZ   = *(float *)(tmpdata+8);
    data.accelX  = *(float *)(tmpdata+12);
    data.accelY  = *(float *)(tmpdata+16);
    data.accelZ  = *(float *)(tmpdata+20);

    // 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.accelX = this->data.accelX;
    data.accelY = this->data.accelY;
    data.accelZ = this->data.accelZ;
    data.gyroX = this->data.gyroX;
    data.gyroY = this->data.gyroY;
    data.gyroZ = this->data.gyroZ;
    // 返回状态，这里简单地返回 0 表示成功
    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 = IMUReadUartData();
        // 处理获取的数据，这里简单地
        // Add delay fun.
    }
}

float IMUReader::getRandomFloat(){
    return 1.22f;
}