/**
 * @file   global_nav.c
 * @brief  璧涢亾绾ф€绘帶 鈥?娣峰悎瀵艰埅鐘舵€佹満瀹炵幇
 *
 *         鍦哄湴缁撴瀯: 鍨勬矡娌縓杞?妯悜)鍒嗗竷锛屽乏鍙充袱绔悇鏈夌旱鍚戠閮ㄩ€氶亾
 *         鍚姩鍖哄湪宸︿笅瑙掞紝鍏ュ彛瀵归綈宸︾閫氶亾
 *
 *         S 鍨嬮亶鍘?
 *           鍏ュ満(鍖楄)鈫掑彸杞叆C1(鈫掍笢)鈫掑彸绔埌绔啋宸﹁浆鈫掑寳琛屸啋宸﹁浆鍏2(鈫愯タ)
 *           鈫掑乏绔埌绔啋鍙宠浆鈫掑寳琛屸啋鍙宠浆鍏3(鈫掍笢)鈫?..鈫扖6(鈫愯タ)
 *           鈫掑乏绔埌绔啋宸﹁浆(鏈濆崡)鈫掑崡琛屽嚭鍦衡啋鍥炲仠鍚姩鍖? *
 *         绔儴閫氶亾鐗圭偣:
 *           - 涓€渚ц创鍥存爮(VL53鑳芥祴鍒?, 鍙︿竴渚т氦鏇垮嚭鐜板瀯鑳岀闈㈠拰鍨勬矡寮€鍙? *           - 涓嶈兘渚濊禆鍙屼晶VL53鍋欵KF, 蹇呴』鐢↖MU鑸悜淇濇寔+鍓嶅悗婵€鍏夊埌绔娴? */

#include "global_nav.h"
#include "est/corridor_filter.h"
#include "preproc/corridor_preproc.h"
#include "robot_params.h"
#include <math.h>
#include <string.h>

/* =========================================================
 * 鍐呴儴鐘舵€? * ========================================================= */
static struct {
    GlobalNavStage_t stage;
    bool             running;
    bool             initialized;

    /* 璧涢亾绾?*/
    uint8_t  current_corridor_id;
    uint8_t  corridors_completed;

    /* 杞悜 */
    float    turn_start_yaw_deg;        /* IMU yaw_continuous at turn start */
    float    turn_target_delta_deg;     /* 90掳 */
    int8_t   turn_sign;                 /* +1 (CCW) or -1 (CW) */

    /* 閲岀▼ (鐢ㄩ噷绋嬭绉垎璺濈) */
    float    stage_entry_odom_vx_accum; /* 杩涘叆闃舵鏃剁殑閲岀▼璁＄疮璁¤窛绂?*/
    float    odom_distance_accum;       /* 杩愯涓寔缁Н鍒嗙殑閲岀▼ */

    /* 瓒呮椂 */
    uint32_t stage_start_ms;

    /* 閲嶆崟鑾?*/
    uint8_t  reacquire_ok_count;

    /* 鍘熷湴瀵规 */
    uint8_t  align_ok_count;

    /* 鍑哄満 */
    bool     exit_vl53_lost;
    float    exit_lost_distance;

    /* 鑸悜淇濇寔 */
    float    heading_ref_deg;

    /* 杩炴帴娈? 澶氫紶鎰熷櫒杈呭姪 */
    float    link_d_front_start;    /* 杩涘叆杩炴帴娈垫椂鍓嶆縺鍏夎鏁?(m) */
    bool     link_d_front_valid;    /* 杩涘叆鏃跺墠婵€鍏夋槸鍚︽湁鏁?*/
    uint8_t  link_gap_count;        /* 闈炲洿鏍忎晶 VL53 杩炵画涓㈠け璁℃暟 (娌熷彛纭) */

    /* EKF 杩涘害淇濆瓨 */
    float    corridor_entry_s;

    /* 閰嶇疆 */
    GlobalNavConfig_t cfg;
} s_nav;

/* 涓婁竴涓懆鏈熺殑閲岀▼璁￠€熷害锛岀敤浜庣Н鍒?*/
static float s_last_odom_vx = 0.0f;
static uint32_t s_last_update_ms = 0;

/* =========================================================
 * 杈呭姪鍑芥暟
 * ========================================================= */
static inline float gnav_clampf(float val, float lo, float hi)
{
    if (val < lo) return lo;
    if (val > hi) return hi;
    return val;
}

static inline float gnav_fabsf(float x)
{
    return x < 0.0f ? -x : x;
}

/** 绠€鍗?P 鎺у埗鑸悜淇濇寔锛岃緭鍏ュ亸宸?(deg)锛岃緭鍑鸿閫熷害 (rad/s) */
static float heading_hold_pd(float current_yaw_deg, float ref_yaw_deg, float kp)
{
    float err_deg = ref_yaw_deg - current_yaw_deg;
    float w = kp * err_deg;  /* kp 鎶?掳 鏄犲皠鍒?rad/s */
    return gnav_clampf(w, -1.0f, 1.0f);
}

/** 妫€鏌ヤ晶鍚?VL53 鏄惁鎺㈠埌澹?(鑷冲皯鏈変竴渚х殑鍓嶅悗閮芥湁鏁? 鈥?浠呯敤浜庨噸鎹曡幏 */
static bool side_walls_detected(const CorridorObs_t* obs)
{
    bool left_ok  = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
                    (obs->valid_mask & CORRIDOR_OBS_MASK_LR);
    bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
                    (obs->valid_mask & CORRIDOR_OBS_MASK_RR);
    return left_ok || right_ok;
}

/**
 * @brief 妫€娴嬮潪鍥存爮渚?VL53 鏄惁鍙戠幇鍨勬矡寮€鍙?(杩炴帴娈垫矡鍙ｈ緟鍔╁垽瀹?
 *
 * 鍦ㄧ旱鍚戠閮ㄩ€氶亾闈㈡湞鍖楄椹舵椂锛? *   - 鍒氳蛋瀹屽悜涓?EAST)鐨勬矡 鈫?褰撳墠鍦ㄥ彸绔€氶亾 鈫?鍙充晶璐村洿鏍?鈫?妫€鏌ュ乏渚L53
 *   - 鍒氳蛋瀹屽悜瑗?WEST)鐨勬矡 鈫?褰撳墠鍦ㄥ乏绔€氶亾 鈫?宸︿晶璐村洿鏍?鈫?妫€鏌ュ彸渚L53
 *
 * 缁忚繃鍨勮儗绔潰鏃? VL53 娴嬪埌绾?(閫氶亾瀹?2 - 杞﹀/2 - VL53鍐呯缉) 鈮?10cm 鈫?鏈夋晥
 * 缁忚繃鍨勬矡寮€鍙ｆ椂: VL53 灏勫叆娌熷唴 220cm+ 鈫?瓒呭嚭鏈夋晥璺濈 鈫?鏃犳晥鎴栬鏁?> 1.2m
 *
 * 鍥犱负 VL53 鍓嶅悗涓ら闂磋窛 12cm锛岃溅韬?20cm 瀹斤紝娌熷彛 40cm 瀹斤紝
 * 褰撹溅韬腑蹇冨鍑嗘矡鍙ｆ椂锛屽墠鍚?VL53 閮藉凡杩涘叆寮€鍙ｅ尯鍩燂紝涓ら鍧囧簲涓㈠け銆? * 浣嗚繃娓″尯鏈夎竟缂樻晥搴旓紝鎵€浠ュ彧瑕佹眰 "鍓嶅悗鑷冲皯涓€棰椾涪澶? 鍗宠涓烘帰鍒版矡鍙ｃ€? *
 * @param obs     褰撳墠瑙傛祴
 * @param prev_travel_dir  鍒氳蛋瀹岀殑閭ｆ潯娌熺殑琛岄┒鏂瑰悜 (EAST/WEST)
 * @return true = 闈炲洿鏍忎晶妫€娴嬪埌寮€鍙? */
