/**
 * @file   global_nav.h
 * @brief  赛道级总控 — 混合导航状态机
 *
 *         三层架构中的最上层：
 *           上层 (本模块): 知道"当前第几沟、下一沟是谁、该往哪转"
 *           中层 (内嵌):   转向/连接段/重捕获动作执行
 *           下层 (已有):   沟内 corridor_ctrl 闭环
 *
 *         替代原 nav_script.c 的赛道编排职能。
 */

#ifndef GLOBAL_NAV_H
#define GLOBAL_NAV_H

#include <stdint.h>
#include <stdbool.h>
#include "preproc/corridor_msgs.h"
#include "Contract/robot_blackboard.h"
#include "nav/track_map.h"

#ifdef __cplusplus
extern "C" {
#endif

/* =========================================================
 * 赛道级阶段枚举
 * ========================================================= */
typedef enum {
    GNAV_IDLE = 0,                  /* 启动区等待 */
    GNAV_ENTRY_STRAIGHT,            /* 入场直线 */
    GNAV_TURN_INTO_CORRIDOR,        /* 转向进入垄沟 (原地转 90°) */
    GNAV_REACQUIRE,                 /* 重捕获走廊 */
    GNAV_ALIGN,                     /* 捕获成功后停车对齐航线 */
    GNAV_CORRIDOR_TRACK,            /* 沟内闭环跟踪 */
    GNAV_TURN_OUT_OF_CORRIDOR,      /* 到端后出沟转向 (原地转 90°) */
    GNAV_LINK_STRAIGHT,             /* 连接段直行 */
    GNAV_TURN_INTO_NEXT,            /* 转向进入下一条沟 (原地转 90°) */
    GNAV_EXIT_STRAIGHT,             /* 出场直行 */
    GNAV_DOCK,                      /* 回停启动区 */
    GNAV_FINISHED,                  /* 终态 */
    GNAV_ERROR                      /* 异常态 (超时兜底) */
} GlobalNavStage_t;

/* =========================================================
 * 配置参数
 * ========================================================= */
typedef struct {
    /* 入场段 */
    float    entry_v;                   /* 入场速度 m/s */
    float    entry_distance;            /* 入场里程上限 m */
    uint32_t entry_timeout_ms;          /* 入场超时 ms */

    /* 转向 */
    float    turn_omega;                /* 转向角速度 rad/s */
    float    turn_tolerance_rad;        /* 转向完成容差 rad */
    float    turn_decel_zone_rad;       /* 接近目标角时的减速区 rad */
    float    turn_min_omega;            /* 减速区最低角速度 rad/s */
    uint32_t turn_timeout_ms;           /* 单次转向超时 ms */

    /* 重捕获 */
    float    reacquire_v;               /* 重捕获速度 m/s */
    float    reacquire_conf_thresh;     /* 重捕获置信度阈值 */
    float    reacquire_width_tol;       /* 走廊宽度容差 m */
    float    reacquire_min_odom;        /* 重捕获最小入沟里程 m */
    uint8_t  reacquire_confirm_ticks;   /* 连续确认拍数 */
    uint32_t reacquire_timeout_ms;

    /* 对齐 */
    float    align_kp_th;               /* 对齐航向P增益 (rad/s per rad) */
    float    align_kp_y;                /* 对齐横向P增益 (rad/s per m) */
    float    align_th_tol_rad;          /* 对齐航向容差 (rad) */
    float    align_y_tol_m;             /* 对齐横向容差 (m) */
    uint8_t  align_confirm_ticks;       /* 对齐确认拍数 */
    uint32_t align_timeout_ms;          /* 对齐超时 ms */
    float    reacquire_min_back_dist;   /* 重捕获最小后激光距离 (m)，用于判断是否真正进沟 */

    /* 沟内 */
    float    corridor_end_detect_dist;  /* 到端检测距离 m */
    float    corridor_length_max;       /* 沟内里程保护上限 m */

    /* 连接段 */
    float    link_v;
    float    link_distance;
    uint32_t link_timeout_ms;
    float    link_gap_runout;          /* 检测到沟口后继续前冲距离 (m) */
    float    link_wall_target;          /* 墙壁跟随目标距离 (m) */
    float    link_wall_kp;              /* 墙壁跟随P增益 */
    float    link_wall_blend;           /* 墙壁跟随权重 (0~1) */

    /* 出场 */
    float    exit_v;                    /* 出场速度 m/s */
    float    exit_runout;               /* 出场冲刺距离 m */
    float    exit_max_dist;             /* 出场里程保护 m */
    uint32_t exit_timeout_ms;

    /* 回停 */
    float    dock_v;                    /* 回停速度 m/s */
    float    dock_distance;             /* 回停距离 m */

    /* 航向保持 */
    float    heading_kp;                /* 航向保持P增益 */

    /* 走廊宽度 (用于重捕获判定) */
    float    corridor_width;            /* 走廊宽度 m */
} GlobalNavConfig_t;

/* =========================================================
 * 输出结构
 * ========================================================= */
typedef struct {
    GlobalNavStage_t  stage;            /* 当前阶段 */
    const char*       stage_name;       /* 阶段名字符串 (调试) */
    uint8_t           corridor_id;      /* 当前/目标垄沟 ID */
    uint8_t           corridors_done;   /* 已完成垄沟数 */

    bool              request_corridor; /* 是否请求沟内闭环 */
    bool              use_override;     /* 是否用 override 指令 */
    float             override_v;
    float             override_w;

    SafetyMode_t      safety_mode;      /* 当前安全模式 */
    bool              active;           /* 导航是否在运行 */
} GlobalNavOutput_t;

/* =========================================================
 * API
 * ========================================================= */
void             GlobalNav_Init(const GlobalNavConfig_t* cfg);
void             GlobalNav_Start(void);
void             GlobalNav_Stop(void);
void             GlobalNav_Reset(void);

/**
 * @brief 核心函数：每个导航周期调用一次
 * @param obs     预处理层的观测快照
 * @param state   EKF 走廊状态
 * @param board   黑板快照 (读 IMU yaw_continuous, odom)
 * @param now_ms  当前系统时间 (HAL_GetTick)，由调用方传入，避免内部依赖 IMU 时间戳
 * @param out     导航输出
 */
void GlobalNav_Update(const CorridorObs_t*      obs,
                      const CorridorState_t*     state,
                      const RobotBlackboard_t*   board,
                      uint32_t                   now_ms,
                      GlobalNavOutput_t*         out);

GlobalNavStage_t GlobalNav_GetStage(void);
const char*      GlobalNav_GetStageName(GlobalNavStage_t stage);

#ifdef __cplusplus
}
#endif

#endif /* GLOBAL_NAV_H */