#include <stdio.h>
 
typedef struct {
    // gains
    float Kp, Ki, Kd;
    // output limits
    float umin, umax;
    // derivative low-pass filter (alpha in (0,1], 1 = no filtering)
    float d_alpha;
 
    // state
    float integ;       // integral term state
    float prev_err;    // for derivative
    float d_filt;      // filtered derivative
    int   initialized; // init flag
} PID;
 
void pid_init(PID *pid, float Kp, float Ki, float Kd,
              float umin, float umax, float d_alpha) {
    pid->Kp = Kp; pid->Ki = Ki; pid->Kd = Kd;
    pid->umin = umin; pid->umax = umax;
    pid->d_alpha = (d_alpha <= 0.0f) ? 1.0f : (d_alpha > 1.0f ? 1.0f : d_alpha);
    pid->integ = 0.0f;
    pid->prev_err = 0.0f;
    pid->d_filt = 0.0f;
    pid->initialized = 0;
}
 
static inline float clampf(float x, float lo, float hi) {
    if (x < lo) return lo;
    if (x > hi) return hi;
    return x;
}
 
float pid_update(PID *pid, float setpoint, float measurement, float dt) {
    // basic dt guard
    if (dt <= 1e-6f) dt = 1e-3f;
 
    float err = setpoint - measurement;
 
    // init derivative on first call to avoid spike
    if (!pid->initialized) {
        pid->prev_err = err;
        pid->d_filt = 0.0f;
        pid->initialized = 1;
    }
 
    // proportional
    float P = pid->Kp * err;
 
    // derivative (backward diff) then low-pass filter
    float d_raw = (err - pid->prev_err) / dt;
    pid->d_filt = pid->d_alpha * pid->d_filt + (1.0f - pid->d_alpha) * d_raw;
    float D = pid->Kd * pid->d_filt;
 
    // integral with anti-windup (clamped via conditional integration)
    float u_unsat_P_D = P + D;
    float integ_candidate = pid->integ + err * dt;
 
    // tentative control with updated integral
    float I_tent = pid->Ki * integ_candidate;
    float u_tent = u_unsat_P_D + I_tent;
 
    // apply saturation
    float u = clampf(u_tent, pid->umin, pid->umax);
 
    // anti-windup: only accept integral growth if not saturating against error direction
    int saturating_high = (u_tent > pid->umax) && (err > 0.0f);
    int saturating_low  = (u_tent < pid->umin) && (err < 0.0f);
    if (!(saturating_high || saturating_low)) {
        pid->integ = integ_candidate; // accept integral update
    }
    // else reject integral update this step (freezes at previous value)
 
    // update memory
    pid->prev_err = err;
 
    return u;
}
 
// Example usage
int main(void) {
    PID pid;
    pid_init(&pid, 1.0f, 2.0f, 0.05f, -10.0f, 10.0f, 0.8f);
 
    float setpoint = 100.0f; // rpm
    float y = 0.0f;          // measurement
    float dt = 0.01f;        // 10 ms
 
    for (int k = 0; k < 1000; ++k) {
        float u = pid_update(&pid, setpoint, y, dt);
        // toy plant: y[k+1] = y[k] + 0.1*u - 0.02*y
        y = y + 0.1f*u - 0.02f*y;
        if (k % 100 == 0) {
            printf("t=%.2f  u=%.3f  y=%.3f\n", k*dt, u, y);
        }
    }
    return 0;
}

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