/* =============================================================================
 * ins.c - 惯导姿态解算封装实现(见 ins.h 说明)
 * 移植自 RoboMaster-C-Board-INS-Example-master 的 ins_task.c:
 *   - 去除 BMI088driver/controller/bsp_PWM/bsp_dwt 依赖,数据源改为本工程
 *     的 bmi088 驱动全局量,温度控制仍由 IMU_Task 负责
 *   - dt 由调用方传入(IMU_Task 固定 1ms),不再使用 DWT 计时
 *   - 保留运动加速度解算(低通 0.0085),供后续功能扩展(如功率/跌倒检测)
 * 欧拉角约定(重要):
 *   QEKF 内部变量命名与航空惯例互换——QEKF_INS.Pitch 是绕 x 轴的角,
 *   QEKF_INS.Roll 是绕 y 轴的角;attitude.c 按"本工程 Pitch 反馈=QEKF.Pitch"
 *   映射(与旧 Mahony 方案的 get_roll 语义一致),勿凭名字想当然
 * ========================================================================== */
#include "ins.h"
#include "QuaternionEKF.h"
#include "bmi088.h"
#include "main.h"
#include <string.h>

/* z 轴静止零偏标定参数:QEKF 无法观测 z 偏置(见 ins.h),上电采 1s */
#define INS_Z_CAL_SAMPLES 1000
static float gyro_z_offset = 0;
static uint8_t z_cal_done = 0;

INS_t INS;

static const float xb[3] = {1, 0, 0};
static const float yb[3] = {0, 1, 0};
static const float zb[3] = {0, 0, 1};

void ins_init(void)
{
    memset(&INS, 0, sizeof(INS));

    /* z 轴静止零偏标定:上电静止采 1s 取平均(x/y 偏置由 EKF 在线估计,
     * 无需校准;此期间 ins_update() 不输出,控制任务由 ins_ready() 门控) */
    float sum = 0;
    for (uint16_t i = 0; i < INS_Z_CAL_SAMPLES; i++)
    {
        sum += bmi088_gyro[2];
        HAL_Delay(1);
    }
    gyro_z_offset = sum / INS_Z_CAL_SAMPLES;
    z_cal_done = 1;

    INS.Initialized = 1;

    /* 与 INS 例程 demo 相同的参数组合:
     * 过程噪声1=10(四元数) 过程噪声2=0.001(零偏) 观测噪声=1e7(加速度)
     * 渐消因子=1(不渐消) 加速度低通=0(内部另有运动加速度低通) */
    IMU_QuaternionEKF_Init(10, 0.001f, 10000000, 1, 0);
}

uint8_t ins_ready(void)
{
    return z_cal_done;
}

void ins_update(float dt)
{
    if (!z_cal_done)
        return;

    /* BMI088 驱动在 SPI DMA 完成回调中后台更新,这里直接取最新值;
     * z 轴减去上电标定的零偏(x/y 由 EKF 在线估计,不再静态校准) */
    float gyro[3] = {bmi088_gyro[0], bmi088_gyro[1],
                     bmi088_gyro[2] - gyro_z_offset};
    float accel[3] = {bmi088_accel[0], bmi088_accel[1], bmi088_accel[2]};

    /* 核心函数,EKF 更新四元数(内部含零偏估计与卡方检验) */
    IMU_QuaternionEKF_Update(gyro[0], gyro[1], gyro[2],
                             accel[0], accel[1], accel[2], dt);

    memcpy(INS.q, QEKF_INS.q, sizeof(INS.q));

    /* 机体系基向量转换到导航系(惯性系=导航系),供扩展功能使用 */
    float xn[3], yn[3], zn[3];
    BodyFrameToEarthFrame(xb, xn, INS.q);
    BodyFrameToEarthFrame(yb, yn, INS.q);
    BodyFrameToEarthFrame(zb, zn, INS.q);

    /* 将重力从导航系转到机体系,计算运动加速度(低通),供扩展使用 */
    const float gravity[3] = {0, 0, 9.81f};
    float gravity_b[3];
    EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
    const float AccelLPF = 0.0085f;
    for (uint8_t i = 0; i < 3; i++)
    {
        INS.MotionAccel_b[i] = (accel[i] - gravity_b[i]) * dt / (AccelLPF + dt) +
                               INS.MotionAccel_b[i] * AccelLPF / (AccelLPF + dt);
    }

    /* 获取最终数据(QEKF 内部已含多圈 Yaw 处理) */
    INS.Yaw = QEKF_INS.Yaw;
    INS.Pitch = QEKF_INS.Pitch;
    INS.Roll = QEKF_INS.Roll;
    INS.YawTotalAngle = QEKF_INS.YawTotalAngle;

    INS.GyroBias[0] = QEKF_INS.GyroBias[0];
    INS.GyroBias[1] = QEKF_INS.GyroBias[1];
    INS.GyroBias[2] = QEKF_INS.GyroBias[2];
    INS.StableFlag = QEKF_INS.StableFlag;
    INS.ConvergeFlag = QEKF_INS.ConvergeFlag;
    INS.ChiSquare = QEKF_INS.ChiSquare_Data[0];
    INS.UpdateCount = QEKF_INS.UpdateCount;

    (void)xn;
    (void)yn;
    (void)zn;
    (void)zb;
}

/**
 * @brief          Transform 3dvector from BodyFrame to EarthFrame
 * @param[1]       vector in BodyFrame
 * @param[2]       vector in EarthFrame
 * @param[3]       quaternion
 */
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
{
    vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
                       (q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
                       (q[1] * q[3] + q[0] * q[2]) * vecBF[2]);

    vecEF[1] = 2.0f * ((q[1] * q[2] + q[0] * q[3]) * vecBF[0] +
                       (0.5f - q[1] * q[1] - q[3] * q[3]) * vecBF[1] +
                       (q[2] * q[3] - q[0] * q[1]) * vecBF[2]);

    vecEF[2] = 2.0f * ((q[1] * q[3] - q[0] * q[2]) * vecBF[0] +
                       (q[2] * q[3] + q[0] * q[1]) * vecBF[1] +
                       (0.5f - q[1] * q[1] - q[2] * q[2]) * vecBF[2]);
}

/**
 * @brief          Transform 3dvector from EarthFrame to BodyFrame
 * @param[1]       vector in EarthFrame
 * @param[2]       vector in BodyFrame
 * @param[3]       quaternion
 */
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
{
    vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
                       (q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
                       (q[1] * q[3] - q[0] * q[2]) * vecEF[2]);

    vecBF[1] = 2.0f * ((q[1] * q[2] - q[0] * q[3]) * vecEF[0] +
                       (0.5f - q[1] * q[1] - q[3] * q[3]) * vecEF[1] +
                       (q[2] * q[3] + q[0] * q[1]) * vecEF[2]);

    vecBF[2] = 2.0f * ((q[1] * q[3] + q[0] * q[2]) * vecEF[0] +
                       (q[2] * q[3] - q[0] * q[1]) * vecEF[1] +
                       (0.5f - q[1] * q[1] - q[2] * q[2]) * vecEF[2]);
}