/* =============================================================================
 * bmi088.c - C 板板载六轴 IMU BMI088 驱动(HAL 版,架构与 legacy 一致)
 * 初始化(bmi088_init,任务上下文):
 *   1. 先关 EXTI4/EXTI9_5 NVIC(数据就绪中断若在初始化期间触发 DMA 读,
 *      会把 SPI 永久占在 BUSY,导致读 ID 全部失败--疯转/绿灯闪的根因)
 *   2. 重配 SPI1 为模式3(CPOL=1,CPHA=2边沿)/16 分频(5.25MHz,
 *      CubeMX 生成的是模式0/2分频 42MHz,BMI088 不支持)
 *   3. 有界重试读 ID(acc=0x1E, gyro=0x0F),成功后按 legacy 序列配置寄存器
 *   4. 清 EXTI 挂起标志后放行中断
 * 运行:  EXTI 下降沿 -> start_dma_read() SPI DMA 读取 -> TxRx 完成回调解析
 *        (加速度 8 字节 -> 链式读温度 4 字节;陀螺 7 字节),量程 ±3g/±2000dps
 * ========================================================================== */
#include "bmi088.h"
#include "main.h"
#include "spi.h"
#include "led.h"

/* ---- 寄存器(与 legacy 一致) ---- */
#define BMI088_ACC_CHIP_ID 0x00
#define BMI088_ACC_PWR_CTRL 0x7D
#define BMI088_ACC_PWR_CONF 0x7C
#define BMI088_ACC_CONF 0x40
#define BMI088_ACC_RANGE 0x41
#define BMI088_INT1_IO_CONF 0x53
#define BMI088_INT1_INT2_MAP_DATA 0x58
#define BMI088_ACC_SOFTRESET 0x7E
#define BMI088_ACC_DATA_START 0x12
#define BMI088_ACC_TEMP_MSB 0x22

#define BMI088_GYRO_CHIP_ID 0x00
#define BMI088_GYRO_RANGE 0x0F
#define BMI088_GYRO_BANDWIDTH 0x10
#define BMI088_GYRO_LPM1 0x11
#define BMI088_GYRO_INT_CTRL 0x15
#define BMI088_INT3_INT4_IO_CONF 0x16
#define BMI088_INT3_INT4_IO_MAP 0x18
#define BMI088_GYRO_SOFTRESET 0x14
#define BMI088_GYRO_DATA_START 0x02

#define BMI088_ACC_ID_VALUE 0x1E
#define BMI088_GYRO_ID_VALUE 0x0F

#define BMI088_CS_ACC(x) HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, (x) ? GPIO_PIN_SET : GPIO_PIN_RESET)
#define BMI088_CS_GYRO(x) HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, (x) ? GPIO_PIN_SET : GPIO_PIN_RESET)

float bmi088_accel[3];
float bmi088_gyro[3];
float bmi088_temperature;
uint8_t bmi088_online = 0;
uint8_t bmi088_last_acc_id = 0;
uint8_t bmi088_last_gyro_id = 0;
volatile uint32_t bmi088_accel_cnt = 0;
volatile uint32_t bmi088_gyro_cnt = 0;

static uint8_t spi_tx_buf[8];
static uint8_t spi_rx_buf[8];

/* 读数阶段(当前 SPI DMA 正在搬运的数据类型) */
typedef enum
{
    BMI088_PHASE_NONE = 0,
    BMI088_PHASE_ACCEL,
    BMI088_PHASE_TEMP,
    BMI088_PHASE_GYRO,
} bmi088_phase_e;

static volatile bmi088_phase_e read_phase = BMI088_PHASE_NONE;
/* 当前读的启动时刻,用于完成回调丢失时的超时兜底(见 EXTI 回调) */
static volatile uint32_t read_phase_tick = 0;

/* 释放当前读对应的片选 */
static void cs_release(bmi088_phase_e phase)
{
    if (phase == BMI088_PHASE_ACCEL || phase == BMI088_PHASE_TEMP)
        BMI088_CS_ACC(1);
    else
        BMI088_CS_GYRO(1);
}

