/*
    动态ZUPT+卡尔曼
        多了加速度死区、摩擦力速度衰减、高通滤波
        原地摆动产生的速度、距离变化还是没法消除
        水平移动、斜坡移动效果貌似还行
*/
#include "skiing_tracker.h"
#include "../sensor/SC7U22.h"
#include <math.h>
#include <string.h>

#define G_ACCELERATION 9.81f
#define DEG_TO_RAD (3.14159265f / 180.0f)

// --- 算法阈值定义 ---
//两个判断是否静止的必要条件
// 动态零速更新(ZUPT)阈值
// 提高阈值，让“刹车”更灵敏，以便在波浪式前进等慢速漂移时也能触发零速更新
#define ZUPT_ACC_VARIANCE_THRESHOLD 0.2f
// 陀螺仪方差阈值
#define ZUPT_GYR_VARIANCE_THRESHOLD 5.0f

// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为正在进行非滑雪的旋转或摆动 -- 没法完全消除
#define ROTATION_GYR_MAG_THRESHOLD 45.0f
// 启动滑雪阈值：加速度模长与重力的差值大于此值，认为开始运动
// 降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f

// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
// alpha = RC / (RC + dt)，
#define HPF_ALPHA 0.95f
// 加速度死区阈值 (m/s^2)。低于此阈值的加速度被认为是噪声，不参与积分。
// 设得太高会忽略真实的慢速启动，设得太低则无法有效抑制噪声。
#define ACC_DEAD_ZONE_THRESHOLD 0.1f

// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 0.98f  

/**
 * @brief 初始化滑雪追踪器
 */
void skiing_tracker_init(skiing_tracker_t *tracker)
{
    if (!tracker) {
        return;
    }
    // 使用memset一次性清零整个结构体，包括新增的缓冲区
    memset(tracker, 0, sizeof(skiing_tracker_t));
    tracker->state = SKIING_STATE_STATIC;
}

/**
 * @brief 将设备坐标系下的加速度转换为世界坐标系
 * @param acc_device 设备坐标系下的加速度 [x, y, z]
 * @param angle 姿态角 [pitch, roll, yaw]，单位: 度
 * @param acc_world 输出：世界坐标系下的加速度 [x, y, z]
 */
static void transform_acc_to_world_frame(const float *acc_device, const float *angle, float *acc_world)
{
    // 驱动输出的角度与标准航空定义相反，需要取反才能用于标准旋转矩阵。
    float pitch = -angle[0] * DEG_TO_RAD;
    float roll  = -angle[1] * DEG_TO_RAD;

    // TODO: 当引入三轴磁力计后，这里的 yaw 应由磁力计和陀螺仪融合解算得出，以解决航向漂移问题。
    // 目前 yaw 暂时不参与计算，因为仅靠加速度计和陀螺仪无法获得准确的绝对航向角。
    // float yaw = -angle[2] * DEG_TO_RAD;

    float cp = cosf(pitch);
    float sp = sinf(pitch);
    float cr = cosf(roll);
    float sr = sinf(roll);

    float ax = acc_device[0];
    float ay = acc_device[1];
    float az = acc_device[2];

    // 使用经过验证的、正确的身体坐标系到世界坐标系的旋转矩阵 (基于 Y-X 旋转顺序)
    // 这个矩阵将设备测量的加速度(ax, ay, az)正确地转换到世界坐标系(acc_world)。
    // 注意：这里没有使用yaw，主要关心的是坡面上的运动，绝对航向暂时不影响速度和距离的计算。
    // TODO
    acc_world[0] = cp * ax + sp * sr * ay + sp * cr * az;
    acc_world[1] = 0  * ax + cr * ay      - sr * az;
    acc_world[2] = -sp * ax + cp * sr * ay + cp * cr * az;
}


/**
 * @brief 计算缓冲区内三轴数据的方差之和
 */
static float calculate_variance(float buffer[VARIANCE_BUFFER_SIZE][3])
{
    float mean[3] = {0};
    float variance[3] = {0};

    // 1. 计算均值
    for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
        mean[0] += buffer[i][0];
        mean[1] += buffer[i][1];
        mean[2] += buffer[i][2];
    }
    mean[0] /= VARIANCE_BUFFER_SIZE;
    mean[1] /= VARIANCE_BUFFER_SIZE;
    mean[2] /= VARIANCE_BUFFER_SIZE;

    // 2. 计算方差
    for (int i = 0; i < VARIANCE_BUFFER_SIZE; i++) {
        variance[0] += (buffer[i][0] - mean[0]) * (buffer[i][0] - mean[0]);
        variance[1] += (buffer[i][1] - mean[1]) * (buffer[i][1] - mean[1]);
        variance[2] += (buffer[i][2] - mean[2]) * (buffer[i][2] - mean[2]);
    }
    variance[0] /= VARIANCE_BUFFER_SIZE;
    variance[1] /= VARIANCE_BUFFER_SIZE;
    variance[2] /= VARIANCE_BUFFER_SIZE;

    // 返回三轴方差之和，作为一个综合的稳定度指标
    return variance[0] + variance[1] + variance[2];
}


