/*******************************************************************************
 * 文件名称 : motion_control.c
 * 文件说明 : 割草机纯追踪运动控制器（C 语言实现）
 * 创建日期 : 2025-11-22
 *******************************************************************************/

#include "motion_control.h"

#include <math.h>
#include <string.h>

#include "arm_math.h"

#include "motion_config.h"

#ifndef M_PI
#define M_PI (3.14159265358979323846f)
#endif

#define DEG2RAD                (0.017453292519943295769236907684886f)
#define RAD2DEG                (57.295779513082320876798154814105f)
#define MC_CLAMP(v, lo, hi)    ((v) < (lo) ? (lo) : (((v) > (hi)) ? (hi) : (v)))

/* 将角度约束到 [-pi, pi]，避免航向跳变 */
static float mc_wrap_angle(float angle)
{
    while (angle > (float)M_PI)
    {
        angle -= 2.0f * (float)M_PI;
    }
    while (angle < -(float)M_PI)
    {
        angle += 2.0f * (float)M_PI;
    }
    return angle;
}

/* 读取路径点，索引越界时截到末尾 */
static void mc_get_point(const MC_State *state, HIDO_UINT32 idx, float outPt[2])
{
    if (idx >= state->path_count)
    {
        idx = state->path_count - 1U;
    }
    outPt[0] = state->path_xy[idx * 2U];
    outPt[1] = state->path_xy[idx * 2U + 1U];
}

/* 计算两个二维点的平方距离 */
static float mc_distance_sq(const float a[2], const float b[2])
{
    float dx = a[0] - b[0];
    float dy = a[1] - b[1];
    return dx * dx + dy * dy;
}

/* 计算两点距离（含 sqrt 防护） */
static float mc_distance(const float a[2], const float b[2])
{
    float dist_sq = mc_distance_sq(a, b);
    float dist = 0.0f;
    if (dist_sq > 0.0f)
    {
        arm_sqrt_f32(dist_sq, &dist);
    }
    return dist;
}

/* 搜索当前位置最近的路径点 */
static HIDO_UINT32 mc_find_nearest(const MC_State *state)
{
    float best_dist = 1.0e9f;
    HIDO_UINT32 best_idx = state->nearest_index;
    float pos[2] = {state->pos[0], state->pos[1]};

    for (HIDO_UINT32 i = 0; i < state->path_count; i++)
    {
        float point[2];
        mc_get_point(state, i, point);
        float dist_sq = mc_distance_sq(point, pos);
        if (dist_sq < best_dist)
        {
            best_dist = dist_sq;
            best_idx = i;
        }
    }
    return best_idx;
}

/* 沿路径累积距离，直到达到前视目标 */
static HIDO_UINT32 mc_find_lookahead(const MC_State *state, HIDO_UINT32 start_idx, float lookahead_m)
{
    float accum = 0.0f;
    float current[2];
    mc_get_point(state, start_idx, current);

    for (HIDO_UINT32 i = start_idx; i < state->path_count - 1U; i++)
    {
        float next[2];
        mc_get_point(state, i + 1U, next);
        float seg = mc_distance(current, next);
        accum += seg;
        if (accum >= lookahead_m)
        {
            return i + 1U;
        }
        current[0] = next[0];
        current[1] = next[1];
    }
    return state->path_count - 1U;
}

/* 计算带符号横向误差（左正右负） */
static float mc_cross_track_error(const MC_State *state, HIDO_UINT32 nearest_idx)
{
    if (state->path_count < 2U)
    {
        return 0.0f;
    }

    HIDO_UINT32 idx0 = (nearest_idx == 0U) ? 0U : nearest_idx - 1U;
    HIDO_UINT32 idx1 = (nearest_idx + 1U < state->path_count) ? nearest_idx + 1U : nearest_idx;

    float p0[2];
    float p1[2];
    float pos[2] = {state->pos[0], state->pos[1]};
    mc_get_point(state, idx0, p0);
    mc_get_point(state, idx1, p1);

    float vx = p1[0] - p0[0];
    float vy = p1[1] - p0[1];
    float wx = pos[0] - p0[0];
    float wy = pos[1] - p0[1];
    float seg_len_sq = vx * vx + vy * vy;

    float t = 0.0f;
    if (seg_len_sq > 0.0f)
    {
        t = (wx * vx + wy * vy) / seg_len_sq;
    }
    if (t < 0.0f)
    {
        t = 0.0f;
    }
    else if (t > 1.0f)
    {
        t = 1.0f;
    }

    float proj_x = p0[0] + vx * t;
    float proj_y = p0[1] + vy * t;
    float dx = pos[0] - proj_x;
    float dy = pos[1] - proj_y;
    float dist_sq = dx * dx + dy * dy;
    float dist = 0.0f;
    if (dist_sq > 0.0f)
    {
        arm_sqrt_f32(dist_sq, &dist);
    }
    float cross = vx * (pos[1] - p0[1]) - vy * (pos[0] - p0[0]);
    return (cross >= 0.0f) ? dist : -dist;
}