extern SPI_HandleTypeDef hspi1;

/* ---- 阻塞式寄存器读写 ---- */

static void accel_write_reg(uint8_t reg, uint8_t data)
{
    uint8_t tx[2] = {(uint8_t)(reg & 0x7F), data};
    BMI088_CS_ACC(0);
    HAL_SPI_TransmitReceive(&hspi1, tx, spi_rx_buf, 2, 10);
    BMI088_CS_ACC(1);
}

static uint8_t accel_read_reg(uint8_t reg)
{
    uint8_t tx[3] = {(uint8_t)(reg | 0x80), 0x00, 0x00};  // 加速度计首字节回读为乱码
    uint8_t rx[3];
    BMI088_CS_ACC(0);
    HAL_SPI_TransmitReceive(&hspi1, tx, rx, 3, 10);
    BMI088_CS_ACC(1);
    return rx[2];
}

static void gyro_write_reg(uint8_t reg, uint8_t data)
{
    uint8_t tx[2] = {(uint8_t)(reg & 0x7F), data};
    BMI088_CS_GYRO(0);
    HAL_SPI_TransmitReceive(&hspi1, tx, spi_rx_buf, 2, 10);
    BMI088_CS_GYRO(1);
}

static uint8_t gyro_read_reg(uint8_t reg)
{
    uint8_t tx[2] = {(uint8_t)(reg | 0x80), 0x00};
    uint8_t rx[2];
    BMI088_CS_GYRO(0);
    HAL_SPI_TransmitReceive(&hspi1, tx, rx, 2, 10);
    BMI088_CS_GYRO(1);
    return rx[1];
}

/* ---- 数据解析(换算系数与 legacy 一致) ---- */

static void process_accel(void)
{
    int16_t raw[3];
    raw[0] = (int16_t)(((uint16_t)spi_rx_buf[3] << 8) | spi_rx_buf[2]);
    raw[1] = (int16_t)(((uint16_t)spi_rx_buf[5] << 8) | spi_rx_buf[4]);
    raw[2] = (int16_t)(((uint16_t)spi_rx_buf[7] << 8) | spi_rx_buf[6]);
    bmi088_accel[0] = raw[0] * 0.0008974358974f;  // (raw/32768*3)*9.8
    bmi088_accel[1] = raw[1] * 0.0008974358974f;
    bmi088_accel[2] = raw[2] * 0.0008974358974f;
    bmi088_accel_cnt++;
}

static void process_temperature(void)
{
    uint16_t raw = ((uint16_t)spi_rx_buf[2] << 3) | (spi_rx_buf[3] >> 5);  // 11bits
    int16_t temp = (raw > 1023) ? (int16_t)(raw - 2048) : (int16_t)raw;
    bmi088_temperature = temp * 0.125f + 23.0f;
}

static void process_gyro(void)
{
    int16_t raw[3];
    raw[0] = (int16_t)(((uint16_t)spi_rx_buf[2] << 8) | spi_rx_buf[1]);
    raw[1] = (int16_t)(((uint16_t)spi_rx_buf[4] << 8) | spi_rx_buf[3]);
    raw[2] = (int16_t)(((uint16_t)spi_rx_buf[6] << 8) | spi_rx_buf[5]);
    bmi088_gyro[0] = raw[0] * 0.0010652644360316952f;  // (raw/32768*2000)/180*π
    bmi088_gyro[1] = raw[1] * 0.0010652644360316952f;
    bmi088_gyro[2] = raw[2] * 0.0010652644360316952f;
    bmi088_gyro_cnt++;
}

