#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,
                   SafetyMode_t mode,
                   SegFsmOutput_t *out)
{
    if (!s_initialized) {
        out->state  = SEG_STATE_IDLE;
        out->safe_v = 0.0f;
        out->safe_w = 0.0f;
        return;
    }

    /* ========================================================
     * 0. IDLE 模式：零速，不做任何裁剪
     * ======================================================== */
    if (mode == SAFETY_MODE_IDLE) {
        out->state  = SEG_STATE_IDLE;
        out->safe_v = 0.0f;
        out->safe_w = 0.0f;
        return;
    }

    /* ========================================================
     * 1. TURN 模式：原地转向，直接放行，不检查前距和置信度
     *    解决 RISK-1: 转向阶段不会被安全层卡死
     * ======================================================== */
    if (mode == SAFETY_MODE_TURN) {
        s_state = SEG_STATE_CORRIDOR;
        out->state  = SEG_STATE_CORRIDOR;
        out->safe_v = raw_cmd->v;
        out->safe_w = raw_cmd->w;
        return;
    }

    /* ========================================================
     * 2. CORRIDOR 模式：完整安全检查 (前向 + 置信度)
     *    STRAIGHT 模式：前向检查，但不检查置信度
     * ======================================================== */

    /* 2a. 置信度检查：仅 CORRIDOR 模式下生效 */
    if (mode == SAFETY_MODE_CORRIDOR) {
        if (state->conf < s_cfg.conf_estop_thresh) {
            s_state = SEG_STATE_ESTOP;
        }
    }

    /* 待命状态 */
    if (s_state == SEG_STATE_IDLE) {
        out->state  = SEG_STATE_IDLE;
        out->safe_v = 0.0f;
        out->safe_w = 0.0f;
        return;
    }

    /* 紧急停车状态：等待置信度恢复 (仅 CORRIDOR 模式可能进入) */
    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;
    }

    /* ========================================================
     * 3. 前向距离判定：CORRIDOR 和 STRAIGHT 都做
     * ======================================================== */
    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;
            }
        }
    }

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

        case SEG_STATE_APPROACH: {
            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;
}