component spindle_supervisor "Spindle supervisor (stable ratio + anti-windup)";

#include <math.h>

pin in float cmd_rpm;
pin in float fb_rpm;
pin in float motor_hz;
pin in float pid_output;
pin in bit enable;

/* INI */
pin in float max_spindle_rpm;
pin in float min_spindle_rpm;

pin in float pid_p_low;
pin in float pid_i_low;
pin in float pid_p_high;
pin in float pid_i_high;
pin in float pid_ff1;

/* outputs */
pin out float pid_cmd_rpm;
pin out float pid_p;
pin out float pid_i;
pin out float pid_ff1_out;

/* debug */
pin out float ratio_dbg;

/* internal */
variable float ratio = 10.0;
variable float ratio_filtered = 10.0;
variable float startup_timer = 0.0;

function _;
license "GPL";
;;

FUNCTION(_) {

    float cmd = cmd_rpm;

    if (cmd > max_spindle_rpm) cmd = max_spindle_rpm;
    if (cmd < 0.0) cmd = 0.0;

    if (!enable) {
        pid_cmd_rpm = 0;
        startup_timer = 0;
        return;
    }

    /* =========================
       OPEN LOOP START (0.5s)
       ========================= */
    if (startup_timer < 0.5) {
        startup_timer += fperiod;

        /* force open loop safe guess */
        pid_cmd_rpm = cmd * ratio;
        pid_p = pid_p_low;
        pid_i = 0.0;   // freeze integrator during startup
        pid_ff1_out = 0.0;

        return;
    }

    /* =========================
       REAL SYSTEM OBSERVATION
       ========================= */

    float motor_rpm = motor_hz * 30.0;

    /* only update ratio when valid */
    int valid =
        (fb_rpm > min_spindle_rpm) &&
        (motor_rpm > 50.0) &&
        (fabs(pid_output) < 1800.0);   // anti-saturation gate

    if (valid) {

        float new_ratio = motor_rpm / (fb_rpm + 0.001);

        if (new_ratio > 0.5 && new_ratio < 50.0) {
            ratio_filtered = ratio_filtered * 0.95 + new_ratio * 0.05;
            ratio = ratio_filtered;
        }
    }

    ratio_dbg = ratio;

    /* =========================
       COMMAND GENERATION
       ========================= */

    pid_cmd_rpm = cmd * ratio;

    /* =========================
       ADAPTIVE GAINS
       ========================= */

    if (ratio > 12.0) {
        pid_p = pid_p_low;
        pid_i = pid_i_low;
    } else {
        pid_p = pid_p_high;
        pid_i = pid_i_high;
    }

    /* =========================
       FEEDFORWARD
       ========================= */

    pid_ff1_out = (cmd < 200.0) ? 0.0 : pid_ff1;
}