/* 启动一次 SPI DMA 读(若 SPI 忙则丢弃本次,等下一个数据就绪沿) */
static void start_dma_read(bmi088_phase_e phase, uint8_t reg, uint8_t len)
{
    if (HAL_SPI_GetState(&hspi1) != HAL_SPI_STATE_READY)
        return;

    read_phase = phase;
    read_phase_tick = HAL_GetTick();
    spi_tx_buf[0] = (uint8_t)(reg | 0x80);
    for (uint8_t i = 1; i < len; i++)
        spi_tx_buf[i] = 0x00;

    if (phase == BMI088_PHASE_ACCEL || phase == BMI088_PHASE_TEMP)
        BMI088_CS_ACC(0);
    else
        BMI088_CS_GYRO(0);

    /* 启动失败必须回滚:此时 read_phase 已置位、片选已拉低,若直接返回,
     * 完成回调永远不会到来,read_phase 会永久停在非 NONE 状态,之后每个
     * 数据就绪中断都被 if (read_phase == NONE) 挡掉 —— 表现为 IMU 数据
     * 冻结在最后一个样本,而 bmi088_online 仍是 1(2026-09-12 排障) */
    if (HAL_SPI_TransmitReceive_DMA(&hspi1, spi_tx_buf, spi_rx_buf, len) != HAL_OK)
    {
        cs_release(phase);
        read_phase = BMI088_PHASE_NONE;
    }
}

/* ---- 初始化 ---- */

static void error_blink(void)
{
    /* BMI088 连接异常报警:绿灯连闪两下(经 TIM5 PWM,引脚在复用模式下
     * 直接写 GPIO 寄存器无效,必须走 LED 模块) */
    static uint8_t counter = 0;
    counter++;
    if (counter == 1)
        led_set_green(1);
    else if (counter == 5)
        led_set_green(0);
    else if (counter == 13)
        led_set_green(1);
    else if (counter == 17)
        led_set_green(0);
    else if (counter == 40)
        counter = 0;
    HAL_Delay(25);
}

void bmi088_init(void)
{
    /* 关键:数据就绪中断(EXTI)在芯片上电后就会持续触发,若不屏蔽,
     * EXTI 回调里的 SPI DMA 读会把 SPI 永远占在 BUSY,初始化的阻塞式
     * 读 ID 全部失败(legacy 靠初始化期间关闭同款 NVIC 规避,此处一致) */
    HAL_NVIC_DisableIRQ(EXTI4_IRQn);
    HAL_NVIC_DisableIRQ(EXTI9_5_IRQn);

    /* CubeMX 生成的 SPI1 为模式0/2分频(42MHz),BMI088 需要模式3 且 <=10MHz,
     * 此处重新配置为模式3/16分频(5.25MHz,与 legacy 相同) */
    HAL_SPI_DeInit(&hspi1);
    hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
    hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
    hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
    if (HAL_SPI_Init(&hspi1) != HAL_OK)
        return;  // 放弃本次初始化,保持系统可运行

    BMI088_CS_ACC(1);
    BMI088_CS_GYRO(1);

    /* 检测 ID 并配置寄存器(与 legacy 序列一致)
     * 有界重试:最多 40 次(约 1s),失败则放弃并保持系统可运行 */
    for (uint8_t retry = 0; retry < 40; retry++)
    {
        bmi088_last_acc_id = accel_read_reg(BMI088_ACC_CHIP_ID);
        bmi088_last_gyro_id = gyro_read_reg(BMI088_GYRO_CHIP_ID);
        if (bmi088_last_acc_id == BMI088_ACC_ID_VALUE &&
            bmi088_last_gyro_id == BMI088_GYRO_ID_VALUE)
        {
            bmi088_online = 1;
            accel_write_reg(BMI088_ACC_SOFTRESET, 0xB6);
            HAL_Delay(1);  // 加速度计软重启 1ms
            gyro_write_reg(BMI088_GYRO_SOFTRESET, 0xB6);
            HAL_Delay(30);  // 陀螺仪软重启 30ms

            accel_read_reg(BMI088_ACC_CHIP_ID);  // 软重启后首笔读写有 bug,丢弃
            gyro_read_reg(BMI088_GYRO_CHIP_ID);

            accel_write_reg(BMI088_ACC_PWR_CTRL, 0x04);
            HAL_Delay(1);  // 上电等待(legacy 300us,放宽到 1ms)
            accel_write_reg(BMI088_ACC_PWR_CONF, 0x00);
            HAL_Delay(1);
            accel_write_reg(BMI088_ACC_CONF, 0xAB);   // 800Hz ODR,正常低通
            HAL_Delay(1);
            accel_write_reg(BMI088_ACC_RANGE, 0x00);  // ±3g
            HAL_Delay(1);
            accel_write_reg(BMI088_INT1_IO_CONF, 0x08);   // INT1 推挽,低有效
            HAL_Delay(1);
            accel_write_reg(BMI088_INT1_INT2_MAP_DATA, 0x04);  // INT1=数据就绪
            HAL_Delay(1);

            gyro_write_reg(BMI088_GYRO_RANGE, 0x00);      // ±2000°/s
            HAL_Delay(1);
            gyro_write_reg(BMI088_GYRO_BANDWIDTH, 0x82);  // 116Hz 带宽,1kHz ODR
            HAL_Delay(1);
            gyro_write_reg(BMI088_GYRO_LPM1, 0x00);       // 正常模式
            HAL_Delay(1);
            gyro_write_reg(BMI088_GYRO_INT_CTRL, 0x80);   // 使能数据就绪中断
            HAL_Delay(1);
            gyro_write_reg(BMI088_INT3_INT4_IO_CONF, 0x00);  // INT3 推挽,低有效
            HAL_Delay(1);
            gyro_write_reg(BMI088_INT3_INT4_IO_MAP, 0x01);   // INT3=数据就绪
            HAL_Delay(1);
            break;
        }
        error_blink();
    }

    /* 配置成功后清除挂起的数据就绪标志,再放行 EXTI 中断 */
    if (bmi088_online)
    {
        __HAL_GPIO_EXTI_CLEAR_IT(INT1_ACCEL_Pin);
        __HAL_GPIO_EXTI_CLEAR_IT(INT1_GYRO_Pin);
        HAL_NVIC_EnableIRQ(EXTI4_IRQn);
        HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
    }
}

