#include "speed_supervisor.h"

#include <assert.h>
#include <limits.h>
#include <stdio.h>
#include <string.h>

static ds_config config(void) {
    ds_config c = {
        .max_rpm = DS_LISTING_MAX_RPM,
        .current_limit_ma = 1000u,
        .feedback_max_age_ms = 5u,
        .max_service_gap_ms = 3u,
        .ramp_rpm_per_s = 10000u,
        .zero_speed_rpm = 100u,
        .zero_speed_dwell_ms = 5u,
        .kp_q16_ma_per_rpm = 65536u,
        .ki_q16_ma_per_rpm_s = 0u
    };
    return c;
}

static ds_inputs inputs(uint32_t now, int32_t rpm) {
    ds_inputs in = {
        .power_ok = true,
        .driver_ready = true,
        .observer_locked = true,
        .current_calibrated = true,
        .hardware_fault = false,
        .overcurrent_fault = false,
        .regen_permitted = false,
        .handpiece_sensor_ok = true,
        .handpiece_parked = false,
        .measured_rpm = rpm,
        .measured_at_ms = now
    };
    return in;
}

static void test_init_and_start_gates(void) {
    ds_supervisor s;
    ds_config c = config();
    assert(ds_init(&s, &c, 0u));
    assert(s.state == DS_DISABLED && s.fault == DS_FAULT_NONE);
    ds_inputs in = inputs(0u, 0);
    assert(!ds_start(&s, 0, 0u, &in));
    assert(!ds_start(&s, (int32_t)DS_LISTING_MAX_RPM + 1, 0u, &in));
    in.driver_ready = false;
    assert(!ds_start(&s, 1000, 0u, &in));
    in = inputs(0u, 200);
    assert(!ds_start(&s, -1000, 0u, &in));
    in = inputs(0u, 0);
    assert(ds_start(&s, 1000, 0u, &in));
    assert(s.state == DS_RUNNING && s.ramped_target_rpm == 0);

    c.max_rpm = DS_LISTING_MAX_RPM + 1u;
    assert(!ds_init(&s, &c, 0u));
}

static void test_ramp_and_current_clamp(void) {
    ds_supervisor s;
    ds_config c = config();
    c.ramp_rpm_per_s = 1000000u;
    c.current_limit_ma = 100u;
    ds_inputs in = inputs(0u, 0);
    assert(ds_init(&s, &c, 0u));
    assert(ds_start(&s, 50000, 0u, &in));
    in = inputs(1u, 0);
    ds_output out = ds_update(&s, 50000, 1u, &in);
    assert(out.ramped_target_rpm == 1000);
    assert(out.q_current_ref_ma == 100);
    assert(out.run_enable && out.state == DS_RUNNING);

    in.measured_rpm = 1000;
    in.measured_at_ms = 2u;
    out = ds_update(&s, 50000, 2u, &in);
    assert(out.q_current_ref_ma == 100);
}

static void test_regen_gate_and_allowance(void) {
    ds_supervisor s;
    ds_config c = config();
    c.ramp_rpm_per_s = 1000000u;
    ds_inputs in = inputs(0u, 1000);
    assert(ds_init(&s, &c, 0u));
    assert(ds_start(&s, 1000, 0u, &in));

    in = inputs(1u, 1500);
    ds_output out = ds_update(&s, 500, 1u, &in);
    assert(out.q_current_ref_ma == 0); /* No unapproved regenerative braking. */

    in.regen_permitted = true;
    in.measured_at_ms = 2u;
    out = ds_update(&s, 500, 2u, &in);
    assert(out.q_current_ref_ma < 0);
}

