#include "trapezoidal_profile.h"
/* USER INCLUDE BEGIN */

/* USER INCLUDE END */


/* --- 梯形速度规划器相关定义与函数 --- */

// 1. 规划器初始化函数
 void Profile_Init(TrapezoidalProfile *p, float start, float target, float v_max, float acc) {
    p->start_pos = start;
    p->target_pos = target;
    float dist = target - start;
    p->direction = (dist >= 0.0f) ? 1 : -1;
    dist = fabsf(dist); 
    
    p->max_vel = v_max;
    p->accel = acc;
    p->elapsed_time = 0.0f;

    p->t_acc = v_max / acc;
    p->d_acc = 0.5f * acc * p->t_acc * p->t_acc;

    if (dist < 2.0f * p->d_acc) { // 走不到最大速度就得减速 (三角形曲线)
        p->d_acc = dist / 2.0f;
        p->t_acc = sqrtf(2.0f * p->d_acc / acc);
        p->t_total = 2.0f * p->t_acc;
        p->d_cruise = 0.0f;
        p->max_vel = acc * p->t_acc; 
    } else { // 标准梯形曲线
        p->d_cruise = dist - 2.0f * p->d_acc;
        p->t_total = 2.0f * p->t_acc + (p->d_cruise / v_max);
    }
}

// 2. 规划器实时更新函数
 void Profile_Update(TrapezoidalProfile *p, float dt) {
    p->elapsed_time += dt;
    float t = p->elapsed_time;
    float v = 0.0f, s = 0.0f;

    if (t < p->t_acc) { 
        // 加速段
        v = p->accel * t;
        s = 0.5f * p->accel * t * t;
    } 
    else if (t < (p->t_total - p->t_acc)) { 
        // 匀速段
        v = p->max_vel;
        s = p->d_acc + p->max_vel * (t - p->t_acc);
    } 
    else if (t < p->t_total) { 
        // 减速段
        float t_dec = t - (p->t_total - p->t_acc);
        v = p->max_vel - p->accel * t_dec;
        s = p->d_acc + p->d_cruise + (p->max_vel * t_dec - 0.5f * p->accel * t_dec * t_dec);
    } 
    else { 
        // 到达终点
        p->current_pos = p->target_pos;
        p->current_vel = 0.0f;
        return;
    }

    p->current_vel = v * (float)p->direction;
    p->current_pos = p->start_pos + s * (float)p->direction;
}
/* -------------------------------------------------------- */


/* USER DEFINE BEGIN */

/* USER DEFINE END */




/* USER FUNCTION BEGIN */

/* USER FUNCTION END */