#ifndef SPEED_PLANNER_H
#define SPEED_PLANNER_H

#include <stdint.h>

#ifdef __cplusplus
extern "C" {
#endif

/* 速度规划器状态枚举 */
typedef enum {
  SPEED_PLANNER_IDLE = 0,       /* 空闲状态 */
  SPEED_PLANNER_ACCELERATING,   /* 加速阶段 */
  SPEED_PLANNER_CONSTANT,       /* 匀速阶段 */
  SPEED_PLANNER_DECELERATING,   /* 减速阶段 */
  SPEED_PLANNER_FINISHED        /* 完成状态 */
} SpeedPlanner_State_t;

/* 速度规划器参数结构体 */
typedef struct {
  float max_velocity;           /* 最大速度 (m/s) */
  float max_acceleration;       /* 最大加速度 (m/s²) */
  float max_deceleration;       /* 最大减速度 (m/s²) */
  float position_error_limit;   /* 位置误差限制 (m) */
  float velocity_tolerance;     /* 速度容差，用于判断是否到达目标速度 (m/s) */
} SpeedPlanner_Param_t;

/* 速度规划器结构体 */
typedef struct {
  /* 参数 */
  SpeedPlanner_Param_t param;
  
  /* 状态变量 */
  SpeedPlanner_State_t state;
  float current_velocity;       /* 当前速度 (m/s) */
  float current_position;       /* 当前位置 (m) */
  float target_velocity;        /* 目标速度 (m/s) */
  float target_position;        /* 目标位置 (m) */
  
  /* 规划变量 */
  float planned_velocity;       /* 规划输出速度 (m/s) */
  float planned_acceleration;   /* 规划输出加速度 (m/s²) */
  
  /* 内部状态 */
  float last_target_velocity;   /* 上次目标速度，用于检测目标变化 */
  float position_error;         /* 位置误差 (目标位置 - 当前位置) */
  float dt;                     /* 控制周期 (s) */
  
  /* 斜坡规划状态 */
  float ramp_start_velocity;    /* 斜坡起始速度 */
  float ramp_end_velocity;      /* 斜坡结束速度 */
  float ramp_duration;          /* 斜坡持续时间 */
  float ramp_time;              /* 斜坡当前时间 */
} SpeedPlanner_t;

/**
 * @brief 初始化速度规划器
 * @param planner 速度规划器指针
 * @param param 参数结构体指针
 * @param control_freq 控制频率 (Hz)
 * @return 0: 成功, -1: 失败
 */
int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, SpeedPlanner_Param_t *param, float control_freq);

/**
 * @brief 更新当前状态
 * @param planner 速度规划器指针
 * @param current_velocity 当前速度 (m/s)
 * @param current_position 当前位置 (m)
 * @return 0: 成功, -1: 失败
 */
int8_t SpeedPlanner_UpdateState(SpeedPlanner_t *planner, float current_velocity, float current_position);

/**
 * @brief 设置目标速度
 * @param planner 速度规划器指针
 * @param target_velocity 目标速度 (m/s)
 * @return 0: 成功, -1: 失败
 */
int8_t SpeedPlanner_SetTargetVelocity(SpeedPlanner_t *planner, float target_velocity);

/**
 * @brief 设置目标位置
 * @param planner 速度规划器指针
 * @param target_position 目标位置 (m)
 * @return 0: 成功, -1: 失败
 */
int8_t SpeedPlanner_SetTargetPosition(SpeedPlanner_t *planner, float target_position);

/**
 * @brief 执行速度规划计算
 * @param planner 速度规划器指针
 * @return 0: 成功, -1: 失败
 */
int8_t SpeedPlanner_Update(SpeedPlanner_t *planner);

/**
 * @brief 获取规划的速度输出
 * @param planner 速度规划器指针
 * @return 规划的速度 (m/s)
 */
float SpeedPlanner_GetPlannedVelocity(SpeedPlanner_t *planner);

/**
 * @brief 获取规划的加速度输出
 * @param planner 速度规划器指针
 * @return 规划的加速度 (m/s²)
 */
float SpeedPlanner_GetPlannedAcceleration(SpeedPlanner_t *planner);

/**
 * @brief 获取位置误差
 * @param planner 速度规划器指针
 * @return 位置误差 (m)
 */
float SpeedPlanner_GetPositionError(SpeedPlanner_t *planner);

/**
 * @brief 获取当前状态
 * @param planner 速度规划器指针
 * @return 当前状态
 */
SpeedPlanner_State_t SpeedPlanner_GetState(SpeedPlanner_t *planner);

/**
 * @brief 重置速度规划器
 * @param planner 速度规划器指针
 * @return 0: 成功, -1: 失败
 */
int8_t SpeedPlanner_Reset(SpeedPlanner_t *planner);

/**
 * @brief 检查是否需要紧急停止（位置误差过大）
 * @param planner 速度规划器指针
 * @return 1: 需要紧急停止, 0: 正常
 */
int8_t SpeedPlanner_IsEmergencyStop(SpeedPlanner_t *planner);

#ifdef __cplusplus
}
#endif

#endif // SPEED_PLANNER_H