#ifndef SEGMENT_FSM_H
#define SEGMENT_FSM_H

#include "preproc/corridor_msgs.h"
#include "preproc/corridor_preproc.h"

/**
 * @brief 段状态机的运行阶段枚举
 */
typedef enum {
    SEG_STATE_IDLE = 0,       // 待命：不输出任何指令
    SEG_STATE_CORRIDOR,       // 走廊跟踪中：放行控制器输出
    SEG_STATE_APPROACH,       // 接近端墙：降速保护
    SEG_STATE_STOP,           // 到端停车：强制零速
    SEG_STATE_ESTOP,          // 紧急停车：传感器全部失效或异常
} SegFsmState_t;

/**
 * @brief 段状态机配置参数
 */
typedef struct {
    float d_front_stop;       // 前向停车距离阈值 (m)，低于此值直接停车
    float d_front_approach;   // 前向减速预警距离 (m)，低于此值开始线性减速
    float approach_min_v;     // 减速区内最低速度 (m/s)，防止爬行太慢永远到不了
    float conf_estop_thresh;  // 置信度紧急停车阈值，低于此值触发 E-Stop
} SegFsmConfig_t;

/**
 * @brief 段状态机对外暴露的完整快照 (供日志或上位机读取)
 */
typedef struct {
    SegFsmState_t state;      // 当前状态
    float safe_v;             // 经安全仲裁后的最终线速度 (m/s)
    float safe_w;             // 经安全仲裁后的最终角速度 (rad/s)
} SegFsmOutput_t;

#ifdef __cplusplus
extern "C" {
#endif

/**
 * @brief 初始化段状态机
 * @param config 距离阈值与降级参数
 */
void SegFsm_Init(const SegFsmConfig_t *config);

/**
 * @brief 启动走廊跟踪（从 IDLE 切换到 CORRIDOR）
 */
void SegFsm_Start(void);

/**
 * @brief 核心函数：输入控制器的期望指令，输出安全仲裁后的最终指令
 * @param raw_cmd   走廊控制器的原始输出 (v, w)
 * @param obs       预处理层的观测快照 (提供 d_front 和 valid_mask)
 * @param state     滤波器的状态输出 (提供 conf 置信度)
 * @param out       安全仲裁后的最终输出
 */
void SegFsm_Update(const RawCmd_t *raw_cmd,
                   const CorridorObs_t *obs,
                   const CorridorState_t *state,
                   SegFsmOutput_t *out);

/**
 * @brief 获取当前状态机状态 (用于日志打印)
 */
SegFsmState_t SegFsm_GetState(void);

/**
 * @brief 获取状态名称字符串 (用于日志打印)
 */
const char* SegFsm_GetStateName(SegFsmState_t s);

#ifdef __cplusplus
}
#endif

#endif // SEGMENT_FSM_H