/**
 * @brief 状态机更新
 */
static void update_state_machine(skiing_tracker_t *tracker, const float *acc_device_ms2, const float *gyr_dps)
{
    // 缓冲区未填满时，不进行状态判断，默认为静止
    if (!tracker->buffer_filled) {
        tracker->state = SKIING_STATE_STATIC;
        return;
    }

    // --- 计算关键指标 ---
    float acc_variance = calculate_variance(tracker->acc_buffer); // 计算加速度方差
    float gyr_variance = calculate_variance(tracker->gyr_buffer); // 计算陀螺仪方差
    float gyr_magnitude = sqrtf(gyr_dps[0]*gyr_dps[0] + gyr_dps[1]*gyr_dps[1] + gyr_dps[2]*gyr_dps[2]);
    float acc_magnitude = sqrtf(acc_device_ms2[0]*acc_device_ms2[0] + acc_device_ms2[1]*acc_device_ms2[1] + acc_device_ms2[2]*acc_device_ms2[2]);

    // --- 状态切换逻辑---

    // 原地旋转/摆动检测
    // 增加一个关键前提：只在当前不处于滑雪状态时，才检测原地旋转。
    // 这可以防止滑雪过程中的高速转弯被误判为原地旋转。
    // 暂时没办法完全消除
    if (gyr_magnitude > ROTATION_GYR_MAG_THRESHOLD && tracker->state != SKIING_STATE_SKIING) {
        tracker->state = SKIING_STATE_ROTATING;
        return;
    }

    // 动态零速更新 (ZUPT) 
    // 必须同时满足加速度和角速度都稳定，才能判断为“真静止”，以区分匀速运动
    if (acc_variance < ZUPT_ACC_VARIANCE_THRESHOLD && gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD) {
        tracker->state = SKIING_STATE_STATIC;
        // 速度清零，抑制漂移
       memset(tracker->velocity, 0, sizeof(tracker->velocity));
       tracker->speed = 0.0f;
       //当检测到静止时，必须重置高通滤波器的状态，否则下次启动时会有跳变
       memset(tracker->acc_world_unfiltered_prev, 0, sizeof(tracker->acc_world_unfiltered_prev));
       memset(tracker->acc_world_filtered, 0, sizeof(tracker->acc_world_filtered));
       return;
   }

    // 从静止/旋转状态启动
    if (tracker->state == SKIING_STATE_STATIC || tracker->state == SKIING_STATE_ROTATING) {
        // 最终版启动逻辑：必须同时满足“有足够大的线性加速度”和“旋转不剧烈”两个条件
        // 新增 gyr_magnitude 判断，防止原地旋转产生的离心加速度被误判为启动
        if (fabsf(acc_magnitude - G_ACCELERATION) > START_SKIING_ACC_THRESHOLD &&
            gyr_variance < ZUPT_GYR_VARIANCE_THRESHOLD &&
            gyr_magnitude < ROTATION_GYR_MAG_THRESHOLD) {
            tracker->state = SKIING_STATE_SKIING;
            return;
        }
    }
    // 最后的 fall-through 逻辑已移除，以修复原地旋转被错误判断为滑雪的bug。
    // 如果不满足任何状态切换条件，状态将保持不变，直到ZUPT或启动条件被满足。
}


/**
 * @brief 主更新函数
 */
