#include "ICM_42688.h"

//==================================================================================================
// 内部变量
//==================================================================================================

static float accel_scale = 1.0f;    // 加速度计 LSB → g
static float gyro_scale  = 1.0f;    // 陀螺仪 LSB → °/s

//==================================================================================================
// SPI 底层操作宏
//==================================================================================================

#define ICM42688_WRITE_REG(reg, data)       spi_write_8bit_register(ICM42688_SPI, reg, data)
#define ICM42688_READ_REGS(reg, data, num)  spi_read_8bit_registers(ICM42688_SPI, (reg) | 0x80, data, num)

//==================================================================================================
// 静态函数 — 寄存器级 SPI 操作
//==================================================================================================

/** @brief 向指定寄存器写入一字节
 *  @param reg  寄存器地址
 *  @param data 写入值
 *  片选 → 写寄存器 → 释放片选
 */
static void icm42688_write_reg(uint8_t reg, uint8_t data)
{
    ICM42688_CS_LEVEL(0);
    ICM42688_WRITE_REG(reg, data);
    ICM42688_CS_LEVEL(1);
}

/** @brief 从指定寄存器连续读取多字节
 *  @param reg  起始寄存器地址
 *  @param data 输出缓冲区指针
 *  @param num  读取字节数
 *  片选 → 读多字节 → 释放片选
 */
static void icm42688_read_regs(uint8_t reg, uint8_t *data, uint8_t num)
{
    ICM42688_CS_LEVEL(0);
    ICM42688_READ_REGS(reg, data, num);
    ICM42688_CS_LEVEL(1);
}

/** @brief 将两个 uint8 拼接为 int16（大端序）
 *  @param hi 高字节
 *  @param lo 低字节
 *  @return 有符号 16 位整数
 */
static int16_t icm42688_combine_bytes(uint8_t hi, uint8_t lo)
{
    return (int16_t)(((uint16_t)hi << 8) | lo);
}

/** @brief 配置加速度计/陀螺仪的量程和输出速率
 *  @param afs  加速度计量程
 *  @param aodr 加速度计输出速率
 *  @param gfs  陀螺仪量程
 *  @param godr 陀螺仪输出速率
 *  同时更新内部比例系数 accel_scale / gyro_scale
 */
static void icm42688_set_range_odr(enum icm42688_afs afs, enum icm42688_aodr aodr,
                                    enum icm42688_gfs gfs, enum icm42688_godr godr)
{
    icm42688_write_reg(ICM42688_ACCEL_CONFIG0, (afs << 5) | (aodr + 1));
    icm42688_write_reg(ICM42688_GYRO_CONFIG0,  (gfs << 5) | (godr + 1));

    switch (afs) {
    case ICM42688_AFS_2G:   accel_scale = 2000.0f  / 32768.0f; break;
    case ICM42688_AFS_4G:   accel_scale = 4000.0f  / 32768.0f; break;
    case ICM42688_AFS_8G:   accel_scale = 8000.0f  / 32768.0f; break;
    case ICM42688_AFS_16G:  accel_scale = 16000.0f / 32768.0f; break;
    default:                accel_scale = 1.0f;                 break;
    }

    switch (gfs) {
    case ICM42688_GFS_15_625DPS: gyro_scale = 15.625f  / 32768.0f; break;
    case ICM42688_GFS_31_25DPS:  gyro_scale = 31.25f   / 32768.0f; break;
    case ICM42688_GFS_62_5DPS:   gyro_scale = 62.5f    / 32768.0f; break;
    case ICM42688_GFS_125DPS:    gyro_scale = 125.0f   / 32768.0f; break;
    case ICM42688_GFS_250DPS:    gyro_scale = 250.0f   / 32768.0f; break;
    case ICM42688_GFS_500DPS:    gyro_scale = 500.0f   / 32768.0f; break;
    case ICM42688_GFS_1000DPS:   gyro_scale = 1000.0f  / 32768.0f; break;
    case ICM42688_GFS_2000DPS:   gyro_scale = 2000.0f  / 32768.0f; break;
    default:                     gyro_scale = 1.0f;                break;
    }
}

