#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define BUFFER_SIZE 100

typedef struct {
    float Kp, Ki, Kd;                // PID gains
    float error_buffer[BUFFER_SIZE]; // Circular buffer for integral calculation
    int buffer_index;                // Current index in the buffer
    float integral_sum;              // Running sum of integral terms
    float prev_error;                // Previous error for derivative term
    float dt;                        // Sample time (seconds)
} PIDController;

// Initialize PID controller
void pid_init(PIDController *pid, float Kp, float Ki, float Kd, float dt) {
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->dt = dt;
    pid->buffer_index = 0;
    pid->integral_sum = 0.0f;
    pid->prev_error = 0.0f;
    memset(pid->error_buffer, 0, sizeof(pid->error_buffer));
}

// Compute PID output
float pid_compute(PIDController *pid, float setpoint, float pv) {
    float error = setpoint - pv;

    // Update integral term using circular buffer
    pid->integral_sum -=
        pid->error_buffer[pid->buffer_index]; // Remove oldest value
    pid->error_buffer[pid->buffer_index] =
        error * pid->dt; // Store new integral term
    pid->integral_sum += pid->error_buffer[pid->buffer_index]; // Add new value

    // Advance circular buffer index
    pid->buffer_index = (pid->buffer_index + 1) % BUFFER_SIZE;

    // Compute derivative term
    float derivative = (error - pid->prev_error) / pid->dt;
    pid->prev_error = error;

    // Compute PID output
    float output = (pid->Kp * error) + (pid->Ki * pid->integral_sum) +
                   (pid->Kd * derivative);

    return output;
}

int main() {
    PIDController pid;
    pid_init(&pid, 1.0f, 0.1f, 0.05f,
             0.01f); // Kp, Ki, Kd, dt (10ms sample time)

    float setpoint = 100.0f;
    float pv = 90.0f;

    for (int i = 0; i < 50; i++) {
        float output = pid_compute(&pid, setpoint, pv);
        printf("Iteration %d: Setpoint: %.2f, PV: %.2f, Output: %.2f\n", i,
               setpoint, pv, output);

        // Simulate process variable update
        pv += output * 0.1f;
        if (fabsf(output) < 0.4)
            setpoint = 150;
    }

    return 0;
}