#ifndef CORRIDOR_CTRL_H
#define CORRIDOR_CTRL_H

#include "preproc/corridor_msgs.h"

/**
 * @brief 走廊控制器调参配置
 * @note  所有增益参数都应在调试阶段"从小往大调"，防止震荡
 */
typedef struct {
    float kp_theta;           // 航向偏差比例增益 (rad/s per rad)
    float kd_theta;           // 航向偏差微分增益 (rad/s per rad/s) [阻尼项]
    float kp_y;               // 横向偏差比例增益 (rad/s per m)

    float v_cruise;           // 走廊内巡航线速度 (m/s)
    float w_max;              // 角速度输出硬限幅 (rad/s)，超过此值一律削峰
    float v_max;              // 线速度输出硬限幅 (m/s)
    float speed_reduction_k;  // 弯道减速系数 (0~1)，公式: v = v_cruise*(1-k*|w/w_max|)
} CorridorCtrlConfig_t;

#ifdef __cplusplus
extern "C" {
#endif

/**
 * @brief 初始化走廊控制器
 * @param config 控制器增益与限幅配置
 */
void CorridorCtrl_Init(const CorridorCtrlConfig_t *config);

/**
 * @brief 核心函数：根据滤波器输出的走廊状态计算控制量
 * @param state    滤波器输出的走廊状态 (e_y, e_th, conf)
 * @param obs      预处理层的观测快照 (用于获取 d_front 等辅助信息)
 * @param imu_wz   当前的 IMU Z轴角速度 (rad/s)，用于微分阻尼
 * @param out_cmd  输出的控制指令 (v, w)
 */
void CorridorCtrl_Compute(const CorridorState_t *state,
                          const CorridorObs_t *obs,
                          float imu_wz,
                          RawCmd_t *out_cmd);

#ifdef __cplusplus
}
#endif

#endif // CORRIDOR_CTRL_H