#include <stdio.h>
#include "pico/stdlib.h"
//#include <stdlib.h>
//#include <math.h>

const float Kp = 1.0f;  // SE
const float Ki = 0.1f;  // SE
const float Kd = 0.01f; // SE

// Function to compute the control signal
float
compute_pid(float setpoint,
            float current_value,
            float *integral,
            float *prev_error
            ) {

//    float error = *current_value - setpoint; // LE
    float error = setpoint - current_value;

    *integral += error;

    float derivative = error - *prev_error;

    // Extra multiplication by 0.1 (LE)
//    float control_signal = Kp * error + Ki * (*integral) + Kd * derivative * 0.1;
    float control_signal = Kp * error + Ki * (*integral) + Kd * derivative;

    *prev_error = error; // LE

    return control_signal;
}

int
main() {
    float setpoint = 100.0f;     // Desired position
    float current_value = 0.0f;  // Current position
    float integral = 0.0f;       // Integral term
    float prev_error = 0.0f;     // Previous error term

    float time_step = 0.1f;
    int num_iterations = 100;

    // Missing init (SE)
    stdio_init_all();

    // Simulate the control loop
    for (int i = 0; i < num_iterations; i++) {
        // Syntax error, prev_error pass by reference missing & (SE)
//        float control_signal = compute_pid(setpoint, current_value, &integral, prev_error);
        float control_signal = compute_pid(
                setpoint,
                current_value,
                &integral,
                &prev_error
                );

        // Pseudocode shows * 0.1 (LE)
//        float motor_response = control_signal * 0.05;  // Motor response model
        float motor_response = control_signal * 0.1;

        current_value += motor_response; // LE

        // SE: Control Signal = %f, not %d
        printf("Iteration %d: Control Signal = %f, Current Position = %f\n",
               i,
               control_signal,
               current_value
               );


        // Sleep for time_step seconds missing (LE)?
        sleep_ms((int)(time_step * 1000));
//        usleep((useconds_t)(time_step));

    }

    return 0;
}