/*******************************************************************************
 * 文件名称 : motion_control.h
 * 文件说明 : 割草机纯追踪运动控制器（C 接口声明）
 * 创建日期 : 2025-11-22
 *******************************************************************************/

#ifndef FML_MOTION_CONTROL_H_
#define FML_MOTION_CONTROL_H_

#include "HIDO_TypeDef.h"
#include "GPS.h"

/* 控制器状态机阶段 */
typedef enum
{
    MC_STAGE_IDLE = 0,       /* 未运行，等待首次 GPS */
    MC_STAGE_GOTO_START,     /* 移动到起点并校准航向 */
    MC_STAGE_FOLLOW_PATH,    /* 正常纯追踪阶段 */
    MC_STAGE_FINISHED        /* 路径已完成（保持静止） */
} E_MCStage;

/* 可调参数（默认值见 motion_config.h） */
typedef struct
{
    float max_forward_mps;           /* m/s：前进限速 */
    float max_reverse_mps;           /* m/s：倒车限速（未使用，可扩展） */
    float max_turn_rate;             /* rad/s：yaw 速率上限 */
    float base_speed_mps;            /* m/s：巡航基准速度 */
    float min_follow_speed_mps;      /* m/s：防止速度降至 0 的下限 */
    float lookahead_min_m;           /* m：低速时的最小前视距离 */
    float lookahead_max_m;           /* m：高速时的最大前视距离 */
    float start_pos_tolerance_m;     /* m：判断到达起点的距离容差 */
    float start_heading_tolerance_rad; /* rad：起点航向容差 */
    float goal_tolerance_m;          /* m：终点完成阈值 */
    float heading_kp;                /* 航向误差 P 增益 */
    float heading_kd;                /* 航向误差 D 增益 */
    float xtrack_kp;                 /* 横向误差 P 增益 */
    float heading_speed_scale;       /* 航向误差→减速比例 */
    float xtrack_speed_scale;        /* 横向误差→减速比例 */
} MC_Config;

/* 控制器输出（前向速度 + 角速度 + 调试信息） */
typedef struct
{
    float forward_mps;               /* 期望前向速度 (m/s) */
    float turn_rate;                 /* 期望 yaw 角速度 (rad/s) */
    float heading_error;             /* 航向误差 (rad) */
    float cross_track_error;         /* 横向误差 (m) */
    HIDO_BOOL active;                /* 是否生成了有效控制量 */
    E_MCStage stage;                 /* 当前控制阶段 */
    float pos_enu[3];                /* 当前 ENU 位置 (E/N/U) */
    float heading_deg;               /* 当前航向 (deg) */
    float target_heading_deg;        /* 目标航向 (deg) */
    float pitch_deg;                 /* 当前俯仰 (deg) */
    float roll_deg;                  /* 当前横滚 (deg) */
    HIDO_BOOL target_valid;          /* 目标点是否有效 */
    float target_xy[2];              /* 目标点 (ENU) */
} MC_Output;

/* 控制器内部状态结构 */
typedef struct
{
    MC_Config config;                /* 运行时使用的配置 */
    const float *path_xy;            /* ENU 路径数组(x0,y0,...) */
    HIDO_UINT32 path_count;          /* 路径点数量 */
    float pos[3];                    /* 当前 ENU 位置 (E,N,U) (m) */
    float vel[2];                    /* ENU 速度向量 (m/s) */
    float heading_rad;               /* 当前航向 (rad) */
    float heading_deg;               /* 当前航向 (deg) */
    float pitch_deg;                 /* 当前俯仰 (deg) */
    float roll_deg;                  /* 当前横滚 (deg) */
    float yaw_rate_rad;              /* 当前角速度 (rad/s) */
    float speed_mps;                 /* 标量速度 (m/s) */
    HIDO_BOOL pose_valid;            /* GPS 数据是否有效 */
    HIDO_BOOL imu_valid;             /* IMU 数据是否有效 */
    E_MCStage stage;                 /* 控制阶段 */
    float current_target_xy[2];      /* 最近一次纯追踪目标点 */
    HIDO_UINT32 nearest_index;       /* 最近路径点索引（缓存） */
    HIDO_UINT32 lookahead_index;     /* 前视点索引（缓存） */
    float last_heading_err;          /* 上一周期航向误差 (rad) */
    float last_turn_sign;            /* 上一周期选择的转向方向（-1/0/+1） */
    float heading_err_d;             /* 保留字段：航向误差微分 */
    float progress_m;                /* 累计里程 (m，可扩展) */
} MC_State;

/* 使用库默认值初始化配置 */
HIDO_VOID MC_DefaultConfig(MC_Config *_cfg);
/* 绑定路径与配置，初始化控制器状态 */
HIDO_VOID MC_Init(MC_State *_state, const MC_Config *_cfg, const float *_path_xy, HIDO_UINT32 _point_count);
/* 注入最新 GPRMI 数据（ENU 位姿 + 航向/速度） */
HIDO_VOID MC_UpdateGps(MC_State *_state, const float _enu[3], const ST_GPRMI *_gprmi);
/* 注入 IMU 陀螺数据（yaw 角速度） */
HIDO_VOID MC_UpdateImu(MC_State *_state, const ST_GPIMU *_gpimu);
/* 执行一次控制循环，输出目标控制量 */
HIDO_VOID MC_Compute(MC_State *_state, float _dt_s, MC_Output *_out);

#endif /* FML_MOTION_CONTROL_H_ */