/**
 * @file speed_planner.c
 * @brief 斜坡速度规划器实现
 */

#include "component/speed_planner.h"
#include <math.h>
#include <string.h>

/**
 * @brief 限制函数
 */
static inline float clamp(float value, float min, float max) {
  if (value < min) return min;
  if (value > max) return max;
  return value;
}

/**
 * @brief 初始化速度规划器
 */
int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, const SpeedPlanner_Params_t *params) {
  if (planner == NULL || params == NULL) {
    return -1;
  }
  
  memset(planner, 0, sizeof(SpeedPlanner_t));
  planner->param = *params;
  
  return 0;
}

/**
 * @brief 重置速度规划器
 */
void SpeedPlanner_Reset(SpeedPlanner_t *planner, float current_position, float current_velocity) {
  if (planner == NULL) {
    return;
  }
  
  planner->current_position = current_position;
  planner->current_velocity = current_velocity;
  planner->target_position = current_position;
  planner->planned_position = current_position;
  planner->planned_velocity = current_velocity;
  planner->target_velocity = current_velocity;
}

/**
 * @brief 更新速度规划器
 */
float SpeedPlanner_Update(SpeedPlanner_t *planner, 
                          float target_velocity,
                          float current_position,
                          float current_velocity,
                          float dt) {
  if (planner == NULL || dt <= 0.0f) {
    return 0.0f;
  }
  
  /* 更新当前状态 */
  planner->current_position = current_position;
  planner->current_velocity = current_velocity;
  planner->target_velocity = target_velocity;
  
  /* 限制目标速度到最大速度范围 */
  float limited_target_velocity = clamp(target_velocity, 
                                        -planner->param.max_velocity, 
                                        planner->param.max_velocity);
  
  /* 计算速度变化 */
  float velocity_error = limited_target_velocity - planner->planned_velocity;
  
  /* 使用最大加速度限制速度变化率 */
  float max_velocity_change = planner->param.max_acceleration * dt;
  float velocity_change = clamp(velocity_error, -max_velocity_change, max_velocity_change);
  
  /* 更新规划速度 */
  planner->planned_velocity += velocity_change;
  
  /* 限制规划速度 */
  planner->planned_velocity = clamp(planner->planned_velocity,
                                    -planner->param.max_velocity,
                                    planner->param.max_velocity);
  
  /* 更新规划位置 */
  planner->planned_position += planner->planned_velocity * dt;
  
  /* 防止位移起飞：限制规划位置与当前位置的误差 */
  float position_error = planner->planned_position - current_position;
  
  if (fabsf(position_error) > planner->param.max_position_error) {
    /* 位置误差过大，重新规划 */
    if (position_error > 0.0f) {
      planner->planned_position = current_position + planner->param.max_position_error;
    } else {
      planner->planned_position = current_position - planner->param.max_position_error;
    }
    
    /* 根据位置误差调整速度，使位置逐渐收敛 */
    /* 使用简单的比例控制，系数可以调整 */
    float position_correction_gain = 2.0f; // 位置收敛增益
    planner->planned_velocity = (planner->planned_position - current_position) * position_correction_gain;
    
    /* 再次限制速度 */
    planner->planned_velocity = clamp(planner->planned_velocity,
                                      -planner->param.max_velocity,
                                      planner->param.max_velocity);
  }
  
  /* 更新目标位置（用于外部参考） */
  planner->target_position = planner->planned_position;
  
  return planner->planned_velocity;
}

/**
 * @brief 获取规划后的目标位置
 */
float SpeedPlanner_GetPlannedPosition(const SpeedPlanner_t *planner) {
  if (planner == NULL) {
    return 0.0f;
  }
  return planner->planned_position;
}

/**
 * @brief 获取规划后的速度
 */
float SpeedPlanner_GetPlannedVelocity(const SpeedPlanner_t *planner) {
  if (planner == NULL) {
    return 0.0f;
  }
  return planner->planned_velocity;
}