#include "pid.h"

void pid_init(pid_t *pid)
{
    if (!pid) return;
    pid->Kp = 0.0f;
    pid->Ki = 0.0f;
    pid->Kd = 0.0f;
    pid->integral = 0.0f;
    pid->prev_measurement = 0.0f;
    pid->output_max   = 0.0f;
    pid->integral_max = 0.0f;
}

float pid_compute(pid_t *pid, float target, float actual)
{
    if (!pid) return 0.0f;
    float error = target - actual;

    /* 比例项 */
    float p_term = pid->Kp * error;

    /* 微分项（测量值微分 — 避免目标突变微分冲击） */
    float d_term = pid->Kd * (-(actual - pid->prev_measurement));
    pid->prev_measurement = actual;

    /* 积分项（带抗饱和限幅） */
    pid->integral += error;
    if (pid->integral_max > 0.0f) {
        if (pid->integral > pid->integral_max)
            pid->integral = pid->integral_max;
        else if (pid->integral < -pid->integral_max)
            pid->integral = -pid->integral_max;
    }
    float i_term = pid->Ki * pid->integral;
    float output = p_term + i_term + d_term;

    /* 输出限幅（0=不限幅） */
    if (pid->output_max > 0.0f) {
        if (output > pid->output_max)
            output = pid->output_max;
        else if (output < -pid->output_max)
            output = -pid->output_max;
    }

    return output;
}

void pid_reset(pid_t *pid)
{
    if (!pid) return;
    pid->integral = 0.0f;
    pid->prev_measurement = 0.0f;
}