/**
 * @file   robot_odom.c
 * @brief  里程计积分实现：消费 CAN 0x200 轮增量帧，
 *         经差分驱动运动学转换为机体速度并推送黑板。
 *
 *         调用链（monitorTask 100ms 周期）:
 *           CAN Rx ISR ──> snc_parse_odom_delta()  [snc_can_app.c]
 *                             │  (每帧累加到 odom_accum，不覆盖)
 *                             v
 *           monitorTask ──> SNC_CAN_ConsumeOdomDelta()  [snc_can_app.c]
 *                             │  (原子取走累计增量并清零)
 *                             v
 *                          Odom_Update()  [robot_odom.c]
 *                             │  (仅在有新帧时调用，杜绝重复积分)
 *                             v
 *           Blackboard_UpdateOdom(vx, wz)  [robot_blackboard.c]
 *                             │
 *                             v
 *           navTask ──> Blackboard_GetSnapshot() ──> CorridorFilter_Update()
 *
 *         运动学公式:
 *           v_left  = (fl_delta + rl_delta) / 2  * TICKS_TO_M  / dt
 *           v_right = (fr_delta + rr_delta) / 2  * TICKS_TO_M  / dt
 *           vx      = (v_left + v_right) / 2       [m/s]
 *           wz      = (v_right - v_left) / track   [rad/s]
 *
 *         里程累加（用于沿程 s 估计）:
 *           s += |vx| * dt   [m]
 */

#include "robot_odom.h"
#include "robot_blackboard.h"
#include "FreeRTOS.h"
#include "task.h"

#include <math.h>

/* =========================================================
 * 内部静态状态
 * ========================================================= */

/** @brief 里程计运行时数据（全部由临界区保护） */
static struct {
    float    vx;           /**< 机体线速度 [m/s] */
    float    wz;           /**< 机体角速度 [rad/s] */
    float    distance;     /**< 累计行驶距离 [m]（从 init 复位后积分） */
    bool     online;       /**< 里程计有效标志 */
    uint32_t last_update_ms; /**< 上次有效更新时的系统时间 [ms] */
    uint16_t zero_cnt;     /**< 连续收到零增量的帧数 */
    bool     initialized;  /**< 是否已完成初始化 */
} s_odom = {
    .vx            = 0.0f,
    .wz            = 0.0f,
    .distance      = 0.0f,
    .online        = false,
    .last_update_ms = 0U,
    .zero_cnt      = 0U,
    .initialized   = false,
};

/* =========================================================
 * 内部辅助函数
 * ========================================================= */

/**
 * @brief  内部临界区读写接口（使用 taskENTER/EXIT_CRITICAL）
 */
static inline void lock_odom(void)
{
    taskENTER_CRITICAL();
}

static inline void unlock_odom(void)
{
    taskEXIT_CRITICAL();
}

/**
 * @brief  差分驱动运动学核心
 *
 * @param  fl  左前轮增量 ticks
 * @param  rl  左后轮增量 ticks
 * @param  fr  右前轮增量 ticks
 * @param  rr  右后轮增量 ticks
 * @param  dt  采样周期 [s]
 * @param  out_vx  输出线速度 [m/s]
 * @param  out_wz  输出角速度 [rad/s]
 *
 * @note   内部使用均值滤波：同侧前后轮求平均后再做差速，
 *         这样可以抑制单轮打滑对整体估计的影响。
 */
static void differential_kinematics(int16_t fl, int16_t rl,
                                    int16_t fr, int16_t rr,
                                    float dt,
                                    float *out_vx, float *out_wz)
{
    /* 1. 同侧均值滤波（抵消前后轮差分） */
    float v_left_ticks  = 0.5f * ((float)fl + (float)rl);  /* [ticks/period] */
    float v_right_ticks = 0.5f * ((float)fr + (float)rr); /* [ticks/period] */

    /* 2. tick -> m/s */
    float v_left  = v_left_ticks  * ODOM_TICKS_TO_M / dt;  /* [m/s] */
    float v_right = v_right_ticks * ODOM_TICKS_TO_M / dt;  /* [m/s] */

    /* 3. 差分驱动运动学逆解 */
    /*    vx = (vL + vR) / 2 */
    /*    wz = (vR - vL) / track */
    *out_vx = 0.5f * (v_left + v_right);
    *out_wz = (v_right - v_left) * ODOM_WHEEL_TRACK_INV;

    /* 4. 合理性检查：物理约束限幅
     *    H743 主频 480MHz，dt 最小约 1ms，
     *    若单帧 tick 绝对值超过 65535/dt，说明增量帧发生异常合并/撕裂。 */
    const float MAX_VX_PERIOD = 5.0f;  /* 允许的最大瞬时线速度 [m/s]（车辆标称最高 1m/s） */
    const float MAX_WZ_PERIOD = 20.0f; /* 允许的最大瞬时角速度 [rad/s]（极端原地转向） */

    if (fabsf(*out_vx) > MAX_VX_PERIOD) {
        *out_vx = (*out_vx > 0.0f) ? MAX_VX_PERIOD : -MAX_VX_PERIOD;
    }
    if (fabsf(*out_wz) > MAX_WZ_PERIOD) {
        *out_wz = (*out_wz > 0.0f) ? MAX_WZ_PERIOD : -MAX_WZ_PERIOD;
    }
}

/* =========================================================
 * 公共 API 实现
 * ========================================================= */

