/**
 * @file   corridor_ekf.h
 * @brief  鲁棒 EKF 走廊相对定位滤波器
 *
 *         状态向量：x = [e_y, e_th, s]^T
 *           - e_y  : 横向偏差 (m)
 *           - e_th : 航向偏差 (rad)
 *           - s    : 沿走廊进度 (m)
 *
 *         观测模型 (方向 B — IMU 主导航向):
 *           - 侧墙观测: 1DOF 标量更新，仅更新 e_y (横向偏差)
 *             z_ey = 左右侧 VL53 平均距离差 (支持分侧内缩补偿)
 *           - IMU 航向观测: 独立 1DOF 标量更新，更新 e_th
 *             z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad
 *           - 侧墙航向观测 (z_eth_L/z_eth_R) 已取消:
 *             VL53 前后差分噪声过大 (±2cm → ~13° 航向噪声)，不适合做航向源
 *
 *         鲁棒机制：
 *           - χ² 马氏距离检验拒绝异常观测
 *           - 分侧 VL53 内缩补偿 (消除左右安装不对称引起的系统性偏置)
 *           - 单侧观测时自适应增大 R (降低退化状态下的信任度)
 *           - 协方差上界保护 (防止发散)
 */

#ifndef CORRIDOR_EKF_H
#define CORRIDOR_EKF_H

#include <stdint.h>
#include <stdbool.h>

/* 先引入消息定义 (含 EKF_STATE_DIM/EKF_OBS_DIM 宏) */
#include "preproc/corridor_msgs.h"

#ifdef __cplusplus
extern "C" {
#endif

/* =========================================================
 * EKF 配置参数
 * ========================================================= */
typedef struct {
    /* 过程噪声协方差 Q */
    float q_ey;         // e_y 过程噪声方差
    float q_eth;        // e_th 过程噪声方差
    float q_s;          // s 过程噪声方差
    float q_ey_eth;     // [改进] e_y 和 e_th 耦合噪声 (横向-航向动力学耦合)

    /* 观测噪声协方差 R */
    float r_ey;         // 横向观测噪声方差 (侧墙)
    float r_eth;        // 航向观测噪声方差 (侧墙)
    float r_eth_imu;    // 航向观测噪声方差 (IMU yaw)，IMU 准确度高时可设较小值

    /* 初始协方差 */
    float P0_diag[3];   // 初始 P 对角线

    /* χ² 检验门限 */
    float chi2_1dof;    // 1 自由度门限 (默认 3.84)
    float chi2_2dof;    // 2 自由度门限 (默认 5.99)

    /* 走廊几何参数 */
    float sensor_base_length;  // 同侧前后雷达间距 L_s
    float corridor_width;      // 走廊标准宽度
    float y_offset;            // 期望偏置
    float side_sensor_inset;   // [兼容] 侧向传感器统一内缩距离 (当 left/right 未单独设置时使用)
    float left_sensor_inset;   // [改进A] 左侧 VL53 内缩距离 (传感器面到车体左外边缘, 实测)
    float right_sensor_inset;  // [改进A] 右侧 VL53 内缩距离 (传感器面到车体右外边缘, 实测)
    float robot_width;         // 车体外轮廓宽度 (用于单侧退化时的精确定位)
} CorridorEKFConfig_t;

/* =========================================================
 * EKF 内部状态
 * ========================================================= */
typedef struct {
    float x[EKF_STATE_DIM];               // 状态向量
    float P[EKF_STATE_DIM][EKF_STATE_DIM]; // 状态协方差
    float K[EKF_STATE_DIM][EKF_OBS_DIM];   // 卡尔曼增益
    float S[EKF_OBS_DIM][EKF_OBS_DIM];     // 新息协方差
    float S_inv[EKF_OBS_DIM][EKF_OBS_DIM]; // S 的逆
} CorridorEKFState_t;

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

/**
 * @brief 初始化 EKF 滤波器
 * @param config 配置参数
 */
void CorridorEKF_Init(const CorridorEKFConfig_t *config);

/**
 * @brief EKF 预测步 (时间更新)
 * @param odom_vx   里程计线速度 (m/s)
 * @param imu_wz    IMU 角速度 (rad/s)
 * @param dt        时间间隔 (s)
 */
void CorridorEKF_Predict(float odom_vx, float imu_wz, float dt);

/**
 * @brief EKF 观测步 (测量更新) - 鲁棒版本
 * @param obs       预处理后的观测快照
 * @param out_state 输出状态 (含马氏距离、拒绝掩码等)
 * @return          成功更新的观测数
 */
int CorridorEKF_Update(const CorridorObs_t *obs, CorridorState_t *out_state);

/**
 * @brief EKF IMU 航向观测更新 (独立于侧墙观测，在 Update 后调用)
 *
 * 将 IMU 连续 yaw 的变化量作为 e_th 的额外标量观测，执行 1DOF EKF 更新。
 * 在侧墙观测丢失时 (转弯/单侧退化) 提供航向约束，防止 e_th 漂移。
 *
 * @param imu_yaw_rad    IMU 连续 yaw 当前值 (rad)
 * @param imu_yaw_ref_rad 进入走廊时锁定的 IMU yaw 参考值 (rad)
 *                        z_eth_imu = imu_yaw_rad - imu_yaw_ref_rad
 * @param valid          IMU 数据是否有效
 */
void CorridorEKF_UpdateIMUYaw(float imu_yaw_rad, float imu_yaw_ref_rad, bool valid);

/**
 * @brief 获取当前状态估计
 */
void CorridorEKF_GetState(CorridorState_t *out);

/**
 * @brief 重置 EKF 状态
 */
void CorridorEKF_Reset(void);

/**
 * @brief 仅重置航向相关状态，用于掉头后重新建立走廊朝向基准
 *
 * 保留横向位置 e_y 与进度 s，只将 e_th 清零并清理其相关协方差，
 * 避免上一趟积累的航向误差在返程首拍继续驱动控制器猛打方向。
 */
void CorridorEKF_ResetHeading(void);

/**
 * @brief 180° 掉头后重建走廊状态
 *
 * 对同一条走廊原地掉头后：
 * - 航向误差 e_th 应回到 0
 * - 横向误差 e_y 在新的前进方向下符号需要翻转
 */
void CorridorEKF_RebaseAfterTurnaround(void);

/**
 * @brief 设置过程噪声 (运行时可调)
 */
void CorridorEKF_SetProcessNoise(float q_ey, float q_eth, float q_s);

/**
 * @brief 设置观测噪声 (运行时可调，用于自适应)
 */
void CorridorEKF_SetMeasurementNoise(float r_ey, float r_eth);

#ifdef __cplusplus
}
#endif

#endif /* CORRIDOR_EKF_H */