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

/* 浮点数比较容差 */
#define FLOAT_EPSILON 1e-6f

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

/* 符号函数 */
static float SpeedPlanner_Sign(float value) {
  if (value > FLOAT_EPSILON) return 1.0f;
  if (value < -FLOAT_EPSILON) return -1.0f;
  return 0.0f;
}

/* 获取速度符号，用于判断加速/减速方向 */
__attribute__((unused)) static float SpeedPlanner_GetVelocitySign(float velocity) {
  return SpeedPlanner_Sign(velocity);
}

int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, SpeedPlanner_Param_t *param, float control_freq) {
  if (planner == NULL || param == NULL || control_freq <= 0.0f) {
    return -1;
  }
  
  /* 参数检查 */
  if (param->max_velocity <= 0.0f || param->max_acceleration <= 0.0f || 
      param->max_deceleration <= 0.0f || param->position_error_limit <= 0.0f) {
    return -1;
  }
  
  /* 复制参数 */
  planner->param = *param;
  
  /* 计算控制周期 */
  planner->dt = 1.0f / control_freq;
  
  /* 初始化状态 */
  SpeedPlanner_Reset(planner);
  
  return 0;
}

int8_t SpeedPlanner_Reset(SpeedPlanner_t *planner) {
  if (planner == NULL) {
    return -1;
  }
  
  planner->state = SPEED_PLANNER_IDLE;
  planner->current_velocity = 0.0f;
  planner->current_position = 0.0f;
  planner->target_velocity = 0.0f;
  planner->target_position = 0.0f;
  planner->planned_velocity = 0.0f;
  planner->planned_acceleration = 0.0f;
  planner->last_target_velocity = 0.0f;
  planner->position_error = 0.0f;
  planner->ramp_start_velocity = 0.0f;
  planner->ramp_end_velocity = 0.0f;
  planner->ramp_duration = 0.0f;
  planner->ramp_time = 0.0f;
  
  return 0;
}

int8_t SpeedPlanner_UpdateState(SpeedPlanner_t *planner, float current_velocity, float current_position) {
  if (planner == NULL) {
    return -1;
  }
  
  planner->current_velocity = current_velocity;
  planner->current_position = current_position;
  planner->position_error = planner->target_position - planner->current_position;
  
  return 0;
}

int8_t SpeedPlanner_SetTargetVelocity(SpeedPlanner_t *planner, float target_velocity) {
  if (planner == NULL) {
    return -1;
  }
  
  /* 限制目标速度在允许范围内 */
  planner->target_velocity = SpeedPlanner_Limit(target_velocity, 
                                                -planner->param.max_velocity, 
                                                planner->param.max_velocity);
  
  return 0;
}

int8_t SpeedPlanner_SetTargetPosition(SpeedPlanner_t *planner, float target_position) {
  if (planner == NULL) {
    return -1;
  }
  
  planner->target_position = target_position;
  planner->position_error = planner->target_position - planner->current_position;
  
  return 0;
}

/* 检查目标速度是否改变 */
static int8_t SpeedPlanner_IsTargetChanged(SpeedPlanner_t *planner) {
  return (fabsf(planner->target_velocity - planner->last_target_velocity) > planner->param.velocity_tolerance);
}

/* 开始新的斜坡规划 */
static void SpeedPlanner_StartRamp(SpeedPlanner_t *planner) {
  planner->ramp_start_velocity = planner->current_velocity;
  planner->ramp_end_velocity = planner->target_velocity;
  
  /* 计算需要的加速度和时间 */
  float velocity_diff = planner->ramp_end_velocity - planner->ramp_start_velocity;
  float required_acceleration = fabsf(velocity_diff);
  
  /* 选择合适的加速度（加速或减速） */
  float max_accel = (velocity_diff >= 0) ? planner->param.max_acceleration : planner->param.max_deceleration;
  
  if (required_acceleration > max_accel) {
    required_acceleration = max_accel;
  }
  
  /* 计算斜坡时间 */
  if (required_acceleration > FLOAT_EPSILON) {
    planner->ramp_duration = fabsf(velocity_diff) / required_acceleration;
  } else {
    planner->ramp_duration = 0.0f;
  }
  
  planner->ramp_time = 0.0f;
  
  /* 设置状态 */
  if (fabsf(velocity_diff) < planner->param.velocity_tolerance) {
    planner->state = SPEED_PLANNER_FINISHED;
  } else if (velocity_diff > 0) {
    planner->state = SPEED_PLANNER_ACCELERATING;
  } else {
    planner->state = SPEED_PLANNER_DECELERATING;
  }
}