static bool gap_detected_on_open_side(const CorridorObs_t* obs,
                                       TravelDirection_t prev_travel_dir)
{
    if (prev_travel_dir == TRAVEL_DIR_EAST) {
        /* 鍦ㄥ彸绔€氶亾锛屽彸渚ц创鍥存爮 鈫?妫€鏌ュ乏渚?VL53 */
        bool lf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LF)
                       || (obs->d_lf > 0.5f);  /* >50cm 瑙嗕负娌熷彛 (姝ｅ父璐村绾?0cm) */
        bool lr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_LR)
                       || (obs->d_lr > 0.5f);
        return lf_lost || lr_lost;
    } else {
        /* 鍦ㄥ乏绔€氶亾锛屽乏渚ц创鍥存爮 鈫?妫€鏌ュ彸渚?VL53 */
        bool rf_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RF)
                       || (obs->d_rf > 0.5f);
        bool rr_lost = !(obs->valid_mask & CORRIDOR_OBS_MASK_RR)
                       || (obs->d_rr > 0.5f);
        return rf_lost || rr_lost;
    }
}

/**
 * @brief [Phase-1] 鑾峰彇鍥存爮渚?VL53 骞冲潎璺濈
 *
 * 鍦ㄧ閮ㄩ€氶亾鍐咃紝鍥存爮渚х敱鍒氳蛋瀹岀殑娌熺殑琛岄┒鏂瑰悜鍐冲畾锛? *   EAST 鈫?鍦ㄥ彸绔€氶亾 鈫?鍥存爮鍦ㄥ彸渚? *   WEST 鈫?鍦ㄥ乏绔€氶亾 鈫?鍥存爮鍦ㄥ乏渚? *
 * 鍥存爮渚т袱棰?VL53 鍙栧钩鍧囧彲灏?卤2cm 涓綋璇樊闄嶄綆鍒?卤1.4cm銆? * 浠呬竴棰楁湁鏁堟椂涔熻繑鍥炶棰楄鏁帮紙绮惧害绋嶄綆浣嗘湁鑳滀簬鏃狅級銆? *
 * @param obs           褰撳墠瑙傛祴
 * @param travel_dir    鍒氳蛋瀹岀殑娌熺殑琛岄┒鏂瑰悜
 * @param[out] d_avg    鍥存爮渚у钩鍧囪窛绂?(m)
 * @param[out] fence_sign  妯悜鏍℃鏂瑰悜绗﹀彿: +1 = 鍋忕鍥存爮鏃跺簲鍚戝乏杞? -1 = 搴斿悜鍙宠浆
 * @return true = 鑷冲皯涓€棰楀洿鏍忎晶 VL53 鏈夋晥
 */
static bool get_fence_side_distance(const CorridorObs_t* obs,
                                     TravelDirection_t travel_dir,
                                     float* d_avg,
                                     float* fence_sign)
{
    float df, dr;
    bool f_ok, r_ok;

    if (travel_dir == TRAVEL_DIR_EAST) {
        /* 鍙崇閫氶亾 鈫?鍥存爮鍦ㄥ彸渚?涓滀晶X=31) */
        df = obs->d_rf;  dr = obs->d_rr;
        f_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) != 0U;
        r_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RR) != 0U;
        /* 鎺у埗寰? w = fence_sign 脳 kp 脳 (d_actual - d_target)
         * 鍙充晶绂诲洿鏍忓お杩?d_actual > d_target) 鈫?搴斿悜鍙宠浆(-w)闈犺繎
         * 鎵€浠?fence_sign = -1 */
        *fence_sign = -1.0f;
    } else {
        /* 宸︾閫氶亾 鈫?鍥存爮鍦ㄥ乏渚?瑗夸晶X=0) */
        df = obs->d_lf;  dr = obs->d_lr;
        f_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) != 0U;
        r_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_LR) != 0U;
        /* 鎺у埗寰? w = fence_sign 脳 kp 脳 (d_actual - d_target)
         * 宸︿晶绂诲洿鏍忓お杩?d_actual > d_target) 鈫?搴斿悜宸﹁浆(+w)闈犺繎
         * 鎵€浠?fence_sign = +1 */
        *fence_sign = +1.0f;
    }

    if (f_ok && r_ok) {
        *d_avg = (df + dr) * 0.5f;  /* 鍙岄骞冲潎锛屽櫔澹?卤2cm 鈫?卤1.4cm */
        return true;
    } else if (f_ok) {
        *d_avg = df;
        return true;
    } else if (r_ok) {
        *d_avg = dr;
        return true;
    }
    return false;
}

/** 妫€鏌ヤ晶鍚?VL53 鏄惁鍏ㄤ涪 */
static bool all_side_lost(const CorridorObs_t* obs)
{
    uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR |
                        CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR;
    return (obs->valid_mask & side_mask) == 0U;
}

