#include "bsp_pid.h"

/**
 * @brief 初始化 PID 控制器参数
 * @param pid 指向 PID 结构体的指针
 * @param kp 比例增益
 * @param ki 积分增益
 * @param kd 微分增益
 * @param out_max 输出最大限幅
 * @param int_max 积分最大限幅 (防止积分饱和)
 */
void pid_init(PID_TypeDef *pid, float kp, float ki, float kd, float out_max, float int_max) {
    pid->target = 0.0f;
    pid->actual = 0.0f;
    pid->err = 0.0f;
    pid->err_last = 0.0f;
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->integral = 0.0f;
    pid->integral_max = int_max;
    pid->output = 0.0f;
    pid->output_max = out_max;
}

/**
 * @brief 计算 PID 闭环输出
 * @param pid 指向 PID 结构体的指针
 * @param target 目标 RPM
 * @param actual 实际反馈 RPM
 * @return float 计算得出的 PWM 输出值
 */
float pid_calculate(PID_TypeDef *pid, float target, float actual) {
    pid->target = target;
    pid->actual = actual;
    pid->err = target - actual;

    /* 积分累加 */
    pid->integral += pid->err;

    /* 积分限幅 (Anti-Windup) */
    if (pid->integral > pid->integral_max) pid->integral = pid->integral_max;
    if (pid->integral < -pid->integral_max) pid->integral = -pid->integral_max;

    /* PID 公式：Output = Kp*err + Ki*integral + Kd*(err - err_last) */
    pid->output = (pid->Kp * pid->err) + 
                  (pid->Ki * pid->integral) + 
                  (pid->Kd * (pid->err - pid->err_last));

    /* 记录误差用于下次计算 */
    pid->err_last = pid->err;

    /* 输出限幅 */
    if (pid->output > pid->output_max) pid->output = pid->output_max;
    if (pid->output < -pid->output_max) pid->output = -pid->output_max;

    return pid->output;
}

/**
 * @brief 重置 PID 内部状态 (通常在电机停止时调用)
 */
void pid_reset(PID_TypeDef *pid) {
    pid->integral = 0.0f;
    pid->err = 0.0f;
    pid->err_last = 0.0f;
    pid->output = 0.0f;
}