#ifndef CORRIDOR_MSGS_H
#define CORRIDOR_MSGS_H

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

/* =========================================================
 * EKF 状态维度定义
 * ========================================================= */
#define EKF_STATE_DIM   3   // [e_y, e_th, s]
#define EKF_OBS_DIM     3   // [z_ey, z_eth_L, z_eth_R]

/* χ² 检验门限 (95% 置信度) */
/* 1 DOF: 3.84, 2 DOF: 5.99, 3 DOF: 7.81 */
#define CHI2_THRESHOLD_1DOF   3.84f
#define CHI2_THRESHOLD_2DOF   5.99f
#define CHI2_THRESHOLD_3DOF   7.81f

/**
 * @brief 走廊观测快照 (由 Blackboard 数据转化清洗而来)
 * @note  所有距离单位统一为 米 (m)
 */
typedef struct {
  uint32_t t_ms;           // 观测时间戳
  float d_lf, d_lr;        // 左侧前后距离 (m)
  float d_rf, d_rr;        // 右侧前后距离 (m)
  float d_front, d_back;   // 前后防撞/到端距离 (m)
  uint8_t valid_mask;      // 位域掩码：标记哪些雷达数据是当前存活且合法的
} CorridorObs_t;

/**
 * @brief 走廊相对定位状态 (EKF 的输出结果)
 */
typedef struct {
  uint32_t t_ms;           // 状态更新时间戳
  float e_y;               // 横向偏差 (m)，向左偏为正
  float e_th;              // 航向偏差 (rad)，车头偏左为正
  float s;                 // 沿走廊进度里程 (m)
  float conf;              // 置信度/健康度 (0.0~1.0)，用于触发降级
  
  /* EKF 扩展输出 */
  float P[EKF_STATE_DIM][EKF_STATE_DIM];  // 状态协方差矩阵
  float innovation[EKF_OBS_DIM];          // 新息向量 (观测残差)
  float mahalanobis_d2;                    // 马氏距离平方
  uint8_t obs_reject_mask;                 // 被拒绝的观测位掩码
} CorridorState_t;

/**
 * @brief 纯控制指令 (准备交给安全层仲裁)
 */
typedef struct {
  uint32_t t_ms;
  float v;                 // 期望线速度 (m/s)
  float w;                 // 期望角速度 (rad/s)
  uint8_t flags;           // 控制特殊标志位
} RawCmd_t;

#endif // CORRIDOR_MSGS_H