//==================================================================================================
// 公开接口
//==================================================================================================

/** @brief 返回当前陀螺仪比例系数 (LSB → °/s)
 *  供 imu_calibrate_gyro() 将原始 ADC 均值转换为物理偏移量
 */
float icm42688_gyro_scale(void)
{
    return gyro_scale;
}

/** @brief 初始化 ICM-42688
 *  1. 初始化 SPI 外设和 CS 引脚
 *  2. 读取 WHO_AM_I 进行自检（超时 500ms 后报错死循环）
 *  3. 复位并配置量程/速率（默认 ±16G / ±2000dps / 200Hz ODR）
 *  4. 设置为低噪声模式
 *  由 imu_init() 内部调用，用户一般不直接调用
 */
void icm42688_init(void)
{
    system_delay_ms(10);
    spi_init(ICM42688_SPI, SPI_MODE0, ICM42688_SPI_SPEED,
             ICM42688_SPC_PIN, ICM42688_SDI_PIN, ICM42688_SDO_PIN, SPI_CS_NULL);
    gpio_init(ICM42688_CS_PIN, GPO, GPIO_HIGH, GPO_PUSH_PULL);

    uint8_t model = 0xFF;
    int timeout = 50;
    while (1) {
        icm42688_read_regs(ICM42688_WHO_AM_I, &model, 1);
        if (model == 0x47) {
            break;
        }
        ICM42688_DELAY_MS(10);
        if (--timeout < 0) {
            zf_log(0, "ICM42688 Init Error!");
            while (1);
        }
    }

    icm42688_write_reg(ICM42688_PWR_MGMT0, 0x00);
    ICM42688_DELAY_MS(20);

    // 如果修改了采样频率（定时器中断周期），则需要对应修改下方的配置
    icm42688_set_range_odr(ICM42688_AFS_16G, ICM42688_AODR_200HZ,
                           ICM42688_GFS_2000DPS, ICM42688_GODR_200HZ);

    icm42688_write_reg(ICM42688_PWR_MGMT0, 0x0F);
    ICM42688_DELAY_MS(10);
}

/** @brief 读取全部传感器数据（加速度 + 角速度）
 *  @param data 输出结构体指针
 *  一次 12 字节 SPI 突发从 ACCEL_DATA_X1 开始连续读取
 *  读取顺序: AX→AY→AZ→GX→GY→GZ（与传感器内部 FIFO 顺序一致）
 */
void icm42688_read_all(icm42688_data_t *data)
{
    uint8_t buf[12];
    icm42688_read_regs(ICM42688_ACCEL_DATA_X1, buf, 12);

    data->accel_x = accel_scale * icm42688_combine_bytes(buf[0], buf[1]);
    data->accel_y = accel_scale * icm42688_combine_bytes(buf[2], buf[3]);
    data->accel_z = accel_scale * icm42688_combine_bytes(buf[4], buf[5]);

    data->gyro_x = gyro_scale * icm42688_combine_bytes(buf[6],  buf[7]);
    data->gyro_y = gyro_scale * icm42688_combine_bytes(buf[8],  buf[9]);
    data->gyro_z = gyro_scale * icm42688_combine_bytes(buf[10], buf[11]);
}

/** @brief 读取陀螺仪原始 ADC 值（不做比例换算）
 *  @param gx/gy/gz 输出 int16 原始值指针
 *  供 imu_calibrate_gyro() 在静止状态下采集原始数据计算零偏
 */
void icm42688_read_gyro_raw(int16_t *gx, int16_t *gy, int16_t *gz)
{
    uint8_t buf[6];
    icm42688_read_regs(ICM42688_GYRO_DATA_X1, buf, 6);

    *gx = icm42688_combine_bytes(buf[0], buf[1]);
    *gy = icm42688_combine_bytes(buf[2], buf[3]);
    *gz = icm42688_combine_bytes(buf[4], buf[5]);
}