#ifndef __IMU_H
#define __IMU_H

#include "ICM_42688.h"

//==================================================================================================
// IMU 姿态解算层
// 负责：传感器数据滤波 → Mahony AHRS 姿态解算 → 欧拉角输出
// 所有数据基于传感器坐标系（无机体坐标转换）
//==================================================================================================

#define ANG2RAD 0.01745329252f   // 角度制 → 弧度制
#define RAD2ANG 57.295779513f    // 弧度制 → 角度制

//==================================================================================================
// 双二阶滤波器 (Biquad Filter)
//==================================================================================================

/** @brief 滤波器类型 */
typedef enum {
    BIQUAD_LOWPASS,         // 低通
    BIQUAD_HIGHPASS,        // 高通
    BIQUAD_BANDPASS_PEAK,   // 带通（峰值）
    BIQUAD_BANDSTOP_NOTCH,  // 带阻（陷波）
} biquad_type_t;

/** @brief 滤波器状态（系数 + 历史值）
 *  典型 Q 值: 0.5773 (Thomson-Bessel, 相位线性度好)
 *             0.7071 (Butterworth, 最平坦)
 *             0.9565 (Chebyshev 1dB, 阻带衰减快)
 */
typedef struct {
    float a0, a1, a2, a3, a4;   // 传递函数系数
    float x1, x2, y1, y2;       // 历史输入/输出
} biquad_state_t;

//==================================================================================================
// 姿态输出
//==================================================================================================

/** @brief 姿态数据（传感器坐标系，volatile 因为 ISR 写入、主循环读取） */
typedef struct {
    float pitch, roll, yaw;             // 欧拉角 (°)
    float gyro_x, gyro_y, gyro_z;       // 滤波后角速度 (°/s)
} imu_attitude_t;

extern volatile imu_attitude_t imu_angle;   // 当前姿态
extern volatile int yaw_round_count;

//==================================================================================================
// 用户接口
//==================================================================================================

/** @brief 初始化 IMU（传感器 + 滤波器 + Mahony）
 *  @param sample_freq_hz  采样频率（Hz），需与定时器中断频率一致，典型值 200
 *  用户只需调用此函数，内部自动调用 icm42688_init()
 */
void imu_init(int sample_freq_hz);

/** @brief 更新姿态（在定时器 ISR 中以固定频率调用）
 *  读取传感器 → 零偏修正 → 双二阶滤波 → Mahony 解算 → 写入 imu_angle
 *  执行时间约 100~150us（80MHz Cortex-M0+ 估算），需保证在中断周期内完成
 */
void imu_update(void);

/** @brief 陀螺仪零偏校准（静止状态下调用）
 *  采集 3 秒静止数据，计算各轴零偏，自动应用于后续 imu_update()
 *  调用时设备必须完全静止
 */
void imu_calibrate_gyro(void);

//==================================================================================================
// 算法接口（也可被外部直接调用）
//==================================================================================================

/** @brief 双二阶滤波器初始化
 *  @param state 滤波器状态指针
 *  @param type  滤波器类型
 *  @param fs    采样频率 (Hz)
 *  @param fc    截止频率 (Hz)
 *  @param q     品质因数
 */
void  biquad_init(biquad_state_t *state, biquad_type_t type, int fs, float fc, float q);

/** @brief 双二阶滤波器单步计算
 *  @param state 滤波器状态指针
 *  @param data  当前输入值
 *  @return 滤波后输出值
 */
float biquad_apply(biquad_state_t *state, float data);

/** @brief Mahony AHRS 姿态解算（6轴 IMU 模式）
 *  @param gx/gy/gz 陀螺仪角速度 (°/s, 传感器坐标系)
 *  @param ax/ay/az 加速度计测量值 (g, 传感器坐标系)
 *  输出写入全局变量 imu_angle.pitch/roll/yaw
 */
void  mahony_ahrs_update(float gx, float gy, float gz, float ax, float ay, float az);

/** @brief 快速平方根倒数（Quake III 算法）
 *  @param x 输入值
 *  @return 1/sqrt(x) 的近似值
 */
float inv_sqrt(float x);

#endif