/* ---- 中断回调 ---- */

/* EXTI4(加速度计数据就绪)/ EXTI5(陀螺仪数据就绪) */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    if (!bmi088_online)
        return;  // 初始化完成前不启动 DMA 读,避免占用 SPI

    /* 超时兜底:一次 SPI 读远小于 1ms,若超过 20ms 还没等到完成回调,
     * 说明回调丢了(SPI/DMA 异常),强行复位状态让数据路能自恢复,
     * 否则会永久冻结 —— 与上面 start_dma_read 的失败回滚同一考虑 */
    if (read_phase != BMI088_PHASE_NONE && (HAL_GetTick() - read_phase_tick) > 20U)
    {
        cs_release(read_phase);
        read_phase = BMI088_PHASE_NONE;
    }

    if (GPIO_Pin == INT1_ACCEL_Pin)
    {
        if (read_phase == BMI088_PHASE_NONE)
            start_dma_read(BMI088_PHASE_ACCEL, BMI088_ACC_DATA_START, 8);
    }
    else if (GPIO_Pin == INT1_GYRO_Pin)
    {
        if (read_phase == BMI088_PHASE_NONE)
            start_dma_read(BMI088_PHASE_GYRO, BMI088_GYRO_DATA_START, 7);
    }
}

/* SPI 收发完成 */
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef * hspi)
{
    if (hspi->Instance != SPI1)
        return;

    switch (read_phase)
    {
        case BMI088_PHASE_ACCEL:
            BMI088_CS_ACC(1);
            process_accel();
            read_phase = BMI088_PHASE_NONE;
            start_dma_read(BMI088_PHASE_TEMP, BMI088_ACC_TEMP_MSB, 4);  // 链式读取温度
            break;

        case BMI088_PHASE_TEMP:
            BMI088_CS_ACC(1);
            process_temperature();
            read_phase = BMI088_PHASE_NONE;
            break;

        case BMI088_PHASE_GYRO:
            BMI088_CS_GYRO(1);
            process_gyro();
            read_phase = BMI088_PHASE_NONE;
            break;

        default:
            break;
    }
}