#include "corridor_ctrl.h"
#include <math.h>
#include <stdbool.h>
#include <stdint.h>

/* ====================== 内部状态 ====================== */
static CorridorCtrlConfig_t s_cfg;
static bool s_initialized = false;
static float s_last_w_cmd = 0.0f;
static uint32_t s_last_t_ms = 0U;

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

void CorridorCtrl_Init(const CorridorCtrlConfig_t *config)
{
    s_cfg = *config;
    s_initialized = true;
    s_last_w_cmd = 0.0f;
    s_last_t_ms = 0U;
}

void CorridorCtrl_Compute(const CorridorState_t *state,
                          const CorridorObs_t *obs,
                          float imu_wz,
                          RawCmd_t *out_cmd)
{
    if (!s_initialized) {
        out_cmd->v = 0.0f;
        out_cmd->w = 0.0f;
        out_cmd->flags = 0U;
        return;
    }

    /* ========================================================
     * 出沟保护: 当前激光检测到接近出口时，停止使用左右激光控制
     * 避免出沟时左右激光数据突变导致车身大幅度转向
     * ======================================================== */
    bool near_exit = false;
    if ((obs->valid_mask & (1U << 4)) != 0U) {  /* 前激光有效 (bit 4) */
        if (obs->d_front <= s_cfg.exit_front_dist) {
            near_exit = true;
        }
    }

    /* ========================================================
     * 核心控制律:
     *   w_cmd = kp_theta * e_th  +  kd_theta * (-imu_wz)  +  kp_y * e_y
     *
     * - kp_theta * e_th : 比例项，车头偏了就回转
     * - kd_theta * (-imu_wz) : 微分阻尼，等价于"阻止车头继续转"
     *   用 IMU 直接读数做微分项，比差分 e_th 更丝滑无噪声
     * - kp_y * e_y : 横向纠偏，车身偏了就产生角速度拉回来
     *
     * 出沟保护: 接近出口时，仅使用航向保持，不使用横向和角度纠偏
     * ======================================================== */

    float w_cmd;
    bool escape_active = false;
    bool front_guard_active = false;
    float kp_theta_eff = s_cfg.kp_theta;
    float kp_y_eff = s_cfg.kp_y;
    float w_max_eff = s_cfg.w_max;

    if (!near_exit && s_cfg.startup_dist > 0.01f && state->s < s_cfg.startup_dist) {
        float ratio = state->s / s_cfg.startup_dist;
        if (ratio < 0.0f) ratio = 0.0f;
        if (ratio > 1.0f) ratio = 1.0f;

        kp_y_eff = s_cfg.kp_y * (s_cfg.startup_kp_y_scale + (1.0f - s_cfg.startup_kp_y_scale) * ratio);
        w_max_eff = s_cfg.w_max * (s_cfg.startup_w_scale + (1.0f - s_cfg.startup_w_scale) * ratio);
        if (w_max_eff < 0.25f) {
            w_max_eff = 0.25f;
        }
    }

    if (near_exit) {
        /* 接近出口: 仅保持航向惯性，禁用左右激光控制 */
        w_cmd = -(s_cfg.kd_theta * imu_wz);
    } else {
        /* 正常控制: 完整PD控制律 */
        w_cmd = -(kp_theta_eff * state->e_th
                + s_cfg.kd_theta * imu_wz
                + kp_y_eff      * state->e_y);

        /* ========================================================
         * 近墙脱离保护:
         *   当某一侧平均距离已经明显过小，说明车身已经在擦壁或即将擦壁。
         *   此时不能只等 EKF 慢慢回中，直接叠加一个远离墙面的转向保护项。
         * ======================================================== */
        {
            bool left_front_ok  = ((obs->valid_mask & (1U << 0)) != 0U);
            bool left_rear_ok   = ((obs->valid_mask & (1U << 1)) != 0U);
            bool right_front_ok = ((obs->valid_mask & (1U << 2)) != 0U);
            bool right_rear_ok  = ((obs->valid_mask & (1U << 3)) != 0U);
            float w_escape = 0.0f;
            float left_min = 10.0f;
            float right_min = 10.0f;

            if (left_front_ok && obs->d_lf < left_min) left_min = obs->d_lf;
            if (left_rear_ok  && obs->d_lr < left_min) left_min = obs->d_lr;
            if (right_front_ok && obs->d_rf < right_min) right_min = obs->d_rf;
            if (right_rear_ok  && obs->d_rr < right_min) right_min = obs->d_rr;

            if (left_min < s_cfg.wall_escape_dist) {
                float err = s_cfg.wall_escape_dist - left_min;
                w_escape -= s_cfg.wall_escape_kp * err;  /* 左侧很近 -> 轻微右转 */
                escape_active = true;
            }

            if (right_min < s_cfg.wall_escape_dist) {
                float err = s_cfg.wall_escape_dist - right_min;
                w_escape += s_cfg.wall_escape_kp * err;  /* 右侧很近 -> 轻微左转 */
                escape_active = true;
            }

            w_escape = clampf(w_escape, -s_cfg.wall_escape_w_max, s_cfg.wall_escape_w_max);
            w_cmd += w_escape;

            /* 前角防呆：
             * 如果某一侧前角已经明显很近，就绝不允许继续朝这一侧打方向。
             * 这专门处理“已贴左墙却继续左打 / 已贴右墙却继续右打”的情况。 */
            {
                float front_guard_dist = s_cfg.wall_escape_dist + 0.01f;

                if (left_front_ok && obs->d_lf < front_guard_dist) {
                    float err = front_guard_dist - obs->d_lf;
                    float w_guard = 0.15f + 0.20f * (err / front_guard_dist);
                    if (w_cmd > -w_guard) {
                        w_cmd = -w_guard;
                    }
                    front_guard_active = true;
                }

                if (right_front_ok && obs->d_rf < front_guard_dist) {
                    float err = front_guard_dist - obs->d_rf;
                    float w_guard = 0.15f + 0.20f * (err / front_guard_dist);
                    if (w_cmd < w_guard) {
                        w_cmd = w_guard;
                    }
                    front_guard_active = true;
                }
            }
        }
    }

    /* 角速度限幅：防止 PD 溢出导致原地打转 */
    w_cmd = clampf(w_cmd, -w_max_eff, w_max_eff);

    /* 近墙保护触发时，再对总角速度做一次更保守的限幅，
     * 防止“贴墙 -> 一把猛打 -> 前向安全仲裁介入”的连锁反应。 */
    if (escape_active) {
        w_cmd = clampf(w_cmd, -0.60f, 0.60f);
    }

    if (front_guard_active) {
        w_cmd = clampf(w_cmd, -0.35f, 0.35f);
    }

    /* 角速度变化率限幅：抑制单拍突变，避免看起来像“突然失控猛打方向”。 */
    {
        float dt = 0.01f;
        if (s_last_t_ms != 0U && state->t_ms > s_last_t_ms) {
            dt = (float)(state->t_ms - s_last_t_ms) * 0.001f;
            if (dt <= 0.0f || dt > 0.1f) {
                dt = 0.01f;
            }
        }

        {
            float max_step = s_cfg.w_slew_rate * dt;
            float delta = w_cmd - s_last_w_cmd;
            delta = clampf(delta, -max_step, max_step);
            w_cmd = s_last_w_cmd + delta;
        }

        s_last_w_cmd = w_cmd;
        s_last_t_ms = state->t_ms;
    }

    /* ========================================================
     * 线速度策略:
     *   基础巡航速度 v_cruise，
     *   当角速度偏大时适当降速（弯道减速），保持运动学协调
     *   公式: v = v_cruise * (1 - k * |w/w_max|)
     *   k 取 0.3~0.5 较保守
     * ======================================================== */
    float speed_reduction = 0.0f;
    {
        float w_ratio = fabsf(w_cmd) / w_max_eff;
        float db = s_cfg.speed_reduction_deadband;
        if (db < 0.0f) db = 0.0f;
        if (db > 0.95f) db = 0.95f;

        if (w_ratio > db) {
            float active_ratio = (w_ratio - db) / (1.0f - db);
            speed_reduction = s_cfg.speed_reduction_k * active_ratio;
        }
    }
    float v_cmd = s_cfg.v_cruise * (1.0f - speed_reduction);

    /* 近墙脱离时轻微降速，避免“贴着墙还继续冲” */
    if (escape_active) {
        v_cmd *= 0.80f;
    }

    if (front_guard_active) {
        v_cmd *= 0.65f;
    }

    /* 线速度限幅：不允许倒车，不允许超速 */
    v_cmd = clampf(v_cmd, 0.0f, s_cfg.v_max);

    /* ========================================================
     * 置信度降级保护:
     *   当滤波器健康度 conf 过低（两边雷达全瞎），
     *   说明走廊参照完全丢失，降低线速度防止盲飞
     *   
     *   注意：阈值不宜过高，否则会过度降级导致控制器失效
     * ======================================================== */
    if (state->conf < 0.2f) {
        /* 健康度极低：速度打三折，保持航向惯性滑行 */
        v_cmd *= 0.3f;
    } else if (state->conf < 0.4f) {
        /* 健康度较低（单侧退化）：速度打七折 */
        v_cmd *= 0.7f;
    }

    /* 输出赋值 */
    out_cmd->t_ms  = state->t_ms;
    out_cmd->v     = v_cmd;
    out_cmd->w     = w_cmd;
    out_cmd->flags = 0U;
}