/** 妫€鏌ラ噸鎹曡幏鏉′欢 */
static bool check_reacquire(const CorridorObs_t* obs, const CorridorState_t* state)
{
    /* ----------------------------------------------------------------
     * 鏉′欢 0 (鏂板): 鍚庢縺鍏夊叆娌熺‘璁?     *
     * 鐗╃悊鍘熺悊锛?     *   杞集瀹屾垚鍚庤溅澶存湞鍚戞矡鍐咃紝鍚庢縺鍏夋湞鍚戠閮ㄩ€氶亾绾靛悜鏂瑰悜銆?     *   - 杞︽湭杩囨矡鍙ｏ細鍚庢縺鍏夋祴鍒扮閮ㄩ€氶亾瀵归潰鐨勫洿鏍忥紝璺濈杈冪煭锛堚増20~35cm锛?     *   - 杞﹁韩瓒婅繃娌熷彛锛氬悗婵€鍏夎绾胯繘鍏ユ矡鍐咃紝璺濈浠庤繎绔洿鏍忚烦鍙樺埌娌熼暱
     *     鏂瑰悜锛?20cm+閫氶亾瀹斤級锛岃鏁?>> 40cm 鎴栬秴閲忕▼鐩存帴鏃犳晥
     *
     *   鍥犳 d_back > 闃堝€?鏄?杞﹁韩宸茶秺杩囨矡鍙?鐨勭洿鎺ョ墿鐞嗚瘉鎹紝
     *   鍖哄垎搴︽瀬澶э紙浠?~30cm 璺冲埌 >120cm锛夛紝杩滀紭浜庨噷绋嬭绉垎銆?     *
     * 闃堝€奸€夊彇锛?     *   绔儴閫氶亾瀹?40cm锛岃溅瀹?20cm锛岃溅韬眳涓椂涓や晶鍚?10cm銆?     *   鍚庢縺鍏夊埌瑗夸晶鍥存爮鏈€杩滅害 40cm锛堥€氶亾鍏ㄥ锛夈€?     *   鍙?0.45m 浣滀负闃堝€硷細楂樹簬閫氶亾瀹界殑鏈€澶у€硷紝浣庝簬娌熷唴鏈€鐭祴閲忓€笺€?     *   鑻ュ悗婵€鍏夋棤鏁堬紙瓒呴噺绋嬶級鍒欏悓鏍锋弧瓒虫潯浠讹紙璇绘暟=0涓攊nvalid锛夆€斺€?     *   浣嗘垜浠敤 MASK_BACK 妫€鏌ユ湁鏁堟€э紝鏃犳晥鏃舵鏉′欢涓嶆垚绔嬶紝閫€鍖栦负閲岀▼淇濇姢銆?     * ---------------------------------------------------------------- */
    bool back_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_BACK) != 0U;
    if (back_valid && obs->d_back <= s_nav.cfg.reacquire_back_thresh) {
        /* 鍚庢縺鍏変粛鑳芥祴鍒扮閮ㄩ€氶亾杩戝 鈫?杞﹁韩灏氭湭瓒婅繃娌熷彛 鈫?鎷掔粷 */
        return false;
    }
    /* 鑻ュ悗婵€鍏夋棤鏁堬紙瓒呭嚭閲忕▼锛夆啋 璇存槑瑙嗙嚎宸叉繁鍏ユ矡鍐咃紝瑙嗕负宸插叆娌燂紝璺宠繃姝ら棬妲?*/

    /* 鏉′欢 1: 鑷冲皯 3 涓晶鍚戜紶鎰熷櫒鏈夋晥 */
    uint8_t side_mask = CORRIDOR_OBS_MASK_LF | CORRIDOR_OBS_MASK_LR |
                        CORRIDOR_OBS_MASK_RF | CORRIDOR_OBS_MASK_RR;
    uint8_t active = obs->valid_mask & side_mask;
    int count = 0;
    for (int i = 0; i < 4; i++) {
        if (active & (1U << i)) count++;
    }
    if (count < 3) return false;

    /* 鏉′欢 2: 宸﹀彸璺濈鍜?鈮?璧板粖瀹藉害 */
    bool left_ok  = (obs->valid_mask & CORRIDOR_OBS_MASK_LF) &&
                    (obs->valid_mask & CORRIDOR_OBS_MASK_LR);
    bool right_ok = (obs->valid_mask & CORRIDOR_OBS_MASK_RF) &&
                    (obs->valid_mask & CORRIDOR_OBS_MASK_RR);

    if (left_ok && right_ok) {
        float d_left  = (obs->d_lf + obs->d_lr) * 0.5f;
        float d_right = (obs->d_rf + obs->d_rr) * 0.5f;
        float total_width = d_left + d_right + PARAM_ROBOT_WIDTH;
        float err = gnav_fabsf(total_width - s_nav.cfg.corridor_width);
        if (err > s_nav.cfg.reacquire_width_tol) return false;
    }

    /* 鏉′欢 3: EKF 缃俊搴?*/
    if (state->conf < s_nav.cfg.reacquire_conf_thresh) return false;

    return true;
}

/** 鑾峰彇褰撳墠闃舵鐨勫凡琛岄┒閲岀▼ */
static float odom_since_entry(void)
{
    return s_nav.odom_distance_accum - s_nav.stage_entry_odom_vx_accum;
}

/** 闃舵杞Щ 鈥?閫氱敤鍒濆鍖?*/
static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board);

/* =========================================================
 * 闃舵鍚嶇О琛? * ========================================================= */
static const char* const s_stage_names[] = {
    "IDLE",
    "ENTRY_STRAIGHT",
    "TURN_INTO_CORRIDOR",
    "REACQUIRE",
    "ALIGN",
    "CORRIDOR_TRACK",
    "TURN_OUT",
    "LINK_STRAIGHT",
    "TURN_INTO_NEXT",
    "EXIT_STRAIGHT",
    "DOCK",
    "FINISHED",
    "ERROR"
};

/* =========================================================
 * 杞悜鎵ц (缁熶竴鐨?90掳 杞悜閫昏緫)
 * ========================================================= */
static void execute_turn(const CorridorObs_t* obs,
                         const CorridorState_t* state,
                         const RobotBlackboard_t* board,
                         uint32_t now_ms,
                         GlobalNavOutput_t* out)
{
    /* [鏀硅繘G] IMU 澶辨晥瀹夊叏淇濇姢: 娌℃湁 IMU 鏁版嵁鏃剁珛鍗冲仠杞︼紝涓嶇洸杞€?     * 瓒呮椂鍚庝細琚閮ㄨ秴鏃朵繚鎶ゆ崟鑾凤紝杩涘叆 GNAV_ERROR銆?*/
    if (!board->imu_yaw_continuous.is_valid) {
        out->override_v  = 0.0f;
        out->override_w  = 0.0f;
        out->use_override = true;
        out->request_corridor = false;
        out->safety_mode = SAFETY_MODE_IDLE;
        return;
    }

    float imu_yaw = board->imu_yaw_continuous.value;

    /* 宸茶浆杩囩殑瑙掑害 (鍙栫粷瀵瑰€? */
    float delta = (imu_yaw - s_nav.turn_start_yaw_deg) * s_nav.turn_sign;
    float target = s_nav.turn_target_delta_deg;
    float remaining_deg = target - delta;

    float omega = s_nav.cfg.turn_omega;

    /* 鎺ヨ繎鐩爣鏃跺噺閫?*/
    float decel_zone_deg = PARAM_RAD2DEG(s_nav.cfg.turn_decel_zone_rad);
    if (remaining_deg < decel_zone_deg && decel_zone_deg > 0.01f) {
        float ratio = remaining_deg / decel_zone_deg;
        if (ratio < 0.0f) ratio = 0.0f;
        omega = s_nav.cfg.turn_min_omega +
                ratio * (s_nav.cfg.turn_omega - s_nav.cfg.turn_min_omega);
    }

    out->override_v = 0.0f;
    out->override_w = (float)s_nav.turn_sign * omega;
    out->use_override = true;
    out->request_corridor = false;
    out->safety_mode = SAFETY_MODE_TURN;

    /* 杞悜瀹屾垚鍒ゅ畾 */
    float tolerance_deg = PARAM_RAD2DEG(s_nav.cfg.turn_tolerance_rad);
    if (delta >= target - tolerance_deg) {
        switch (s_nav.stage) {
        case GNAV_TURN_INTO_CORRIDOR:
        case GNAV_TURN_INTO_NEXT:
            transition_to(GNAV_REACQUIRE, board);
            break;
        case GNAV_TURN_OUT_OF_CORRIDOR:
            if (TrackMap_IsLastCorridor(s_nav.current_corridor_id)) {
                transition_to(GNAV_EXIT_STRAIGHT, board);
            } else {
                transition_to(GNAV_LINK_STRAIGHT, board);
            }
            break;
        default:
            break;
        }
        return;
    }

    /* 瓒呮椂淇濇姢 */
    if (now_ms - s_nav.stage_start_ms > s_nav.cfg.turn_timeout_ms) {
        transition_to(GNAV_ERROR, board);
    }
}

