#include "pid.h"

void PID_Init(PID *pid, float Kp, float Ki, float Kd,
              float out_min, float out_max)
{
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
    pid->output_min = out_min;
    pid->output_max = out_max;
    pid->integral_limit = (out_max - out_min) * 0.3f;
    pid->out = 0.0f;
}

void PID_SetGains(PID *pid, float Kp, float Ki, float Kd)
{
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
}

float PID_Update(PID *pid, float setpoint, float measurement, float dt)
{
    float error = setpoint - measurement;
    float output;

    /* Proportional */
    float P = pid->Kp * error;

    /* Integral with anti-windup */
    pid->integral += error * dt;
    if (pid->integral > pid->integral_limit)
        pid->integral = pid->integral_limit;
    if (pid->integral < -pid->integral_limit)
        pid->integral = -pid->integral_limit;
    float I = pid->Ki * pid->integral;

    /* Derivative */
    float D = pid->Kd * (error - pid->prev_error) / dt;

    /* Sum */
    output = P + I + D;

    /* Clamp */
    if (output > pid->output_max) output = pid->output_max;
    if (output < pid->output_min) output = pid->output_min;

    pid->prev_error = error;
    pid->out = output;

    return output;
}

void PID_Reset(PID *pid)
{
    pid->integral = 0.0f;
    pid->prev_error = 0.0f;
    pid->out = 0.0f;
}