#include "../2025_include_user/Imu.h"

uint8_t Imu::_buffer = 0;
Imu::Data Imu::_data = {};
static uint8_t frame_index = 0;
static uint8_t frame_buffer[11];

extern "C" void IMU_INST_IRQHandler(void) {
    switch (DL_UART_getPendingInterrupt(IMU_INST)) {
        case DL_UART_IIDX_RX:
            Imu::_buffer = DL_UART_Main_receiveData(IMU_INST);
            Imu::inject();
            break;
        default:
            break;
    }

    NVIC_ClearPendingIRQ(IMU_INST_INT_IRQN);
    DL_UART_enableInterrupt(IMU_INST, DL_UART_INTERRUPT_RX); // 启用接收中断
}

void Imu::init() {
    NVIC_ClearPendingIRQ(IMU_INST_INT_IRQN);
    NVIC_EnableIRQ(IMU_INST_INT_IRQN);
    DL_UART_enable(IMU_INST); // 启用 UART 模块
    DL_UART_enableInterrupt(IMU_INST, DL_UART_INTERRUPT_RX); // 启用接收中断

}

void Imu::inject() {
    if (frame_index == 0 && _buffer == 0x55) {
        frame_buffer[frame_index++] = _buffer;
    } else if (frame_index == 1 && _buffer == 0x55) {
        frame_buffer[frame_index++] = _buffer;
    } else if (frame_index >= 2) {
        frame_buffer[frame_index++] = _buffer;
        if (frame_index >= 11) {
            uint8_t sum = 0;
            for (int i = 0; i < 10; i++) {
                sum += frame_buffer[i];
            }

            if (sum == frame_buffer[10] && frame_buffer[2] == 0x01 && frame_buffer[3] == 0x06) {
                int16_t roll = (frame_buffer[5] << 8) | frame_buffer[4];
                int16_t pitch = (frame_buffer[7] << 8) | frame_buffer[6];
                int16_t yaw = (frame_buffer[9] << 8) | frame_buffer[8];

                _data.roll = (float) roll / 32768.0f * 180.0f;
                _data.pitch = (float) pitch / 32768.0f * 180.0f;
                _data.yaw = (float) yaw / 32768.0f * 180.0f;
            }
            frame_index = 0;
        }
    } else {
        frame_index = 0;
    }
}

Imu::Data Imu::getData() {
    return _data;
}

float Imu::getRoll() {
    return _data.roll;
}

float Imu::getPitch() {
    return _data.pitch;
}

float Imu::getYaw() {
    return _data.yaw;
}