void Odom_Init(void)
{
    lock_odom();
    s_odom.vx             = 0.0f;
    s_odom.wz             = 0.0f;
    s_odom.distance       = 0.0f;
    s_odom.online         = false;
    s_odom.last_update_ms = 0U;
    s_odom.zero_cnt      = 0U;
    s_odom.initialized    = true;
    unlock_odom();
}

void Odom_Update(uint32_t now_ms,
                 int16_t fl_delta, int16_t rl_delta,
                 int16_t fr_delta, int16_t rr_delta,
                 uint32_t odom_span_ms)
{
    if (!s_odom.initialized) {
        Odom_Init();
    }

    /* 计算采样周期 dt
     *
     * 优先使用 odom_span_ms（CAN 0x200 帧的真实时间跨度），
     * 这是编码器增量实际覆盖的时间窗，不受 monitorTask 调度抖动影响。
     *
     * 退化策略：
     *   - odom_span_ms > 0  → 直接使用（最精确）
     *   - odom_span_ms == 0 → 仅收到一帧，无法算帧间 dt，
     *                          退化为 now_ms - last_update_ms（兼容旧行为）
     *   - 首帧（last_update_ms == 0 且 odom_span_ms == 0）→ 默认 60ms
     *     （0x200 帧典型间隔）
     */
    float dt;
    if (odom_span_ms > 0U && odom_span_ms <= 1000U) {
        /* 有真实帧时间跨度，优先使用 */
        dt = (float)odom_span_ms / 1000.0f;
    } else if (s_odom.last_update_ms == 0U || !s_odom.online) {
        /*
         * 系统首帧，或 odom 刚从断流中恢复但当前只有一帧时，
         * 无法可靠反推出这份增量覆盖的真实时间窗，退化为典型帧间隔。
         */
        dt = 0.06f;
    } else {
        uint32_t dt_ms = now_ms - s_odom.last_update_ms;
        if (dt_ms == 0U || dt_ms > 1000U) {
            /* 超时（超过 1 秒没有新数据）视为里程计离线 */
            dt = 0.0f;
        } else {
            dt = (float)dt_ms / 1000.0f;
        }
    }

    /* 检测零增量：四轮全 0 可能是底盘未动或通信丢失 */
    bool is_zero_delta = (fl_delta == 0 && rl_delta == 0 &&
                          fr_delta == 0 && rr_delta == 0);

    if (dt <= 0.0f) {
        /* 非法 dt，强制离线 */
        lock_odom();
        s_odom.online = false;
        unlock_odom();
        return;
    }

    /* 计算差分运动学 */
    float vx_local, wz_local;
    differential_kinematics(fl_delta, rl_delta, fr_delta, rr_delta,
                            dt, &vx_local, &wz_local);

    /* 零速度保护：连续多帧零增量后，强制归零（防止漂移） */
    lock_odom();
    if (is_zero_delta) {
        s_odom.zero_cnt++;
        if (s_odom.zero_cnt >= 3U) {
            /* 连续 3 帧零增量（~180ms @ 60ms 帧率），认为机器人静止 */
            s_odom.vx = 0.0f;
            s_odom.wz = 0.0f;
            s_odom.online = true;
        }
    } else {
        s_odom.zero_cnt = 0U;
        s_odom.vx = vx_local;
        s_odom.wz = wz_local;
        s_odom.online = true;

        /* 积分累积里程（取线速度的绝对值，保证后退也累加） */
        s_odom.distance += fabsf(s_odom.vx) * dt;
    }

    s_odom.last_update_ms = now_ms;
    unlock_odom();

    /* 推送黑板（黑板内部自带临界区保护） */
    Blackboard_UpdateOdom(s_odom.vx, s_odom.wz);
}

void Odom_HandleTimeout(uint32_t now_ms, uint32_t timeout_ms)
{
    bool timed_out = false;

    if (!s_odom.initialized || timeout_ms == 0U) {
        return;
    }

    lock_odom();
    if (s_odom.online &&
        s_odom.last_update_ms != 0U &&
        (now_ms - s_odom.last_update_ms) > timeout_ms) {
        s_odom.vx = 0.0f;
        s_odom.wz = 0.0f;
        s_odom.online = false;
        s_odom.zero_cnt = 0U;
        timed_out = true;
    }
    unlock_odom();

    if (timed_out) {
        Blackboard_UpdateOdom(0.0f, 0.0f);
    }
}

void Odom_GetSpeed(float *out_vx, float *out_wz, OdomStatus_t *out_status)
{
    if (out_vx != NULL)    *out_vx = s_odom.vx;
    if (out_wz != NULL)    *out_wz = s_odom.wz;

    if (out_status != NULL) {
        lock_odom();
        out_status->online           = s_odom.online;
        out_status->zero_delta_count = s_odom.zero_cnt;
        out_status->last_update_ms   = s_odom.last_update_ms;
        unlock_odom();
    }
}

float Odom_GetDistance(void)
{
    float d;
    lock_odom();
    d = s_odom.distance;
    unlock_odom();
    return d;
}

void Odom_Reset(void)
{
    lock_odom();
    s_odom.vx             = 0.0f;
    s_odom.wz             = 0.0f;
    s_odom.distance       = 0.0f;
    s_odom.online         = false;
    s_odom.last_update_ms = 0U;
    s_odom.zero_cnt       = 0U;
    unlock_odom();

    Blackboard_UpdateOdom(0.0f, 0.0f);
}