/* 执行斜坡插值 */
static float SpeedPlanner_RampInterpolation(SpeedPlanner_t *planner) {
  if (planner->ramp_duration <= FLOAT_EPSILON) {
    return planner->ramp_end_velocity;
  }
  
  /* 线性插值 */
  float progress = planner->ramp_time / planner->ramp_duration;
  progress = SpeedPlanner_Limit(progress, 0.0f, 1.0f);
  
  return planner->ramp_start_velocity + 
         (planner->ramp_end_velocity - planner->ramp_start_velocity) * progress;
}

int8_t SpeedPlanner_Update(SpeedPlanner_t *planner) {
  if (planner == NULL) {
    return -1;
  }
  
  /* 检查位置误差是否超限 */
  if (fabsf(planner->position_error) > planner->param.position_error_limit) {
    /* 紧急停止：设置目标速度为0 */
    planner->target_velocity = 0.0f;
  }
  
  /* 检查目标是否改变 */
  if (SpeedPlanner_IsTargetChanged(planner) || planner->state == SPEED_PLANNER_IDLE) {
    SpeedPlanner_StartRamp(planner);
    planner->last_target_velocity = planner->target_velocity;
  }
  
  /* 根据当前状态执行规划 */
  switch (planner->state) {
    case SPEED_PLANNER_IDLE:
      planner->planned_velocity = planner->current_velocity;
      planner->planned_acceleration = 0.0f;
      break;
      
    case SPEED_PLANNER_ACCELERATING:
    case SPEED_PLANNER_DECELERATING:
      /* 执行斜坡规划 */
      planner->planned_velocity = SpeedPlanner_RampInterpolation(planner);
      
      /* 计算规划加速度 */
      if (planner->dt > FLOAT_EPSILON) {
        planner->planned_acceleration = (planner->planned_velocity - planner->current_velocity) / planner->dt;
      } else {
        planner->planned_acceleration = 0.0f;
      }
      
      /* 限制加速度 */
      float max_accel = (planner->state == SPEED_PLANNER_ACCELERATING) ? 
                        planner->param.max_acceleration : planner->param.max_deceleration;
      planner->planned_acceleration = SpeedPlanner_Limit(planner->planned_acceleration, -max_accel, max_accel);
      
      /* 更新斜坡时间 */
      planner->ramp_time += planner->dt;
      
      /* 检查是否完成斜坡 */
      if (planner->ramp_time >= planner->ramp_duration || 
          fabsf(planner->planned_velocity - planner->ramp_end_velocity) < planner->param.velocity_tolerance) {
        planner->state = SPEED_PLANNER_FINISHED;
        planner->planned_velocity = planner->ramp_end_velocity;
      }
      break;
      
    case SPEED_PLANNER_CONSTANT:
    case SPEED_PLANNER_FINISHED:
      planner->planned_velocity = planner->target_velocity;
      planner->planned_acceleration = 0.0f;
      break;
      
    default:
      planner->state = SPEED_PLANNER_IDLE;
      break;
  }
  
  /* 最终限制输出速度 */
  planner->planned_velocity = SpeedPlanner_Limit(planner->planned_velocity, 
                                                -planner->param.max_velocity, 
                                                planner->param.max_velocity);
  
  return 0;
}

float SpeedPlanner_GetPlannedVelocity(SpeedPlanner_t *planner) {
  if (planner == NULL) {
    return 0.0f;
  }
  return planner->planned_velocity;
}

float SpeedPlanner_GetPlannedAcceleration(SpeedPlanner_t *planner) {
  if (planner == NULL) {
    return 0.0f;
  }
  return planner->planned_acceleration;
}

float SpeedPlanner_GetPositionError(SpeedPlanner_t *planner) {
  if (planner == NULL) {
    return 0.0f;
  }
  return planner->position_error;
}

SpeedPlanner_State_t SpeedPlanner_GetState(SpeedPlanner_t *planner) {
  if (planner == NULL) {
    return SPEED_PLANNER_IDLE;
  }
  return planner->state;
}

int8_t SpeedPlanner_IsEmergencyStop(SpeedPlanner_t *planner) {
  if (planner == NULL) {
    return 0;
  }
  return (fabsf(planner->position_error) > planner->param.position_error_limit) ? 1 : 0;
}