/* 阶段：旋转/接近起点，使车准备好跟踪路径 */
static void mc_compute_goto_start(MC_State *state, float dt_s, MC_Output *out)
{
    float start_point[2];
    mc_get_point(state, 0U, start_point);
    state->current_target_xy[0] = start_point[0];
    state->current_target_xy[1] = start_point[1];
    out->target_valid = HIDO_TRUE;
    out->target_xy[0] = start_point[0];
    out->target_xy[1] = start_point[1];
    float vec_x = start_point[0] - state->pos[0];
    float vec_y = start_point[1] - state->pos[1];
    float dist = mc_distance(start_point, state->pos);

    float desired_heading = arm_atan2_f32(vec_y, vec_x);
    float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
    float yaw_rate_cmd = state->config.heading_kp * heading_err;
    yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);

    float forward = 0.0f;
    if (dist > state->config.start_pos_tolerance_m)
    {
        if (fabsf(heading_err) < state->config.start_heading_tolerance_rad)
        {
            float desired = state->config.base_speed_mps + 0.5f * dist;
            forward = MC_CLAMP(desired,
                               state->config.min_follow_speed_mps,
                               state->config.max_forward_mps);
        }
    }
    else if (fabsf(heading_err) < state->config.start_heading_tolerance_rad)
    {
        state->stage = MC_STAGE_FOLLOW_PATH;
    }

    out->forward_mps = forward;
    out->turn_rate = yaw_rate_cmd;
    out->heading_error = heading_err;
    out->cross_track_error = dist;
    out->active = (forward != 0.0f) || (fabsf(yaw_rate_cmd) > 0.01f);
    (void)dt_s;
}

/* 阶段：纯追踪 + 航向/横向误差补偿 */
static void mc_compute_follow_path(MC_State *state, float dt_s, MC_Output *out)
{
    HIDO_UINT32 nearest_idx = mc_find_nearest(state);
    state->nearest_index = nearest_idx;

    float lookahead = state->config.lookahead_min_m;
    float speed_ratio = MC_CLAMP(state->speed_mps / state->config.max_forward_mps, 0.0f, 1.0f);
    lookahead += (state->config.lookahead_max_m - state->config.lookahead_min_m) * speed_ratio;
    HIDO_UINT32 lookahead_idx = mc_find_lookahead(state, nearest_idx, lookahead);
    state->lookahead_index = lookahead_idx;

    float target[2];
    mc_get_point(state, lookahead_idx, target);
    state->current_target_xy[0] = target[0];
    state->current_target_xy[1] = target[1];
    out->target_valid = HIDO_TRUE;
    out->target_xy[0] = target[0];
    out->target_xy[1] = target[1];

    float vec_x = target[0] - state->pos[0];
    float vec_y = target[1] - state->pos[1];
    float desired_heading = arm_atan2_f32(vec_y, vec_x);
    float heading_err = mc_wrap_angle(desired_heading - state->heading_rad);
    float heading_err_rate = (heading_err - state->last_heading_err) / (dt_s > 0.0f ? dt_s : 0.013f);
    state->last_heading_err = heading_err;

    float cross_track = mc_cross_track_error(state, nearest_idx);
    float yaw_rate_cmd = state->config.heading_kp * heading_err
                       + state->config.heading_kd * heading_err_rate
                       + state->config.xtrack_kp * cross_track;
    yaw_rate_cmd = MC_CLAMP(yaw_rate_cmd, -state->config.max_turn_rate, state->config.max_turn_rate);

    float forward = state->config.base_speed_mps
                  - state->config.heading_speed_scale * fabsf(heading_err)
                  - state->config.xtrack_speed_scale * fabsf(cross_track);
    forward = MC_CLAMP(forward,
                       state->config.min_follow_speed_mps,
                       state->config.max_forward_mps);

    float goal[2];
    mc_get_point(state, state->path_count - 1U, goal);
    float goal_dist = mc_distance(goal, state->pos);
    if (goal_dist < state->config.goal_tolerance_m)
    {
        state->stage = MC_STAGE_FINISHED;
        forward = 0.0f;
        yaw_rate_cmd = 0.0f;
    }

    out->forward_mps = forward;
    out->turn_rate = yaw_rate_cmd;
    out->heading_error = heading_err;
    out->cross_track_error = cross_track;
    out->active = (state->stage != MC_STAGE_FINISHED);
}

/* 填充默认配置 */
HIDO_VOID MC_DefaultConfig(MC_Config *_cfg)
{
    if (_cfg == HIDO_NULL)
    {
        return;
    }
    _cfg->max_forward_mps = MC_CFG_MAX_FORWARD_MPS;
    _cfg->max_reverse_mps = MC_CFG_MAX_REVERSE_MPS;
    _cfg->max_turn_rate = MC_CFG_MAX_TURN_RATE_RAD;
    _cfg->base_speed_mps = MC_CFG_BASE_SPEED_MPS;
    _cfg->min_follow_speed_mps = MC_CFG_MIN_FOLLOW_SPEED_MPS;
    _cfg->lookahead_min_m = MC_CFG_LOOKAHEAD_MIN_M;
    _cfg->lookahead_max_m = MC_CFG_LOOKAHEAD_MAX_M;
    _cfg->start_pos_tolerance_m = MC_CFG_START_POS_TOL_M;
    _cfg->start_heading_tolerance_rad = MC_CFG_START_HEADING_TOL_RAD;
    _cfg->goal_tolerance_m = MC_CFG_GOAL_TOL_M;
    _cfg->heading_kp = MC_CFG_HEADING_KP;
    _cfg->heading_kd = MC_CFG_HEADING_KD;
    _cfg->xtrack_kp = MC_CFG_XTRACK_KP;
    _cfg->heading_speed_scale = MC_CFG_HEADING_SPEED_SCALE;
    _cfg->xtrack_speed_scale = MC_CFG_XTRACK_SPEED_SCALE;
}

