/**
 * @file   robot_odom.h
 * @brief  里程计积分模块：从 CAN 0x200 轮增量帧累加积分，
 *         经差分驱动运动学转换为机体线速度 vx 与角速度 wz，
 *         最终通过 Blackboard_UpdateOdom() 推送给导航流水线。
 *
 * @note  本模块不依赖任何传感器，只消费 CAN 协议层已有的 odom_delta 数据。
 *        所有标定参数统一在 robot_params.h 中管理。
 *
 *        运动学模型（差分驱动）:
 *          v_left  = (fl_delta + rl_delta) / 2
 *          v_right = (fr_delta + rr_delta) / 2
 *          vx      = (v_left + v_right) / 2   [m/s]
 *          wz      = (v_right - v_left) / wheel_track  [rad/s]
 */

#ifndef ROBOT_ODOM_H
#define ROBOT_ODOM_H

#include <stdint.h>
#include <stdbool.h>
#include "robot_params.h"  /* 统一参数头文件 */

/* =========================================================
 * 标定参数（从 robot_params.h 引入，勿在此文件重复定义）
 * ========================================================= */
#define ODOM_TICKS_PER_REV    PARAM_ENCODER_CPR
#define ODOM_WHEEL_DIAMETER   PARAM_WHEEL_DIAMETER
#define ODOM_WHEEL_TRACK      PARAM_WHEEL_TRACK

/* =========================================================
 * 预计算常数（自动生成，勿手动修改）
 * ========================================================= */
#define ODOM_TICKS_TO_M       (3.14159265358979f * (ODOM_WHEEL_DIAMETER) / (ODOM_TICKS_PER_REV))
/**< @brief 每 tick 对应的轮缘弧长 [m]，即 PI*D / CPR */
#define ODOM_WHEEL_TRACK_INV  (1.0f / (ODOM_WHEEL_TRACK))
/**< @brief 轮距倒数 [1/m]，用于快速计算角速度 */

/* =========================================================
 * 配置结构
 * ========================================================= */
typedef struct {
    /** @brief 里程计有效标志，false 时 vx/wz 强制为 0 */
    bool     online;
    /** @brief 里程计在线但 Tick 增量为 0 的连续帧数（用于检测卡死） */
    uint16_t zero_delta_count;
    /** @brief 上次更新时的系统时间 [ms] */
    uint32_t last_update_ms;
} OdomStatus_t;

/* =========================================================
 * API 接口
 * ========================================================= */

/**
 * @brief  里程计模块初始化
 * @note   重置内部累加器，建议在系统启动阶段调用一次
 */
void Odom_Init(void);

/**
 * @brief  里程计更新主函数
 * @param  now_ms       当前系统时间 [ms]，由 HAL_GetTick() 提供
 * @param  fl_delta     左前轮增量 ticks（本周期内编码器增量，可为负）
 * @param  rl_delta     左后轮增量 ticks
 * @param  fr_delta     右前轮增量 ticks
 * @param  rr_delta     右后轮增量 ticks
 * @param  odom_span_ms 本批次增量帧的真实时间跨度 [ms]，由
 *                      SNC_CAN_ConsumeOdomDelta() 返回。
 *                      传 0 表示无法计算（如仅一帧），此时退化为
 *                      now_ms - last_update_ms。
 *
 * @note   由调用方（本例中 monitorTask 或 navTask）定期轮询 CAN 上下文
 *         中的 odom_delta 数据后主动调用，推荐调用周期 20~100ms。
 *         函数内部完成：均值滤波 -> 差分运动学 -> Blackboard 更新。
 */
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);

/**
 * @brief  odom 断流超时处理
 * @param  now_ms 当前系统时间 [ms]
 * @param  timeout_ms 允许的最大无更新时长 [ms]
 *
 * @note   超时后会将 vx/wz 清零、标记离线，并同步刷新黑板，
 *         防止导航层继续使用最后一次有效速度。
 */
void Odom_HandleTimeout(uint32_t now_ms, uint32_t timeout_ms);

/**
 * @brief  读取当前里程计状态快照（供外部任务消费）
 * @param  out_status  输出状态结构体
 * @param  out_vx      输出线速度 [m/s]
 * @param  out_wz      输出角速度 [rad/s]
 *
 * @note   内部使用临界区保护，调用方可在任何上下文安全使用。
 */
void Odom_GetSpeed(float *out_vx, float *out_wz, OdomStatus_t *out_status);

/**
 * @brief  获取累积里程（沿程 s）
 * @return 累计行驶距离 [m]（从 Odom_Init 复位后开始积分）
 *
 * @note   由走廊状态机用于段落终止判断。也可用于比赛计时参考。
 */
float Odom_GetDistance(void);

/**
 * @brief  复位里程计（清零累积距离和状态）
 */
void Odom_Reset(void);

#endif /* ROBOT_ODOM_H */