static void test_reversal_waits_for_zero_speed(void) {
    ds_supervisor s;
    ds_config c = config();
    c.ramp_rpm_per_s = 1000000u;
    c.max_service_gap_ms = 10u;
    ds_inputs in = inputs(0u, 1000);
    assert(ds_init(&s, &c, 0u));
    assert(ds_start(&s, 1000, 0u, &in));

    in = inputs(1u, 1000);
    ds_output out = ds_update(&s, -1000, 1u, &in);
    assert(out.state == DS_REVERSAL_WAIT && out.q_current_ref_ma == 0);
    assert(out.ramped_target_rpm == 0);

    in = inputs(2u, 50);
    out = ds_update(&s, -1000, 2u, &in);
    assert(out.state == DS_REVERSAL_WAIT && out.q_current_ref_ma == 0);
    in = inputs(6u, 50);
    out = ds_update(&s, -1000, 6u, &in);
    assert(out.state == DS_REVERSAL_WAIT);
    in = inputs(7u, 0);
    out = ds_update(&s, -1000, 7u, &in);
    assert(out.state == DS_RUNNING && out.ramped_target_rpm < 0);
    assert(out.q_current_ref_ma < 0);
}

static void test_beginner_speed_cap(void) {
    ds_supervisor s;
    ds_config c = config();
    c.max_service_gap_ms = 3u;
    c.ramp_rpm_per_s = 1000000u;
    ds_inputs in = inputs(0u, 0);
    assert(ds_init(&s, &c, 0u));
    assert(ds_set_beginner_mode(&s, true));
    assert(ds_start(&s, 50000, 0u, &in));
    assert(!ds_set_beginner_mode(&s, false)); /* Must stop before changing mode. */
    assert(s.target_rpm == (int32_t)DS_BEGINNER_MAX_RPM);
    ds_output out = {0};
    for (uint32_t now = 1u; now <= 40u; ++now) {
        in = inputs(now, 0);
        out = ds_update(&s, 50000, now, &in);
        assert(out.state == DS_RUNNING);
        assert(out.beginner_mode);
        assert(out.active_speed_limit_rpm == DS_BEGINNER_MAX_RPM);
        assert(out.ramped_target_rpm <= (int32_t)DS_BEGINNER_MAX_RPM);
    }
    assert(out.ramped_target_rpm == (int32_t)DS_BEGINNER_MAX_RPM);
    ds_stop(&s);
    assert(ds_set_beginner_mode(&s, false));
    assert(ds_start(&s, 50000, 41u, &in));
    assert(s.target_rpm == 50000);
    assert(ds_update(&s, 50000, 42u, &(ds_inputs){
        .power_ok = true, .driver_ready = true, .observer_locked = true,
        .current_calibrated = true, .handpiece_sensor_ok = true,
        .measured_rpm = 0, .measured_at_ms = 42u
    }).active_speed_limit_rpm == DS_LISTING_MAX_RPM);
}

static void test_magnetic_park_stops_and_requires_explicit_restart(void) {
    ds_supervisor s;
    ds_config c = config();
    c.ramp_rpm_per_s = 1000000u;
    ds_inputs in = inputs(0u, 0);
    assert(ds_init(&s, &c, 0u));
    assert(ds_start(&s, 10000, 0u, &in));
    in = inputs(1u, 0);
    ds_output out = ds_update(&s, 10000, 1u, &in);
    assert(out.run_enable && out.state == DS_RUNNING);

    in = inputs(2u, 0);
    in.handpiece_parked = true;
    out = ds_update(&s, 10000, 2u, &in);
    assert(!out.run_enable && out.state == DS_DISABLED);
    assert(out.ramped_target_rpm == 0 && out.q_current_ref_ma == 0);
    assert(!ds_start(&s, 10000, 3u, &in));

    in = inputs(4u, 0); /* Pickup only clears the dock condition. */
    assert(s.state == DS_DISABLED);
    assert(ds_start(&s, 10000, 4u, &in)); /* A new user start is required. */
}

static void test_park_sensor_failure_latches_off(void) {
    ds_supervisor s;
    ds_config c = config();
    ds_inputs in = inputs(0u, 0);
    assert(ds_init(&s, &c, 0u));
    assert(ds_start(&s, 1000, 0u, &in));
    in = inputs(1u, 0);
    in.handpiece_sensor_ok = false;
    ds_output out = ds_update(&s, 1000, 1u, &in);
    assert(out.state == DS_FAULT && out.fault == DS_FAULT_INTERLOCK);
    assert(!out.run_enable && out.q_current_ref_ma == 0);
}

