#include "pid.h"

void pid_init(pid_t *pid, float kp, float ki, float kd, float max_out, float max_i)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->integral = 0.0f;
    pid->last_error = 0.0f;
    pid->max_out = max_out;
    pid->max_integral = max_i;
}

float pid_calc(pid_t *pid, float target, float actual)
{
    float error = target - actual;
    pid->integral += error;
    if (pid->integral > pid->max_integral) pid->integral = pid->max_integral;
    if (pid->integral < -pid->max_integral) pid->integral = -pid->max_integral;
    float output = pid->kp * error + pid->ki * pid->integral
                 + pid->kd * (error - pid->last_error);
    pid->last_error = error;
    if (output > pid->max_out) output = pid->max_out;
    if (output < -pid->max_out) output = -pid->max_out;
    return output;
}