#include <util/atomic.h>
#include "pid.h"

float pid_step(volatile struct pid* controller, float dt, float error) {
    // Calculate p term
    float p = error * controller->kP;

    // Calculate i term
    ATOMIC_BLOCK(ATOMIC_FORCEON){
        controller->integral += error * dt * controller->kI;
        if(controller->integral > 80)
            controller->integral = 80;
        if(controller->integral < -80)
            controller->integral = -80;
    }

    // Calculate d term, taking care to not divide by zero
    float d = dt == 0 ? 0 : ((error - controller->lastError) / dt) * controller->kD;
    controller->lastError = error;

    return p + controller->integral + d;
}

void init_pid(volatile struct pid* controller, float p, float i, float d){
    *controller = (struct pid){
        .kP = p,
        .kI = i,
        .kD = d,
        .lastError = 0,
        .integral = 0
    };
}