/* =========================================================
 * 闃舵杞Щ
 * ========================================================= */
static void transition_to(GlobalNavStage_t next, const RobotBlackboard_t* board)
{
    float imu_yaw = (board != NULL && board->imu_yaw_continuous.is_valid)
                    ? board->imu_yaw_continuous.value : 0.0f;

    /* 閫氱敤: 璁板綍杩涘叆鏃堕棿鍜岄噷绋?*/
    s_nav.stage_start_ms = s_last_update_ms;
    s_nav.stage_entry_odom_vx_accum = s_nav.odom_distance_accum;
    s_nav.reacquire_ok_count = 0;

    switch (next) {
    case GNAV_ENTRY_STRAIGHT:
        s_nav.heading_ref_deg = imu_yaw;
        s_nav.current_corridor_id = TrackMap_Get()->entry_corridor_id;
        break;

    case GNAV_TURN_INTO_CORRIDOR: {
        const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
        s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
        s_nav.turn_start_yaw_deg = imu_yaw;
        s_nav.turn_target_delta_deg = 90.0f;
        break;
    }
    case GNAV_TURN_OUT_OF_CORRIDOR: {
        const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
        s_nav.turn_sign = (int8_t)cd->exit_turn_dir;
        s_nav.turn_start_yaw_deg = imu_yaw;
        s_nav.turn_target_delta_deg = 90.0f;
        break;
    }
    case GNAV_TURN_INTO_NEXT: {
        uint8_t next_id = TrackMap_GetNextCorridorId(s_nav.current_corridor_id);
        const CorridorDescriptor_t* cd = TrackMap_GetCorridor(next_id);
        s_nav.turn_sign = (int8_t)cd->entry_turn_dir;
        s_nav.turn_start_yaw_deg = imu_yaw;
        s_nav.turn_target_delta_deg = 90.0f;
        s_nav.current_corridor_id = next_id;
        break;
    }
    case GNAV_REACQUIRE:
        /* EKF 閲嶇疆: 鏂版矡鐨?e_y 鍙傝€冧笉鍚岋紝蹇呴』閲嶅缓 */
        CorridorFilter_Reset();
        s_nav.heading_ref_deg = imu_yaw;
        break;

    case GNAV_ALIGN:
        /* 鍋滆溅瀵规锛氭竻闆惰鏁板櫒锛孍KF 姝ゆ椂宸叉湁鍒濇浼拌 */
        s_nav.align_ok_count = 0;
        break;

    case GNAV_CORRIDOR_TRACK:
        s_nav.corridor_entry_s = 0.0f;  /* EKF 宸?reset, s 浠?0 寮€濮?*/
        break;

    case GNAV_LINK_STRAIGHT:
        s_nav.heading_ref_deg = imu_yaw;
        s_nav.link_d_front_start = 0.0f;
        s_nav.link_d_front_valid = false;  /* 棣栨媿鍐嶈褰?*/
        s_nav.link_gap_count = 0;
        break;

    case GNAV_EXIT_STRAIGHT:
        s_nav.heading_ref_deg = imu_yaw;
        s_nav.exit_vl53_lost = false;
        s_nav.exit_lost_distance = 0.0f;
        break;

    case GNAV_DOCK:
        break;

    case GNAV_FINISHED:
        s_nav.running = false;
        break;

    case GNAV_ERROR:
        break;

    default:
        break;
    }

    s_nav.stage = next;
}

/* =========================================================
 * 鍏紑 API
 * ========================================================= */

void GlobalNav_Init(const GlobalNavConfig_t* cfg)
{
    memset(&s_nav, 0, sizeof(s_nav));
    s_nav.cfg = *cfg;
    s_nav.stage = GNAV_IDLE;
    s_nav.running = false;
    s_nav.initialized = true;
    s_last_odom_vx = 0.0f;
    s_last_update_ms = 0;
}

void GlobalNav_Start(void)
{
    if (!s_nav.initialized) return;
    s_nav.running = true;
    s_nav.corridors_completed = 0;
    s_nav.odom_distance_accum = 0.0f;
    s_nav.stage_entry_odom_vx_accum = 0.0f;
    s_nav.current_corridor_id = TrackMap_Get()->entry_corridor_id;
    /* 涓嶅湪杩欓噷 transition_to锛屽洜涓鸿繕娌℃湁 board 鏁版嵁銆?       涓嬩竴涓?Update 鍛ㄦ湡閲屽彂鐜?running && IDLE 鏃跺啀 transition銆?*/
    s_nav.stage = GNAV_IDLE;
}

void GlobalNav_Stop(void)
{
    s_nav.running = false;
    s_nav.stage = GNAV_FINISHED;
}

void GlobalNav_Reset(void)
{
    s_nav.stage = GNAV_IDLE;
    s_nav.running = false;
    s_nav.corridors_completed = 0;
    s_nav.odom_distance_accum = 0.0f;
}

GlobalNavStage_t GlobalNav_GetStage(void)
{
    return s_nav.stage;
}

const char* GlobalNav_GetStageName(GlobalNavStage_t stage)
{
    if (stage <= GNAV_ERROR) {
        return s_stage_names[stage];
    }
    return "UNKNOWN";
}

/* =========================================================
 * 鏍稿績 Update
 * ========================================================= */