static void test_latched_faults(void) {
    ds_supervisor s;
    ds_config c = config();
    ds_inputs in = inputs(0u, 0);
    assert(ds_init(&s, &c, 0u));
    assert(ds_start(&s, 1000, 0u, &in));

    in = inputs(1u, 1000);
    in.hardware_fault = true;
    ds_output out = ds_update(&s, 1000, 1u, &in);
    assert(out.state == DS_FAULT && out.fault == DS_FAULT_DRIVER);
    assert(!out.run_enable && out.q_current_ref_ma == 0);
    assert(!ds_start(&s, 1000, 2u, &in));
    assert(!ds_clear_fault(&s, 2u, &in));

    in = inputs(3u, 0);
    assert(ds_clear_fault(&s, 3u, &in));
    assert(s.state == DS_DISABLED && s.fault == DS_FAULT_NONE);

    assert(ds_start(&s, 1000, 4u, &in));
    ds_output missing = ds_update(&s, 1000, 5u, NULL);
    assert(missing.state == DS_FAULT && missing.fault == DS_FAULT_INTERLOCK);
}

static void test_stale_feedback_gap_overspeed_and_target(void) {
    ds_supervisor s;
    ds_config c = config();
    ds_inputs in = inputs(0u, 0);
    assert(ds_init(&s, &c, 0u));
    assert(ds_start(&s, 1000, 0u, &in));

    in = inputs(6u, 100);
    ds_output out = ds_update(&s, 1000, 6u, &in);
    assert(out.state == DS_FAULT && out.fault == DS_FAULT_SERVICE_GAP);

    in = inputs(0u, 0);
    assert(ds_init(&s, &c, 0u));
    assert(ds_start(&s, 1000, 0u, &in));
    in = inputs(1u, 0);
    in.measured_at_ms = 0u;
    out = ds_update(&s, 1000, 1u, &in);
    assert(out.state == DS_RUNNING); /* Age 1 ms remains within the 5 ms limit. */
    in = inputs(2u, 0);
    in.observer_locked = false;
    out = ds_update(&s, 1000, 2u, &in);
    assert(out.state == DS_FAULT && out.fault == DS_FAULT_FEEDBACK_STALE);

    in = inputs(0u, 0);
    assert(ds_init(&s, &c, 0u));
    assert(ds_start(&s, 1000, 0u, &in));
    in = inputs(1u, 50001);
    out = ds_update(&s, 1000, 1u, &in);
    assert(out.state == DS_FAULT && out.fault == DS_FAULT_OVERSPEED);

    in = inputs(0u, 0);
    assert(ds_init(&s, &c, 0u));
    assert(ds_start(&s, 1000, 0u, &in));
    in = inputs(1u, 0);
    out = ds_update(&s, 50001, 1u, &in);
    assert(out.state == DS_FAULT && out.fault == DS_FAULT_TARGET);
}

static void test_stop_and_clock_wrap(void) {
    ds_supervisor s;
    ds_config c = config();
    uint32_t start = UINT32_MAX - 1u;
    ds_inputs in = inputs(start, 0);
    assert(ds_init(&s, &c, start));
    assert(ds_start(&s, 1000, start, &in));
    in = inputs(0u, 0);
    ds_output out = ds_update(&s, 1000, 0u, &in);
    assert(out.state == DS_RUNNING && out.ramped_target_rpm == 20);
    ds_stop(&s);
    assert(s.state == DS_DISABLED && s.q_current_ref_ma == 0);
}

int main(void) {
    test_init_and_start_gates();
    test_ramp_and_current_clamp();
    test_regen_gate_and_allowance();
    test_reversal_waits_for_zero_speed();
    test_beginner_speed_cap();
    test_magnetic_park_stops_and_requires_explicit_restart();
    test_park_sensor_failure_latches_off();
    test_latched_faults();
    test_stale_feedback_gap_overspeed_and_target();
    test_stop_and_clock_wrap();
    puts("PASS: signed RPM ramp/PI, 50,000/30,000 rpm limits, beginner mode, magnetic-park stop, feedback freshness, q-current clamp, regen gate, reversal interlock, fault latch, watchdog gap and wraparound.");
    return 0;
}