/* 初始化控制器，绑定路径与配置 */
HIDO_VOID MC_Init(MC_State *_state, const MC_Config *_cfg, const float *_path_xy, HIDO_UINT32 _point_count)
{
    if (_state == HIDO_NULL || _cfg == HIDO_NULL || _path_xy == HIDO_NULL || _point_count < 2U)
    {
        return;
    }

    memset(_state, 0, sizeof(MC_State));
    _state->config = *_cfg;
    _state->path_xy = _path_xy;
    _state->path_count = _point_count;
    _state->stage = MC_STAGE_GOTO_START;
    _state->nearest_index = 0U;
    _state->lookahead_index = 0U;
}

/* 注入最新 ENU 位姿及航向/速度 */
HIDO_VOID MC_UpdateGps(MC_State *_state, const float _enu[3], const ST_GPRMI *_gprmi)
{
    if (_state == HIDO_NULL || _enu == HIDO_NULL || _gprmi == HIDO_NULL)
    {
        return;
    }

    _state->pos[0] = _enu[0];
    _state->pos[1] = _enu[1];
    _state->pos[2] = _enu[2];
    _state->heading_rad = mc_wrap_angle((_gprmi->m_fHeadingAngle) * DEG2RAD);
    _state->heading_deg = _gprmi->m_fHeadingAngle;
    _state->pitch_deg = _gprmi->m_fPitchAngle;
    _state->roll_deg = _gprmi->m_fRollAngle;
    _state->vel[0] = _gprmi->m_fEastVelocity;
    _state->vel[1] = _gprmi->m_fNorthVelocity;
    float speed_sq = _state->vel[0] * _state->vel[0] + _state->vel[1] * _state->vel[1];
    float speed = 0.0f;
    if (speed_sq > 0.0f)
    {
        arm_sqrt_f32(speed_sq, &speed);
    }
    _state->speed_mps = speed;
    _state->pose_valid = HIDO_TRUE;
}

/* 更新 IMU 陀螺 Z 轴（yaw 角速度） */
HIDO_VOID MC_UpdateImu(MC_State *_state, const ST_GPIMU *_gpimu)
{
    if (_state == HIDO_NULL || _gpimu == HIDO_NULL)
    {
        return;
    }

    _state->yaw_rate_rad = _gpimu->m_fGyroZ * DEG2RAD;
    _state->imu_valid = HIDO_TRUE;
}

/* 入口：根据阶段计算控制量 */
HIDO_VOID MC_Compute(MC_State *_state, float _dt_s, MC_Output *_out)
{
    if (_state == HIDO_NULL || _out == HIDO_NULL)
    {
        return;
    }

    memset(_out, 0, sizeof(MC_Output));
    _out->target_valid = HIDO_FALSE;
    _out->target_xy[0] = 0.0f;
    _out->target_xy[1] = 0.0f;

    if (_state->path_xy == HIDO_NULL || _state->path_count < 2U)
    {
        return;
    }

    if (_state->pose_valid == HIDO_FALSE)
    {
        return;
    }

    if (_state->stage == MC_STAGE_IDLE)
    {
        _state->stage = MC_STAGE_GOTO_START;
    }

    switch (_state->stage)
    {
    case MC_STAGE_GOTO_START:
        mc_compute_goto_start(_state, _dt_s, _out);
        break;
    case MC_STAGE_FOLLOW_PATH:
        mc_compute_follow_path(_state, _dt_s, _out);
        break;
    case MC_STAGE_FINISHED:
        _out->active = HIDO_FALSE;
        break;
    default:
        break;
    }

    _out->stage = _state->stage;
    _out->pos_enu[0] = _state->pos[0];
    _out->pos_enu[1] = _state->pos[1];
    _out->pos_enu[2] = _state->pos[2];
    _out->heading_deg = _state->heading_deg;
    _out->pitch_deg = _state->pitch_deg;
    _out->roll_deg = _state->roll_deg;
    if (_state->stage == MC_STAGE_FOLLOW_PATH || _state->stage == MC_STAGE_GOTO_START)
    {
        _out->target_valid = HIDO_TRUE;
        _out->target_xy[0] = _state->current_target_xy[0];
        _out->target_xy[1] = _state->current_target_xy[1];
    }
    else
    {
        _out->target_valid = HIDO_FALSE;
        _out->target_xy[0] = 0.0f;
        _out->target_xy[1] = 0.0f;
    }
}