/*********************************************************************************************************************
* TC264 智能车 - 姿态解算模块 (attitude_solution.c)
*
* 基于 Mahony 互补滤波算法：
*   陀螺仪积分得到姿态角，加速度计修正陀螺零漂。
*   使用四元数表示姿态，避免万向节死锁。
*   移植自 RT1064 开源库中 ICM_AHRSupdate 实现。
*
* 调用周期: 5ms (在 isr.c 的 CCU60_CH0 中断中调用, 陀螺仪校准完成后)
*
* IMU660RB 转换因子 (2000DPS / 8G):
*   陀螺仪: raw / 14.3 = °/s,  * π/180 = rad/s
*   加速度: raw / 4098 = G (m/s^2单位)
*********************************************************************************************************************/

#include "zf_common_typedef.h"
#include "zf_driver_delay.h"
#include "zf_device_imu660rb.h"
#include "attitude_solution.h"

/* ---------------------------------------------------------------------------
 * 常数与可调参数 (匹配 RT1064 参考实现)
 * --------------------------------------------------------------------------- */
#define DELTA_T         0.005f          /* 姿态更新周期 (秒), 匹配 5ms ISR      */
#define DEG_TO_RAD      0.01745329252f  /* π / 180                                */
#define RAD_TO_DEG      57.2957795131f  /* 180 / π                                */

#define KP              0.17f           /* 加速度计收敛比例增益 (同 RT1064)       */
#define KI              0.004f          /* 陀螺仪零漂积分增益  (同 RT1064)       */
#define ACC_ALPHA       0.3f            /* 加速度低通滤波系数 (同 RT1064)         */

/* ---------------------------------------------------------------------------
 * 全局姿态角 (外部引用)
 * --------------------------------------------------------------------------- */
euler_param_t eulerAngle;

/* ---------------------------------------------------------------------------
 * 静态变量 — 四元数 & 误差积分 & 加速度滤波状态
 * --------------------------------------------------------------------------- */
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
static float i_ex = 0.0f, i_ey = 0.0f, i_ez = 0.0f;
static float acc_fx = 0.0f, acc_fy = 0.0f, acc_fz = 0.0f;

/* ---------------------------------------------------------------------------
 * 快速反平方根 (Quake III 算法, 两次牛顿迭代)
 * --------------------------------------------------------------------------- */
static float fast_rsqrt(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long *)&y;
    i = 0x5f3759df - (i >> 1);
    y = *(float *)&i;
    y = y * (1.5f - halfx * y * y);
    y = y * (1.5f - halfx * y * y);
    return y;
}

/* ---------------------------------------------------------------------------
 * 姿态解算初始化
 * 多次读取加速度计取平均, 用加速度计计算初始 pitch/roll, 设置正确的初始四元数
 * --------------------------------------------------------------------------- */
void attitude_init(void)
{
    float ax_sum, ay_sum, az_sum;
    float ax, ay, az;
    float norm, mag;
    float roll_init, pitch_init;
    float cr, sr, cp, sp;
    int i;

    /* 传感器上电后需要时间稳定, 丢弃前若干次读数 */
    for(i = 0; i < 10; i++)
    {
        imu660rb_get_acc();
        system_delay_ms(5);
    }

    /* 多次采样取平均, 提高初始姿态估计的稳健性 */
    ax_sum = 0.0f; ay_sum = 0.0f; az_sum = 0.0f;
    for(i = 0; i < 20; i++)
    {
        imu660rb_get_acc();
        ax_sum += imu660rb_acc_transition(imu660rb_acc_x);
        ay_sum += imu660rb_acc_transition(imu660rb_acc_y);
        az_sum += imu660rb_acc_transition(imu660rb_acc_z);
        system_delay_ms(5);
    }
    ax = ax_sum / 20.0f;
    ay = ay_sum / 20.0f;
    az = az_sum / 20.0f;

    /* 校验加速度幅值是否在合理范围 (0.6G ~ 1.6G), 若异常则回退到默认值 */
    mag = ax * ax + ay * ay + az * az;
    if(mag < 0.36f || mag > 2.56f)
    {
        ax = 0.0f;
        ay = 0.0f;
        az = 1.0f;
    }
    else
    {
        norm = fast_rsqrt(mag);
        ax *= norm;
        ay *= norm;
        az *= norm;
    }

    /* 从重力矢量估计初始 roll 和 pitch */
    roll_init  = atan2f(ay, az);
    pitch_init = atan2f(-ax, sqrtf(ay * ay + az * az));

    /* 欧拉角 -> 四元数 */
    cr = cosf(roll_init * 0.5f);
    sr = sinf(roll_init * 0.5f);
    cp = cosf(pitch_init * 0.5f);
    sp = sinf(pitch_init * 0.5f);

    q0 = cr * cp;
    q1 = sr * cp;
    q2 = cr * sp;
    q3 = -sr * sp;

    /* 四元数归一化 */
    norm = fast_rsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= norm;
    q1 *= norm;
    q2 *= norm;
    q3 *= norm;

    /* 复位积分项 */
    i_ex = 0.0f; i_ey = 0.0f; i_ez = 0.0f;

    /* 加速度滤波状态初始化为首次有效读数 (避免从零开始的衰减过程) */
    acc_fx = ax;
    acc_fy = ay;
    acc_fz = az;

    /* 根据初始四元数计算初始欧拉角 */
    {
        float q0q0 = q0 * q0;
        float q0q1 = q0 * q1;
        float q0q2 = q0 * q2;
        float q0q3 = q0 * q3;
        float q1q1 = q1 * q1;
        float q1q2 = q1 * q2;
        float q1q3 = q1 * q3;
        float q2q2 = q2 * q2;
        float q2q3 = q2 * q3;
        float q3q3 = q3 * q3;

        eulerAngle.pitch = asinf(-2.0f * q1q3 + 2.0f * q0q2) * RAD_TO_DEG;
        eulerAngle.roll  = atan2f(2.0f * q2q3 + 2.0f * q0q1,
                                  -2.0f * q1q1 - 2.0f * q2q2 + 1.0f) * RAD_TO_DEG;
        eulerAngle.yaw   = 0.0f; /* 无磁力计，yaw 从 0 开始 */
    }
}

