#include "pid.h"

fix16_t pid_step(pid_state_t *state, pid_config_t *conf,
                 fix16_t measured, fix16_t goal, fix16_t timestep)
{
    fix16_t filtered = fix16_mul(measured, F16(1) - conf->lowpass) + fix16_mul(state->prev_measured, conf->lowpass);
    
    fix16_t error = filtered - goal;
    
    fix16_t delta = filtered - state->prev_measured;
    state->prev_measured = filtered;
    
    state->integral = fix16_sadd(state->integral, fix16_mul(error, timestep));
    
    // Limit integral to avoid windup
    fix16_t maxint = fix16_abs(fix16_div(F16(1), conf->I));
    if (state->integral > maxint) state->integral = maxint;
    if (state->integral < -maxint) state->integral = -maxint;
    
    fix16_t output = fix16_mul(conf->P, error)
        + fix16_mul(conf->I, state->integral)
        + fix16_mul(conf->D, fix16_div(delta, timestep));
    
    return output;
}