/**
 * @file speed_planner.h
 * @brief 斜坡速度规划器
 * @details 提供最大加速度和最大速度限制,防止目标位移起飞
 */

#ifndef SPEED_PLANNER_H
#define SPEED_PLANNER_H

#include <stdint.h>

/* 速度规划器参数结构体 */
typedef struct {
  float max_velocity;     /* 最大速度 (m/s) */
  float max_acceleration; /* 最大加速度 (m/s²) */
  float max_position_error; /* 最大位置误差 (m), 防止位移起飞 */
} SpeedPlanner_Params_t;

/* 速度规划器状态结构体 */
typedef struct {
  float current_velocity;  /* 当前速度 (m/s) */
  float target_velocity;   /* 目标速度 (m/s) */
  float planned_velocity;  /* 规划后的速度 (m/s) */
  float current_position;  /* 当前位置 (m) */
  float target_position;   /* 目标位置 (m) */
  float planned_position;  /* 规划后的位置 (m) */
  
  SpeedPlanner_Params_t param; /* 参数 */
} SpeedPlanner_t;

/**
 * @brief 初始化速度规划器
 * @param planner 规划器结构体指针
 * @param params 参数结构体指针
 * @return 0:成功, -1:失败
 */
int8_t SpeedPlanner_Init(SpeedPlanner_t *planner, const SpeedPlanner_Params_t *params);

/**
 * @brief 重置速度规划器
 * @param planner 规划器结构体指针
 * @param current_position 当前位置
 * @param current_velocity 当前速度
 */
void SpeedPlanner_Reset(SpeedPlanner_t *planner, float current_position, float current_velocity);

/**
 * @brief 更新速度规划器
 * @param planner 规划器结构体指针
 * @param target_velocity 目标速度 (m/s)
 * @param current_position 当前位置 (m)
 * @param current_velocity 当前速度 (m/s)
 * @param dt 时间间隔 (s)
 * @return 规划后的速度 (m/s)
 */
float SpeedPlanner_Update(SpeedPlanner_t *planner, 
                          float target_velocity,
                          float current_position,
                          float current_velocity,
                          float dt);

/**
 * @brief 获取规划后的目标位置
 * @param planner 规划器结构体指针
 * @return 规划后的目标位置 (m)
 */
float SpeedPlanner_GetPlannedPosition(const SpeedPlanner_t *planner);

/**
 * @brief 获取规划后的速度
 * @param planner 规划器结构体指针
 * @return 规划后的速度 (m/s)
 */
float SpeedPlanner_GetPlannedVelocity(const SpeedPlanner_t *planner);

#endif // SPEED_PLANNER_H