/* ---------------------------------------------------------------------------
 * 姿态更新 — 每 5ms 调用一次 (陀螺仪校准完成后)
 * 读取已去除零偏的陀螺仪数据 + 原始加速度计数据,
 * 运行 Mahony 互补滤波, 输出欧拉角
 * --------------------------------------------------------------------------- */
void attitude_update(void)
{
    float gx, gy, gz;
    float ax, ay, az;
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;
    float halfT = 0.5f * DELTA_T;

    /* --- 1. 获取传感器数据并转换为物理量 --- */
    /* 陀螺仪: ISR 已减零偏 (LSB) -> °/s -> rad/s, 量程因子由驱动层统一管理 */
    gx = imu660rb_gyro_transition(imu660rb_gyro_x) * DEG_TO_RAD;
    gy = imu660rb_gyro_transition(imu660rb_gyro_y) * DEG_TO_RAD;
    gz = imu660rb_gyro_transition(imu660rb_gyro_z) * DEG_TO_RAD;

    /* 加速度计: LSB -> G, 一阶低通滤波抑制振动噪声 (同 RT1064 alpha=0.3) */
    ax = imu660rb_acc_transition(imu660rb_acc_x);
    ay = imu660rb_acc_transition(imu660rb_acc_y);
    az = imu660rb_acc_transition(imu660rb_acc_z);
    acc_fx = ACC_ALPHA * ax + (1.0f - ACC_ALPHA) * acc_fx;
    acc_fy = ACC_ALPHA * ay + (1.0f - ACC_ALPHA) * acc_fy;
    acc_fz = ACC_ALPHA * az + (1.0f - ACC_ALPHA) * acc_fz;
    ax = acc_fx;
    ay = acc_fy;
    az = acc_fz;

    /* --- 2. 加速度归一化 (单位重力向量) --- */
    norm = fast_rsqrt(ax * ax + ay * ay + az * az);
    ax *= norm;
    ay *= norm;
    az *= norm;

    /* --- 3. 估算重力分量 (从当前四元数) --- */
    vx = 2.0f * (q1 * q3 - q0 * q2);
    vy = 2.0f * (q0 * q1 + q2 * q3);
    vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

    /* --- 3.5 重力方向检测: 若估计重力与实测重力反平行 (点积<0),
     *     则四元数处于翻转平衡点, 绕X轴旋转180°拉回正确方向 --- */
    if(ax * vx + ay * vy + az * vz < 0.0f)
    {
        float t0 = -q1, t1 = q0, t2 = q3, t3 = -q2;
        q0 = t0; q1 = t1; q2 = t2; q3 = t3;
        vx = 2.0f * (q1 * q3 - q0 * q2);
        vy = 2.0f * (q0 * q1 + q2 * q3);
        vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
    }

    /* --- 4. 叉积求重力误差 --- */
    ex = ay * vz - az * vy;
    ey = az * vx - ax * vz;
    ez = ax * vy - ay * vx;

    /* --- 5. PI 修正陀螺仪 --- */
    i_ex += halfT * ex;
    i_ey += halfT * ey;
    i_ez += halfT * ez;
    gx += KP * ex + KI * i_ex;
    gy += KP * ey + KI * i_ey;
    gz += KP * ez + KI * i_ez;

    /* --- 6. 一阶龙格库塔更新四元数 --- */
    {
        float dq0 = -q1 * gx - q2 * gy - q3 * gz;
        float dq1 =  q0 * gx + q2 * gz - q3 * gy;
        float dq2 =  q0 * gy - q1 * gz + q3 * gx;
        float dq3 =  q0 * gz + q1 * gy - q2 * gx;

        q0 += dq0 * halfT;
        q1 += dq1 * halfT;
        q2 += dq2 * halfT;
        q3 += dq3 * halfT;
    }

    /* --- 7. 四元数归一化 --- */
    norm = fast_rsqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= norm;
    q1 *= norm;
    q2 *= norm;
    q3 *= norm;

    /* --- 8. 四元数 -> 欧拉角 (°) --- */
    {
        float q0q0 = q0 * q0;
        float q0q1 = q0 * q1;
        float q0q2 = q0 * q2;
        float q0q3 = q0 * q3;
        float q1q1 = q1 * q1;
        float q1q2 = q1 * q2;
        float q1q3 = q1 * q3;
        float q2q2 = q2 * q2;
        float q2q3 = q2 * q3;
        float q3q3 = q3 * q3;

        eulerAngle.pitch = asinf(-2.0f * q1q3 + 2.0f * q0q2) * RAD_TO_DEG;
        eulerAngle.roll  = atan2f(2.0f * q2q3 + 2.0f * q0q1,
                                  -2.0f * q1q1 - 2.0f * q2q2 + 1.0f) * RAD_TO_DEG;
        eulerAngle.yaw   = atan2f(2.0f * q1q2 + 2.0f * q0q3,
                                  -2.0f * q2q2 - 2.0f * q3q3 + 1.0f) * RAD_TO_DEG;
    }

    /* --- 9. Yaw 归一化到 [0, 360) --- */
    if (eulerAngle.yaw >= 360.0f)
        eulerAngle.yaw -= 360.0f;
    else if (eulerAngle.yaw < 0.0f)
        eulerAngle.yaw += 360.0f;
}