void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_dps, float *angle, float dt)
{
    if (!tracker || !acc_g || !gyr_dps || !angle || dt <= 0) {
        return;
    }

    // --- 数据预处理和缓冲 ---
    float acc_device_ms2[3];
    acc_device_ms2[0] = acc_g[0] * G_ACCELERATION;
    acc_device_ms2[1] = acc_g[1] * G_ACCELERATION;
    acc_device_ms2[2] = acc_g[2] * G_ACCELERATION;

    // 将最新数据存入缓冲区
    memcpy(tracker->acc_buffer[tracker->buffer_index], acc_device_ms2, sizeof(acc_device_ms2));
    memcpy(tracker->gyr_buffer[tracker->buffer_index], gyr_dps, 3 * sizeof(float));
    
    tracker->buffer_index++;
    if (tracker->buffer_index >= VARIANCE_BUFFER_SIZE) {
        tracker->buffer_index = 0;
        tracker->buffer_filled = 1; // 标记缓冲区已满
    }

    // --- 更新状态机 ---
    update_state_machine(tracker, acc_device_ms2, gyr_dps);

    // --- 根据状态进行计算 ---
    if (tracker->state == SKIING_STATE_SKIING) {
        // 坐标转换 & 移除重力
        transform_acc_to_world_frame(acc_device_ms2, angle, tracker->acc_world);
        tracker->acc_world[2] -= G_ACCELERATION;

        // 对世界坐标系下的加速度进行高通滤波，消除直流偏置和重力残差
        for (int i = 0; i < 3; i++) {
            tracker->acc_world_filtered[i] = HPF_ALPHA * (tracker->acc_world_filtered[i] + tracker->acc_world[i] - tracker->acc_world_unfiltered_prev[i]);
            tracker->acc_world_unfiltered_prev[i] = tracker->acc_world[i];
        }

        // 应用加速度死区，忽略微小抖动和噪声
        float acc_horizontal_mag = sqrtf(tracker->acc_world_filtered[0] * tracker->acc_world_filtered[0] +
                                         tracker->acc_world_filtered[1] * tracker->acc_world_filtered[1]);

        if (acc_horizontal_mag > ACC_DEAD_ZONE_THRESHOLD) {
            // 只有当水平加速度足够大时，才进行速度积分
            tracker->velocity[0] += tracker->acc_world_filtered[0] * dt;
            tracker->velocity[1] += tracker->acc_world_filtered[1] * dt;
            // 垂直方向的速度暂时不积分，极易受姿态误差影响而漂移
            // tracker->velocity[2] += tracker->acc_world_filtered[2] * dt;
        }
        // 如果加速度小于阈值，则不更新速度，相当于速度保持不变（或受下一步的阻尼影响而衰减）

    } else {
        // 在静止或旋转状态下，速度已经在状态机内部被清零
        // 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
        tracker->velocity[0] *= SPEED_ATTENUATION;
        tracker->velocity[1] *= SPEED_ATTENUATION;
        tracker->velocity[2] = 0; // 垂直速度强制归零
    }

    // --- 更新速率和距离 ---
    // 只基于水平速度计算速率和距离
    tracker->speed = sqrtf(tracker->velocity[0] * tracker->velocity[0] +
                           tracker->velocity[1] * tracker->velocity[1]);
    tracker->distance += tracker->speed * dt;
}


// 传感器数据采集与处理任务
void sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) {
    static skiing_tracker_t my_skiing_tracker;
    static int initialized = 0;
    static int calibration_done = 0;

    static signed short combined_raw_data[6];
    static float final_angle_data[3];   // 计算得到的欧若拉角
    static float calibrated_acc_g[3];   // 转换后的加速度计数据
    static float calibrated_gyr_dps[3]; // 转换后的陀螺仪数据

    const float delta_time = 0.01f;

    if (!initialized) {
        skiing_tracker_init(&my_skiing_tracker);
        initialized = 1;
        printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
    }

    memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
    memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));

    unsigned char status;
    if (!calibration_done) {  //第1次启动，开启零漂检测
        status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
        if (status == 1) {
            calibration_done = 1;
            printf("Sensor calibration successful! Skiing mode is active.\n");
        }
    } else {
        status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
    }

    if (status == 1) {
        // 加速度 LSB to g
        calibrated_acc_g[0] = (float)combined_raw_data[0] / 8192.0f;
        calibrated_acc_g[1] = (float)combined_raw_data[1] / 8192.0f;
        calibrated_acc_g[2] = (float)combined_raw_data[2] / 8192.0f;

        // 陀螺仪 LSB to dps (度/秒)
        // ±2000dps量程下，转换系数约为 0.061
        calibrated_gyr_dps[0] = (float)combined_raw_data[3] * 0.061f;
        calibrated_gyr_dps[1] = (float)combined_raw_data[4] * 0.061f;
        calibrated_gyr_dps[2] = (float)combined_raw_data[5] * 0.061f;

        skiing_tracker_update(&my_skiing_tracker, calibrated_acc_g, calibrated_gyr_dps, final_angle_data, delta_time);

        // 打印逻辑保持不变
        static int count = 0;
        if(count < 10){
            count++;
            return;
        } else {
            count = 0;
        }
        printf("State: %d, Speed: %.2f m/s, Distance: %.2f m\n",
               my_skiing_tracker.state,
               my_skiing_tracker.speed,
               my_skiing_tracker.distance);

    } else if (status == 0) {
        // printf("Sensor is calibrating...\n");
    } else {
        // printf("Angle calculation error or calibration not finished.\n");
    }
}