/*
 * cc_pid.c
 *
 *  Created on: Jun 10, 2025
 *      Author: janik
 */

#include <PI.h>

void PI_Init(PIController *pi, float Kp, float Ki, uint16_t out_min, uint16_t out_max)
{
    pi->Kp = Kp;
    pi->Ki = Ki;
    pi->prev_error = 0.0f;
    pi->output = 0;
    pi->out_min = out_min;
    pi->out_max = out_max;
}

uint16_t PI_Update(PIController *pi, float target, float measurement)
{
    float error = target - measurement;
    float delta = pi->Kp * (error - pi->prev_error) + pi->Ki * error;

    // Add delta to the output (cast to float for calculation)
    float new_output = (float)(pi->output) + delta;

    // Clamp
    if (new_output > (float)(pi->out_max)) new_output = (float)(pi->out_max);
    else if (new_output < (float)(pi->out_min)) new_output = (float)(pi->out_min);

    // Update internal state
    pi->output = (uint16_t)new_output;
    pi->prev_error = error;

    return pi->output;
}