#include "segment_fsm.h"
#include <string.h>

/* ====================== 内部状态 ====================== */
static SegFsmConfig_t  s_cfg;
static SegFsmState_t   s_state = SEG_STATE_IDLE;
static bool            s_initialized = false;

/* 辅助：浮点数限幅 */
static inline float seg_clampf(float val, float lo, float hi)
{
    if (val < lo) return lo;
    if (val > hi) return hi;
    return val;
}

void SegFsm_Init(const SegFsmConfig_t *config)
{
    s_cfg = *config;
    s_state = SEG_STATE_IDLE;
    s_initialized = true;
}

void SegFsm_Start(void)
{
    if (s_initialized) {
        s_state = SEG_STATE_CORRIDOR;
    }
}

SegFsmState_t SegFsm_GetState(void)
{
    return s_state;
}

const char* SegFsm_GetStateName(SegFsmState_t s)
{
    switch (s) {
        case SEG_STATE_IDLE:      return "IDLE";
        case SEG_STATE_CORRIDOR:  return "CORRIDOR";
        case SEG_STATE_APPROACH:  return "APPROACH";
        case SEG_STATE_STOP:      return "STOP";
        case SEG_STATE_ESTOP:     return "E-STOP";
        default:                  return "UNKNOWN";
    }
}

void SegFsm_Update(const RawCmd_t *raw_cmd,
                   const CorridorObs_t *obs,
                   const CorridorState_t *state,
                   SegFsmOutput_t *out)
{
    if (!s_initialized) {
        out->state  = SEG_STATE_IDLE;
        out->safe_v = 0.0f;
        out->safe_w = 0.0f;
        return;
    }

    /* ========================================================
     * 1. 全局安全断言：无论在什么状态，满足条件直接超驰
     * ======================================================== */

    /* 1a. 置信度极低 → 紧急停车 */
    if (state->conf < s_cfg.conf_estop_thresh) {
        s_state = SEG_STATE_ESTOP;
    }

    /* ========================================================
     * 2. 待命状态：什么都不做
     * ======================================================== */
    if (s_state == SEG_STATE_IDLE) {
        out->state  = SEG_STATE_IDLE;
        out->safe_v = 0.0f;
        out->safe_w = 0.0f;
        return;
    }

    /* ========================================================
     * 3. 紧急停车状态：等待置信度恢复
     * ======================================================== */
    if (s_state == SEG_STATE_ESTOP) {
        out->state  = SEG_STATE_ESTOP;
        out->safe_v = 0.0f;
        out->safe_w = 0.0f;

        /* 自动恢复开关：当置信度重新回到安全区且前方有路，恢复巡航 */
        if (state->conf >= 0.5f) {
            s_state = SEG_STATE_CORRIDOR;
        }
        return;
    }

    /* ========================================================
     * 4. 前向距离判定：决定 CORRIDOR / APPROACH / STOP
     * ======================================================== */
    bool front_valid = (obs->valid_mask & CORRIDOR_OBS_MASK_FRONT) != 0U;
    float d_front = obs->d_front;

    if (front_valid) {
        if (d_front <= s_cfg.d_front_stop) {
            /* 距离过近：到端停车 */
            s_state = SEG_STATE_STOP;
        } else if (d_front <= s_cfg.d_front_approach) {
            /* 进入减速预警区 */
            s_state = SEG_STATE_APPROACH;
        } else {
            /* 前方安全：正常走廊跟踪 */
            if (s_state == SEG_STATE_STOP || s_state == SEG_STATE_APPROACH) {
                s_state = SEG_STATE_CORRIDOR;
            }
        }
    } else {
        /* 前向雷达失效：保守策略，保持当前状态但降速
         * (不往 STOP 跳，避免前雷达偶发掉线就急停) */
    }

    /* ========================================================
     * 5. 根据当前状态输出最终安全速度
     * ======================================================== */
    switch (s_state) {
        case SEG_STATE_CORRIDOR: {
            /* 正常放行控制器输出 */
            out->safe_v = raw_cmd->v;
            out->safe_w = raw_cmd->w;
            break;
        }

        case SEG_STATE_APPROACH: {
            /* 线性插值减速:
             * 当 d_front 从 approach 距离滑向 stop 距离时，
             * 速度从 raw_cmd->v 线性衰减至 approach_min_v
             */
            float range = s_cfg.d_front_approach - s_cfg.d_front_stop;
            float ratio = 1.0f;
            if (range > 0.001f) {
                ratio = (d_front - s_cfg.d_front_stop) / range;
                ratio = seg_clampf(ratio, 0.0f, 1.0f);
            }
            float v_scaled = s_cfg.approach_min_v + ratio * (raw_cmd->v - s_cfg.approach_min_v);
            out->safe_v = seg_clampf(v_scaled, 0.0f, raw_cmd->v);
            out->safe_w = raw_cmd->w;  // 角速度不限，仍然允许纠偏
            break;
        }

        case SEG_STATE_STOP:
        default: {
            /* 到端停车 / 紧急停车：零速 */
            out->safe_v = 0.0f;
            out->safe_w = 0.0f;
            break;
        }
    }

    out->state = s_state;
}