void GlobalNav_Update(const CorridorObs_t*      obs,
                      const CorridorState_t*     state,
                      const RobotBlackboard_t*   board,
                      uint32_t                   now_ms,
                      GlobalNavOutput_t*         out)
{
    if (!s_nav.initialized) {
        memset(out, 0, sizeof(*out));
        return;
    }

    /* 閲岀▼璁＄Н鍒? 螖d = vx * dt
     * now_ms 鐢辫皟鐢ㄦ柟浼犲叆 (HAL_GetTick)锛屼笌浠讳綍浼犳劅鍣ㄦ椂闂存埑鏃犲叧锛?     * 閬垮厤 IMU 鏃堕棿鎴冲仠鏇存椂閲岀▼鍜岃秴鏃跺弻鍙屽喕缁?(鍘?TODO-1 淇) */
    {
        float odom_vx = board->odom_vx;
        if (s_last_update_ms > 0) {
            float dt = (float)(now_ms - s_last_update_ms) * 0.001f;
            if (dt > 0.0f && dt < 0.5f) {
                s_nav.odom_distance_accum += gnav_fabsf(odom_vx) * dt;
            }
        }
        s_last_update_ms = now_ms;
        s_last_odom_vx = odom_vx;
    }

    /* [鏀硅繘G] IMU yaw 鎻愬彇: 澶辨晥鏃朵娇鐢ㄥ弬鑰冨€硷紝淇濇寔鑸悜涓嶅彉鑰屼笉鏄烦鍒?0 */
    float imu_yaw_deg = board->imu_yaw_continuous.is_valid
                        ? board->imu_yaw_continuous.value : s_nav.heading_ref_deg;

    /* 榛樿杈撳嚭 */
    out->use_override     = true;
    out->request_corridor = false;
    out->override_v       = 0.0f;
    out->override_w       = 0.0f;
    out->safety_mode      = SAFETY_MODE_IDLE;
    out->stage            = s_nav.stage;
    out->corridor_id      = s_nav.current_corridor_id;
    out->corridors_done   = s_nav.corridors_completed;
    out->active           = s_nav.running;
    out->stage_name       = GlobalNav_GetStageName(s_nav.stage);

    if (!s_nav.running) return;

    /* IDLE 鈫?鑷姩杩涘叆 ENTRY_STRAIGHT */
    if (s_nav.stage == GNAV_IDLE) {
        transition_to(GNAV_ENTRY_STRAIGHT, board);
        out->stage      = s_nav.stage;
        out->stage_name = GlobalNav_GetStageName(s_nav.stage);
    }

    uint32_t elapsed_ms = now_ms - s_nav.stage_start_ms;

    switch (s_nav.stage) {

    /* ============================================================
     * 鍏ュ満鐩寸嚎 (浠庡惎鍔ㄥ尯娌垮乏绔旱鍚戦€氶亾鍖楄)
     *
     * 浼犳劅鍣ㄦ儏鍐碉細
     *   - 宸︿晶 VL53 璐村洿鏍忥紝濮嬬粓鏈夋晥 鈫?鐢ㄤ簬妯悜璐村鎺у埗
     *   - 鍙充晶 VL53 涓€鍑哄惎鍔ㄥ尯灏卞鐫€C1寮€鍙?260cm)锛屾祴涓嶅埌
     *   - 鍏ュ満璺濈鏋佺煭锛堝惎鍔ㄥ尯鍏ュ彛鍒癈1鍏ュ彛浠呯害 10~30cm锛?     *   - 涓昏闈犻噷绋嬭鎺ㄨ繘瓒冲璺濈鍚庡嵆鍙浆鍚?     * ============================================================ */
    case GNAV_ENTRY_STRAIGHT: {
        out->override_v  = s_nav.cfg.entry_v;
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        /* 鍏ュ満娈靛湪宸︾閫氶亾鍖楄锛氬乏渚ц创鍥存爮锛屼笌 LINK_STRAIGHT 鐩稿悓閫昏緫
         * ENTRY 鏂瑰悜绛夋晥浜?TRAVEL_DIR_WEST锛堝湪宸︾閫氶亾锛屽洿鏍忓湪宸︿晶锛?/
        {
            float w_imu = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
                                           s_nav.cfg.heading_kp);
            float d_fence_avg = 0.0f;
            float fence_sign  = 0.0f;
            bool  fence_valid = get_fence_side_distance(obs, TRAVEL_DIR_WEST,
                                                         &d_fence_avg, &fence_sign);
            if (fence_valid) {
                float ey_wall = d_fence_avg - s_nav.cfg.link_wall_target_d;
                float w_wall  = fence_sign * s_nav.cfg.link_wall_kp_y * ey_wall;
                w_wall = gnav_clampf(w_wall, -1.0f, 1.0f);
                float blend = s_nav.cfg.link_wall_blend;
                out->override_w = blend * w_wall + (1.0f - blend) * w_imu;
            } else {
                out->override_w = w_imu;
            }
            out->override_w = gnav_clampf(out->override_w, -1.0f, 1.0f);
        }

        /* 閲岀▼璁?+ 瓒呮椂鍒ゅ畾 */
        if (odom_since_entry() >= s_nav.cfg.entry_distance ||
            elapsed_ms > s_nav.cfg.entry_timeout_ms)
        {
            transition_to(GNAV_TURN_INTO_CORRIDOR, board);
        }
        break;
    }

    /* ============================================================
     * 涓夌杞悜鐘舵€佺粺涓€澶勭悊
     * ============================================================ */
    case GNAV_TURN_INTO_CORRIDOR:
    case GNAV_TURN_OUT_OF_CORRIDOR:
    case GNAV_TURN_INTO_NEXT:
        execute_turn(obs, state, board, now_ms, out);
        break;

    /* ============================================================
     * 閲嶆崟鑾疯蛋寤?     *
     * 闂鑳屾櫙锛?     *   杞集鍒氬畬鎴愭椂锛岃溅韬彲鑳借繕鍦ㄦ矡鍙ｅ锛堢閮ㄩ€氶亾鍐咃級锛?     *   浣嗘鏃朵袱渚L53鍚屾牱鑳芥祴鍒颁袱渚у瀯鑳岀闈?鈮?0cm)锛?     *   瀹藉害鍜?= 10+10+20 = 40cm锛屼笌鐪熸鍦ㄦ矡鍐呭畬鍏ㄤ竴鑷淬€?     *   杩欏鑷村皻鏈叆娌熷氨婊¤冻閲嶆崟鑾锋潯浠讹紝鍒囧埌CORRIDOR_TRACK鍚?     *   EKF鐨別_y鍙兘鏄ぇ鍋忓樊锛宑orridor_ctrl杈撳嚭澶锛岃溅鍗℃鍦ㄥ叆鍙ｃ€?     *
     * 淇鏂规锛?     *   寮曞叆 reacquire_min_depth 闂ㄦ锛?     *   鍙湁閲岀▼璁℃樉绀鸿溅韬凡娣卞叆娌熷唴瓒冲璺濈鍚庯紝鎵嶅厑璁稿紑濮嬭鏁般€?     *   杩欑‘淇濆洓棰梀L53鍏ㄩ儴鑴辩娌熷彛杈圭紭杩涘叆绋冲畾渚у鍖哄煙銆?     * ============================================================ */
    case GNAV_REACQUIRE:
        out->override_v  = s_nav.cfg.reacquire_v;
        out->override_w  = 0.0f;  /* 涓嶅仛鑸悜鎺у埗锛岃杞﹁嚜鐒惰繘娌燂紝ALIGN闃舵鍐嶆憜姝?*/
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        {
            /* 鍏ユ矡娣卞害瀹堝崼锛氭湭杈惧埌鏈€灏忔繁搴﹀墠锛岀姝㈠紑濮嬬‘璁よ鏁?*/
            bool depth_ok = (odom_since_entry() >= s_nav.cfg.reacquire_min_depth);

            if (depth_ok && check_reacquire(obs, state)) {
                s_nav.reacquire_ok_count++;
            } else {
                /* 娣卞害涓嶈冻 鎴?鏉′欢鏈弧瓒?鈫?璁℃暟涓ユ牸娓呴浂
                 * 娉ㄦ剰锛氭繁搴︿笉瓒虫椂涔熸竻闆讹紝闃叉鍦ㄦ矡鍙ｅ绉疮浜嗛儴鍒嗚鏁?                 * 鐒跺悗杩涙矡鍚庡彧闇€寰堝皯鍑犳媿灏辫Е鍙?*/
                s_nav.reacquire_ok_count = 0;
            }
        }

        if (s_nav.reacquire_ok_count >= s_nav.cfg.reacquire_confirm_ticks) {
            /* 閲嶆崟鑾风‘璁?鈫?杩涘叆鍘熷湴瀵规闃舵锛屽仠杞︾敤VL53鎽嗘鍚庡啀鍓嶈繘 */
            transition_to(GNAV_ALIGN, board);
        }
        if (elapsed_ms > s_nav.cfg.reacquire_timeout_ms) {
            transition_to(GNAV_ERROR, board);
        }
        break;

    /* ============================================================
     * 鍘熷湴瀵规 (ALIGN)
     *
     * 瑙﹀彂鏃舵満锛歊EACQUIRE 纭鎴愬姛鍚庯紝鍋滆溅鍘熷湴鐢?EKF 鎽嗘杞﹁韩鍐嶅墠杩涖€?     *
     * 鎺у埗寰嬶細v = 0锛寃 = -(kp_th路e_th + kp_y路e_y)
     *
     * 涓轰粈涔堢敤 EKF 鐨?e_th/e_y 鑰屼笉鏄師濮?VL53 宸垎锛?     *   鍘熷 VL53 鍓嶅悗宸及瑙掞細卤2cm / 12cm 鍩虹嚎 鈮?卤18掳 鍣０锛屽畬鍏ㄤ笉鍙敤銆?     *   EKF 鐨?e_th 鏄?IMU 楂樼簿搴︾Н鍒?+ 渚у妯悜鑱斿悎绾︽潫鐨勪骇鐗╋紝鍙俊銆?     *
     * 瀹屾垚鏉′欢锛殀e_th| < th_tol AND |e_y| < y_tol锛岃繛缁?N 鎷嶃€?     * 瓒呮椂锛氱洿鎺ヨ繘娌燂紝涓嶉樆濉炴祦绋嬶紙瓒呮椂璇存槑VL53璐ㄩ噺宸紝浣嗕篃瑕佺户缁級銆?     * ============================================================ */
    case GNAV_ALIGN: {
        out->override_v  = 0.0f;   /* 鍋滆溅 */
        out->use_override = true;
        out->request_corridor = false;
        out->safety_mode = SAFETY_MODE_TURN;  /* TURN妯″紡锛氬厑璁稿師鍦版棆杞紝璺宠繃鍓嶅悜纰版挒妫€鏌?*/

        /* 瀵规鎺у埗寰嬶細浠呰閫熷害锛屾棤绾块€熷害 */
        float w_align = -(s_nav.cfg.align_kp_th * state->e_th
                        + s_nav.cfg.align_kp_y  * state->e_y);
        out->override_w = gnav_clampf(w_align, -1.0f, 1.0f);

        /* 瀹屾垚鍒ゅ畾锛氬Э鎬佸拰浣嶇疆閮藉湪瀹瑰樊鍐?*/
        bool th_ok = gnav_fabsf(state->e_th) < s_nav.cfg.align_th_tol_rad;
        bool y_ok  = gnav_fabsf(state->e_y)  < s_nav.cfg.align_y_tol_m;
        bool ekf_trusted = (state->conf >= s_nav.cfg.reacquire_conf_thresh);

        if (th_ok && y_ok && ekf_trusted) {
            s_nav.align_ok_count++;
        } else {
            s_nav.align_ok_count = 0;
        }

        if (s_nav.align_ok_count >= s_nav.cfg.align_confirm_ticks) {
            /* 瀵规瀹屾垚锛岄『渚跨敤姝ゆ椂鐨?e_th 鏇存柊 IMU 鍙傝€冭 */
            s_nav.heading_ref_deg += PARAM_RAD2DEG(state->e_th);
            transition_to(GNAV_CORRIDOR_TRACK, board);
        }

        /* 瓒呮椂淇濇姢锛氫笉姝荤瓑锛岀洿鎺ヨ繘娌?*/
        if (elapsed_ms > s_nav.cfg.align_timeout_ms) {
            transition_to(GNAV_CORRIDOR_TRACK, board);
        }
        break;
    }

    /* ============================================================
     * 娌熷唴闂幆璺熻釜 (浜ょ粰 corridor_ctrl)
     * ============================================================ */
    case GNAV_CORRIDOR_TRACK:
        out->use_override     = false;
        out->request_corridor = true;
        out->safety_mode      = SAFETY_MODE_CORRIDOR;

        /* [Phase-3] 娌熷唴 IMU 鍙傝€冩柟鍚戞參鏍℃
         *
         * 鍘熺悊锛欵KF 鐨?e_th 鍙嶆槧浜嗗綋鍓嶈埅鍚戠浉瀵逛簬 IMU 鍙傝€冭鐨勫亸宸€?         * 濡傛灉 e_th 闀挎湡涓嶄负闆讹紙渚嬪鎸佺画 +2掳锛夛紝璇存槑 IMU 鍙傝€冭鏈夌郴缁熸€у亸宸€?         * 鎺у埗鍣ㄨ櫧鐒惰兘閫氳繃 kp_theta 鎶?e_th 鍘嬩笅鍘伙紝浣嗚繖鎰忓懗鐫€鎺у埗鍣ㄤ竴鐩村湪
         * "鎶楀亸宸?锛岃溅澶存寔缁亸鍚戜竴渚э紝妯悜浼氱紦鎱㈡紓绉汇€?         *
         * 瑙ｅ喅鏂规硶锛氭瘡鎷嶇敤鏋佸皬鐨?alpha 灏?e_th 鍙嶉鍒?heading_ref_deg 涓婏紝
         * 璁╁弬鑰冭缂撴參瀵归綈鐪熷疄娌熻酱銆?         *
         * 涓轰粈涔堜笉鐢ㄤ晶澧欏墠鍚庡樊锛圴L53 宸垎娴嬭锛夛細
         *   VL53 鏈?卤2cm 涓綋璇樊锛屽熀绾?12cm
         *   鏈€鍧忓櫔澹?= atan2(4cm, 12cm) 鈮?18掳
         *   鍗充娇鍙栧钩鍧囦篃鏈?~9掳 鍣０锛屼笉閫傚悎鍋氳搴︿及璁?         *
         * 涓轰粈涔堢敤 EKF e_th锛?         *   e_th 鏄?IMU 楂樼簿搴︾Н鍒?+ 渚у妯悜瑙傛祴鑱斿悎绾︽潫鐨勪骇鐗?         *   濡傛灉鍙屼晶瑙傛祴绋冲畾锛坈onf 楂橈級锛宔_th 鐨勮秼鍔挎槸鍙俊鐨?         *
         * 闂ㄦ帶鏉′欢锛?         *   - EKF conf >= corridor_ref_correct_conf锛堥粯璁?0.75锛?         *   - alpha 鏋佸皬锛?.001锛夛紝鏃堕棿甯告暟 ~1s锛屼笉浼氳鐬椂鍣０甯﹁窇
         */
        if (s_nav.cfg.corridor_ref_correct_alpha > 0.0f &&
            state->conf >= s_nav.cfg.corridor_ref_correct_conf)
        {
            float eth_deg = PARAM_RAD2DEG(state->e_th);
            s_nav.heading_ref_deg += s_nav.cfg.corridor_ref_correct_alpha * eth_deg;
        }

        /* 鍒扮妫€娴?*/
        {
            bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
            if (front_valid && obs->d_front <= s_nav.cfg.corridor_end_detect_dist) {
                /* 閲岀▼涓嬮檺淇濇姢: 鑷冲皯璧颁簡 1.0m 鎵嶅厑璁歌瀹氬埌绔紝閬垮厤鍋囬槼鎬?*/
                float corridor_odom = odom_since_entry();
                if (corridor_odom > 1.0f) {
                    s_nav.corridors_completed++;
                    transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
                }
            }
        }
        /* 閲岀▼瓒呴暱淇濇姢 */
        if (odom_since_entry() > s_nav.cfg.corridor_length_max) {
            s_nav.corridors_completed++;
            transition_to(GNAV_TURN_OUT_OF_CORRIDOR, board);
        }
        break;

    /* ============================================================
     * 杩炴帴娈电洿琛?(绾靛悜绔儴閫氶亾鍖楄, 鏂规2: 涓変俊鍙疯仈鍚堝垽瀹?
     *
     * 浼犳劅鍣ㄦ儏鍐碉紙杞悜瀹屾垚鍚庨潰鏈濆寳锛夛細
     *   - 鍓嶆縺鍏夛紙鏈濆寳锛夆啋 涓婂洿鏍忥紝d_front 闅忓寳琛岄€掑噺      [绮惧害楂榏
     *   - 鍚庢縺鍏夛紙鏈濆崡锛夆啋 涓嬪洿鏍忥紝d_back 闅忓寳琛岄€掑       [绮惧害楂榏
     *   - 鍥存爮渚?VL53 鈫?濮嬬粓鏈夋晥 (~10cm)                [涓嶇敤浜庡垽瀹歖
     *   - 闈炲洿鏍忎晶 VL53 鈫?璐村瀯鑳岀闈㈡椂鏈夋晥锛屽埌鍨勬矡寮€鍙ｆ椂涓㈠け  [娌熷彛鏍囧織]
     *
     * 淇″彿瀹氫箟锛?     *   A: 閲岀▼璁?    odom >= link_distance * 0.85  (鎵撴粦琛板噺, 鏉冮噸浣?
     *   B: 鍓嶆縺鍏夊彉鍖? d_front缂╁皬 >= link_distance * 0.85 (鏉冮噸楂?
     *   C: 闈炲洿鏍忎晶VL53 涓㈠け/璺冲埌>50cm锛岃繛缁?鎷嶇‘璁? (鐩存帴鎺㈡祴娌熷彛, 鏉冮噸涓?
     *
     * 瑙﹀彂閫昏緫: B || (A && C)
     *   - 鍓嶆縺鍏夊彉鍖栭噺瓒冲 鈫?鐩存帴瑙﹀彂锛堟渶鍙潬鐨勫崟涓€淇″彿锛?     *   - 閲岀▼璁″埌浣?+ VL53鎺㈠埌娌熷彛 鈫?鑱斿悎瑙﹀彂锛堜簰鐩告牎楠岋級
     * ============================================================ */
    case GNAV_LINK_STRAIGHT:
        out->override_v  = s_nav.cfg.link_v;
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        /* [Phase-1] 鍥存爮渚у崟杈硅创澧?+ IMU 娣峰悎鎺у埗
         *
         * 鍘熸潵锛氱函 IMU heading_hold_pd()
         * 鐜板湪锛氬鏋滃洿鏍忎晶 VL53 鏈夋晥锛岀敤鍥存爮璺濈鍋氭í鍚戠害鏉燂紝涓?IMU 娣峰悎
         *       濡傛灉鍥存爮渚?VL53 鍏ㄩ儴鏃犳晥锛岄€€鍖栦负鍘熸潵鐨勭函 IMU
         *
         * 涓轰粈涔堜笉鐢ㄥ洿鏍忓墠鍚庡樊鍋氳埅鍚戠害鏉燂細
         *   VL53 鏈?卤2cm 涓綋璇樊锛屽熀绾夸粎 12cm 鈫?atan2(卤4cm, 12cm) = 卤18掳
         *   鍣０澶ぇ锛屼笉閫傚悎鍋氳埅鍚戞帶鍒躲€傚彧鐢ㄥ钩鍧囪窛绂诲仛妯悜绾︽潫銆?         */
        {
            float w_imu = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
                                           s_nav.cfg.heading_kp);

            const CorridorDescriptor_t* cd = TrackMap_GetCorridor(s_nav.current_corridor_id);
            float d_fence_avg = 0.0f;
            float fence_sign  = 0.0f;
            bool  fence_valid = get_fence_side_distance(obs, cd->travel_dir,
                                                         &d_fence_avg, &fence_sign);

            if (fence_valid) {
                /* 妯悜鍋忓樊 = 瀹為檯璺濈 - 鐩爣璺濈
                 * 姝ｅ€?= 绂诲洿鏍忓お杩?鈫?fence_sign 鍐冲畾鏍℃鏂瑰悜 */
                float ey_wall = d_fence_avg - s_nav.cfg.link_wall_target_d;
                float w_wall  = fence_sign * s_nav.cfg.link_wall_kp_y * ey_wall;
                w_wall = gnav_clampf(w_wall, -1.0f, 1.0f);

                /* 娣峰悎锛氬洿鏍忕害鏉熶负涓?+ IMU 骞虫粦涓鸿緟 */
                float blend = s_nav.cfg.link_wall_blend;
                out->override_w = blend * w_wall + (1.0f - blend) * w_imu;
            } else {
                /* 鍥存爮渚?VL53 鍏ㄩ儴鏃犳晥 鈫?閫€鍖栦负绾?IMU锛堜笌鍘熻涓轰竴鑷达級 */
                out->override_w = w_imu;
            }

            out->override_w = gnav_clampf(out->override_w, -1.0f, 1.0f);

            bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;

            /* 棣栨媿璁板綍鍓嶆縺鍏夊熀鍑嗗€?*/
            if (!s_nav.link_d_front_valid && front_valid) {
                s_nav.link_d_front_start = obs->d_front;
                s_nav.link_d_front_valid = true;
            }

            /* ---- 淇″彿 A: 閲岀▼璁?(鑰冭檻鎵撴粦, 鐢?85% 瀹瑰樊) ---- */
            bool odom_ok = odom_since_entry() >= s_nav.cfg.link_distance * 0.85f;

            /* ---- 淇″彿 B: 鍓嶆縺鍏夊彉鍖栭噺 (楂樼簿搴? ---- */
            bool laser_ok = false;
            if (s_nav.link_d_front_valid && front_valid) {
                float d_front_delta = s_nav.link_d_front_start - obs->d_front;
                /* 杞﹁韩涓績鍒板墠婵€鍏夋湁鍋忕疆(FRONT_LASER_OFFSET)锛屼絾杩欓噷鐢ㄧ殑鏄樊鍊硷紝鍋忕疆鎶垫秷 */
                laser_ok = (d_front_delta >= s_nav.cfg.link_distance * 0.85f);
            }

            /* ---- 淇″彿 C: 闈炲洿鏍忎晶 VL53 娌熷彛妫€娴?(闇€杩炵画2鎷嶇‘璁? ----
             *
             * 鍒ゅ畾闃堝€?0.5m 鐨勬潵婧?
             *   姝ｅ父璐村瀯鑳岀闈㈡椂 VL53 璇绘暟 鈮?(閫氶亾瀹?2 - 杞﹀/2 - VL53鍐呯缉)
             *                               = (0.40/2 - 0.20/2 - 0.0)
             *                               = 0.10m
             *   鍒板瀯娌熷紑鍙ｆ椂 VL53 璇绘暟 > 1.2m (瓒呭嚭鏈夋晥璺濈) 鎴栨棤鏁?             *   闃堝€?0.5m 鍦ㄤ袱鑰呬箣闂达紝瓒冲鍖哄垎
             */
            /* cd 宸插湪涓婃柟鍥存爮渚ч€昏緫涓幏鍙栵紝澶嶇敤 */
            /* 鍦?LINK_STRAIGHT 闃舵锛宑urrent_corridor_id 浠嶆槸鍒氳蛋瀹岀殑娌?(灏氭湭鏇存柊)
               鎵€浠?cd->travel_dir 灏辨槸鍒氳蛋瀹岄偅鏉℃矡鐨勬柟鍚戯紝鐩存帴鐢ㄦ潵鍒ゆ柇褰撳墠鍦ㄥ摢涓閮ㄩ€氶亾 */
            bool gap_now = gap_detected_on_open_side(obs, cd->travel_dir);

            if (gap_now) {
                if (s_nav.link_gap_count < 255) s_nav.link_gap_count++;
            } else {
                s_nav.link_gap_count = 0;
            }
            bool gap_confirmed = (s_nav.link_gap_count >= 2);  /* 杩炵画2鎷?(40ms @ 20ms鍛ㄦ湡) */

            /* ---- 鑱斿悎鍒ゅ畾: B || (A && C) ---- */
            if (laser_ok || (odom_ok && gap_confirmed))
            {
                transition_to(GNAV_TURN_INTO_NEXT, board);
            }
        }
        if (elapsed_ms > s_nav.cfg.link_timeout_ms) {
            transition_to(GNAV_ERROR, board);
        }
        break;

    /* ============================================================
     * 鍑哄満鐩磋
     * ============================================================ */
    case GNAV_EXIT_STRAIGHT:
        out->override_v  = s_nav.cfg.exit_v;
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        /* [Phase-1] 鍑哄満娈典篃浣跨敤鍥存爮渚ц创澧欐帶鍒?         * C6 鍑烘矡鍚庡湪宸︾閫氶亾鍗楄锛屽洿鏍忓湪宸︿晶锛堜笌 LINK_STRAIGHT 鍏辩敤閫昏緫锛?         * 璺濈 ~3.9m锛屾瘮杩炴帴娈甸暱寰楀锛屽洿鏍忕害鏉熸洿鏈変环鍊?         */
        {
            float w_imu = heading_hold_pd(imu_yaw_deg, s_nav.heading_ref_deg,
                                           s_nav.cfg.heading_kp);

            const CorridorDescriptor_t* cd_exit = TrackMap_GetCorridor(s_nav.current_corridor_id);
            float d_fence_avg = 0.0f;
            float fence_sign  = 0.0f;
            bool  fence_valid = get_fence_side_distance(obs, cd_exit->travel_dir,
                                                         &d_fence_avg, &fence_sign);

            if (fence_valid && !s_nav.exit_vl53_lost) {
                float ey_wall = d_fence_avg - s_nav.cfg.link_wall_target_d;
                float w_wall  = fence_sign * s_nav.cfg.link_wall_kp_y * ey_wall;
                w_wall = gnav_clampf(w_wall, -1.0f, 1.0f);
                float blend = s_nav.cfg.link_wall_blend;
                out->override_w = blend * w_wall + (1.0f - blend) * w_imu;
            } else {
                out->override_w = w_imu;
            }
            out->override_w = gnav_clampf(out->override_w, -1.0f, 1.0f);
        }

        /* 妫€娴嬩晶鍚戝叏涓?*/
        if (!s_nav.exit_vl53_lost && all_side_lost(obs)) {
            s_nav.exit_vl53_lost = true;
            s_nav.exit_lost_distance = s_nav.odom_distance_accum;
        }
        if (s_nav.exit_vl53_lost) {
            float since_lost = s_nav.odom_distance_accum - s_nav.exit_lost_distance;
            if (since_lost >= s_nav.cfg.exit_runout) {
                transition_to(GNAV_DOCK, board);
            }
        }
        /* 閲岀▼涓婇檺淇濇姢 */
        if (odom_since_entry() >= s_nav.cfg.exit_max_dist) {
            transition_to(GNAV_DOCK, board);
        }
        if (elapsed_ms > s_nav.cfg.exit_timeout_ms) {
            transition_to(GNAV_DOCK, board);
        }
        break;

    /* ============================================================
     * 鍥炲仠鍚姩鍖?     * ============================================================ */
    case GNAV_DOCK:
        out->override_v  = s_nav.cfg.dock_v;
        out->override_w  = 0.0f;
        out->safety_mode = SAFETY_MODE_STRAIGHT;

        if (odom_since_entry() >= s_nav.cfg.dock_distance ||
            elapsed_ms > 5000U)
        {
            transition_to(GNAV_FINISHED, board);
        }
        break;

    /* ============================================================
     * 缁堟€?     * ============================================================ */
    case GNAV_FINISHED:
        out->override_v = 0.0f;
        out->override_w = 0.0f;
        out->safety_mode = SAFETY_MODE_IDLE;
        out->active = false;
        break;

    /* ============================================================
     * 寮傚父鎬?     * ============================================================ */
    case GNAV_ERROR:
        out->override_v = 0.0f;
        out->override_w = 0.0f;
        out->safety_mode = SAFETY_MODE_IDLE;

        if (elapsed_ms > 2000U) {
            transition_to(GNAV_FINISHED, board);
        }
        break;

    default:
        out->override_v = 0.0f;
        out->override_w = 0.0f;
        break;
    }

    /* 鏇存柊杈撳嚭闃舵 (鍙兘鍦?switch 鍐呭凡缁?transition) */
    out->stage       = s_nav.stage;
    out->stage_name  = GlobalNav_GetStageName(s_nav.stage);
    out->corridor_id = s_nav.current_corridor_id;
    out->corridors_done = s_nav.corridors_completed;
    out->active      = s_nav.running;
}