/*
 * test_steering_pid.c — unit tests for steering_pid (Issue #616).
 *
 * Host build (no HAL):
 *   gcc -I include -I src -DTEST_HOST -lm -o /tmp/test_steering test/test_steering_pid.c
 *   /tmp/test_steering
 *
 * Stubs out jlink.h to avoid the full HAL dependency chain.
 */

/* ---- Minimal stubs ---- */
#define JLINK_H  /* prevent real jlink.h from loading */

#include <stdint.h>
#include <stdbool.h>

/* Minimal jlink_tlm_steering_t and send stub */
typedef struct __attribute__((packed)) {
    int16_t  target_x10;
    int16_t  actual_x10;
    int16_t  output;
    uint8_t  enabled;
    uint8_t  _pad;
} jlink_tlm_steering_t;

static jlink_tlm_steering_t g_last_tlm;
static int g_tlm_count = 0;

void jlink_send_steering_tlm(const jlink_tlm_steering_t *tlm)
{
    g_last_tlm = *tlm;
    g_tlm_count++;
}

/* ---- Include the module under test ---- */
#include "steering_pid.h"
#include "steering_pid.c"   /* single-TU build; steering_pid.c found via -I src */

/* ---- Test framework ---- */
#include <stdio.h>
#include <math.h>
#include <string.h>

static int s_pass = 0;
static int s_fail = 0;

#define EXPECT(cond, msg) do { \
    if (cond) { s_pass++; } \
    else { s_fail++; printf("FAIL [%s:%d]: %s\n", __func__, __LINE__, msg); } \
} while(0)

#define EXPECT_EQ(a, b, msg)  EXPECT((a) == (b), msg)
#define EXPECT_NEAR(a, b, eps, msg) EXPECT(fabsf((a) - (b)) <= (eps), msg)
#define EXPECT_TRUE(x, msg)   EXPECT((x), msg)
#define EXPECT_FALSE(x, msg)  EXPECT(!(x), msg)

/* ---- Tests ---- */

static void test_init(void)
{
    steering_pid_t s;
    steering_pid_init(&s);

    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "init: target_omega 0");
    EXPECT_NEAR(s.actual_omega_dps, 0.0f, 1e-6f, "init: actual_omega 0");
    EXPECT_NEAR(s.integral,         0.0f, 1e-6f, "init: integral 0");
    EXPECT_NEAR(s.prev_error,       0.0f, 1e-6f, "init: prev_error 0");
    EXPECT_EQ  (s.output,           0,           "init: output 0");
    EXPECT_TRUE(s.enabled,                        "init: enabled true");
}

static void test_reset(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    s.target_omega_dps = 30.0f;
    s.integral  = 100.0f;
    s.prev_error = 5.0f;
    s.output     = 200;

    steering_pid_reset(&s);

    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "reset: target 0");
    EXPECT_NEAR(s.integral,         0.0f, 1e-6f, "reset: integral 0");
    EXPECT_NEAR(s.prev_error,       0.0f, 1e-6f, "reset: prev_error 0");
    EXPECT_EQ  (s.output,           0,           "reset: output 0");
    EXPECT_TRUE(s.enabled,                        "reset: preserves enabled");
}

static void test_disabled_returns_zero(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_enabled(&s, false);
    steering_pid_set_target(&s, 50.0f);

    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);

    EXPECT_EQ(out, 0, "disabled: update returns 0");
    EXPECT_EQ(steering_pid_get_output(&s), 0, "disabled: get_output 0");
    /* Target should not have been set when disabled */
    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "disabled: target not set");
}

static void test_zero_dt_returns_zero(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 30.0f);

    int16_t out = steering_pid_update(&s, 0.0f, 0.0f);
    EXPECT_EQ(out, 0, "zero_dt: returns 0");
}

static void test_negative_dt_returns_zero(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 30.0f);

    int16_t out = steering_pid_update(&s, 0.0f, -0.01f);
    EXPECT_EQ(out, 0, "neg_dt: returns 0");
}

static void test_proportional_only(void)
{
    /*
     * With Ki=0 and Kd=0, output = Kp * error.
     * Set STEER_KP=2.0, error=50 deg/s → raw = 100 counts.
     * Rate limiter allows max_step = STEER_RAMP_RATE_PER_MS * dt_ms = 10 * 1 = 10 per step.
     * After 1 step (dt=0.001s): output capped at 10 by rate limiter.
     */
    steering_pid_t s;
    memset(&s, 0, sizeof(s));
    steering_pid_init(&s);
    s.integral   = 0.0f;
    s.prev_error = 0.0f;
    s.output     = 0;

    steering_pid_set_target(&s, 50.0f);
    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);

    /* Rate limiter: max_step = 10*0.001*1000 = 10 → output = 10 */
    EXPECT_EQ(out, 10, "prop_only: rate-limited step");
}

static void test_converges_to_setpoint(void)
{
    /*
     * With no disturbance, run many steps until output reaches the setpoint.
     * With target=20 deg/s, Kp=2, after many steps the P term = 2*20 = 40,
     * plus I accumulation. The rate limiter will eventually allow the output
     * to grow. Run for 1s at 1kHz (1000 steps, dt=0.001s each).
     * Expected: eventually output > 0 (PID working), integral accumulates.
     */
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 20.0f);

    int16_t out = 0;
    for (int i = 0; i < 1000; i++) {
        out = steering_pid_update(&s, 0.0f, 0.001f);
    }
    EXPECT_TRUE(out > 0, "convergence: output positive after 1s");
    EXPECT_TRUE(s.integral > 0.0f, "convergence: integral positive");
}

static void test_output_clamped_to_max(void)
{
    /*
     * Very large error should produce output clamped to STEER_OUTPUT_MAX.
     * Run many ticks so rate limiter is satisfied.
     * STEER_OUTPUT_MAX = 400; 400 ticks at 10 counts/ms should saturate.
     */
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 500.0f);  /* 500 deg/s — way beyond robot capability */

    int16_t out = 0;
    for (int i = 0; i < 500; i++) {
        out = steering_pid_update(&s, 0.0f, 0.001f);
    }
    EXPECT_EQ(out, STEER_OUTPUT_MAX, "clamp: output saturates at STEER_OUTPUT_MAX");
}

static void test_output_clamped_negative(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, -500.0f);

    int16_t out = 0;
    for (int i = 0; i < 500; i++) {
        out = steering_pid_update(&s, 0.0f, 0.001f);
    }
    EXPECT_EQ(out, -STEER_OUTPUT_MAX, "clamp_neg: output saturates at -STEER_OUTPUT_MAX");
}

static void test_anti_windup(void)
{
    /*
     * Run with a persistent error for a long time.
     * Integral should be clamped to ±STEER_INTEGRAL_MAX.
     */
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 50.0f);

    for (int i = 0; i < 10000; i++) {
        steering_pid_update(&s, 0.0f, 0.001f);  /* actual stays 0, error = 50 */
    }
    EXPECT_NEAR(s.integral, STEER_INTEGRAL_MAX, 0.01f, "anti-windup: integral clamped to max");
}

static void test_anti_windup_negative(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, -50.0f);

    for (int i = 0; i < 10000; i++) {
        steering_pid_update(&s, 0.0f, 0.001f);
    }
    EXPECT_NEAR(s.integral, -STEER_INTEGRAL_MAX, 0.01f, "anti-windup-neg: integral clamped");
}

static void test_rate_limiter_single_step(void)
{
    /*
     * At dt=0.001s: max_step = STEER_RAMP_RATE_PER_MS * 1 = 10.
     * Even if raw PID output = 400, first step can only reach 10.
     */
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 500.0f);

    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
    EXPECT_EQ(out, STEER_RAMP_RATE_PER_MS * 1, "rate_lim: first step bounded");
}

static void test_rate_limiter_larger_dt(void)
{
    /*
     * At dt=0.01s (100Hz): max_step = 10 * 10 = 100.
     * First step limited to 100.
     */
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 500.0f);

    int16_t out = steering_pid_update(&s, 0.0f, 0.01f);
    EXPECT_EQ(out, STEER_RAMP_RATE_PER_MS * 10, "rate_lim_100hz: first step bounded");
}

static void test_rate_limiter_decreasing(void)
{
    /*
     * After saturating to STEER_OUTPUT_MAX, set target to 0.
     * Output should decrease at most STEER_RAMP_RATE_PER_MS per ms.
     */
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 500.0f);
    for (int i = 0; i < 500; i++) {
        steering_pid_update(&s, 0.0f, 0.001f);
    }
    /* Now output = STEER_OUTPUT_MAX = 400 */
    int16_t before = s.output;
    steering_pid_reset(&s);  /* reset integral and target */
    /* output is also zeroed by reset — which is fine: the test is
       really about rate limiting when driving toward 0 after a step */
    (void)before;

    /* Re-saturate, then drive to 0 with matching actual */
    steering_pid_init(&s);
    steering_pid_set_target(&s, 500.0f);
    for (int i = 0; i < 500; i++) {
        steering_pid_update(&s, 0.0f, 0.001f);
    }
    steering_pid_set_target(&s, 0.0f);
    int16_t pre  = s.output;
    int16_t post = steering_pid_update(&s, 0.0f, 0.001f);

    /* Should decrease by no more than STEER_RAMP_RATE_PER_MS per tick */
    int16_t delta = pre - post;
    EXPECT_TRUE(delta <= STEER_RAMP_RATE_PER_MS, "rate_lim_dec: decrease bounded per tick");
}

static void test_zero_error_zero_output(void)
{
    /*
     * When target == actual from the first tick, error = 0 and output stays 0.
     */
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 30.0f);
    int16_t out = steering_pid_update(&s, 30.0f, 0.001f);
    EXPECT_EQ(out, 0, "zero_error: no output when tracking exactly");
}

static void test_enable_disable_clears_state(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 50.0f);
    for (int i = 0; i < 100; i++) {
        steering_pid_update(&s, 0.0f, 0.001f);
    }

    /* Disable */
    steering_pid_set_enabled(&s, false);

    EXPECT_FALSE(s.enabled,                       "disable: enabled false");
    EXPECT_NEAR (s.target_omega_dps, 0.0f, 1e-6f, "disable: target zeroed");
    EXPECT_NEAR (s.integral,         0.0f, 1e-6f, "disable: integral zeroed");
    EXPECT_EQ   (s.output,           0,           "disable: output zeroed");

    /* Calls while disabled return 0 */
    int16_t out = steering_pid_update(&s, 50.0f, 0.001f);
    EXPECT_EQ(out, 0, "disable: update returns 0");
}

static void test_re_enable(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_enabled(&s, false);
    steering_pid_set_enabled(&s, true);

    EXPECT_TRUE(s.enabled, "re-enable: enabled true");

    /* After re-enable, new target and update should work */
    steering_pid_set_target(&s, 20.0f);
    EXPECT_NEAR(s.target_omega_dps, 20.0f, 1e-6f, "re-enable: set_target works");
}

static void test_set_target_ignored_when_disabled(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_enabled(&s, false);
    steering_pid_set_target(&s, 99.0f);

    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "disabled_target: target not set");
}

static void test_actual_omega_updated(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_update(&s, 42.5f, 0.001f);
    EXPECT_NEAR(s.actual_omega_dps, 42.5f, 1e-4f, "actual_omega: stored after update");
}

static void test_get_output_matches_update(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 20.0f);
    int16_t from_update = steering_pid_update(&s, 0.0f, 0.001f);
    EXPECT_EQ(from_update, steering_pid_get_output(&s), "get_output: matches update return");
}

static void test_omega_scale_convention(void)
{
    /*
     * Verify that setting target via STEER_OMEGA_SCALE:
     *   omega_dps = steer * STEER_OMEGA_SCALE
     *   steer=100 → omega=10 deg/s
     */
    float omega = 100.0f * STEER_OMEGA_SCALE;
    EXPECT_NEAR(omega, 10.0f, 1e-4f, "omega_scale: 100 units = 10 deg/s");

    omega = -1000.0f * STEER_OMEGA_SCALE;
    EXPECT_NEAR(omega, -100.0f, 1e-4f, "omega_scale: -1000 units = -100 deg/s");
}

static void test_tlm_first_call_fires(void)
{
    /*
     * After init, last_tlm_ms is set so that the first send_tlm fires immediately
     * at now_ms = 0.
     */
    steering_pid_t s;
    steering_pid_init(&s);
    g_tlm_count = 0;

    steering_pid_send_tlm(&s, 0);
    EXPECT_EQ(g_tlm_count, 1, "tlm_first: fires at ms=0 after init");
}

static void test_tlm_rate_limited(void)
{
    /*
     * STEER_TLM_HZ = 10 → interval = 100ms.
     * Two calls at ms=0 and ms=50 should only send once.
     */
    steering_pid_t s;
    steering_pid_init(&s);
    g_tlm_count = 0;

    steering_pid_send_tlm(&s, 0);    /* fires (first call) */
    steering_pid_send_tlm(&s, 50);   /* blocked (< 100ms) */
    EXPECT_EQ(g_tlm_count, 1, "tlm_rate: blocked within interval");

    steering_pid_send_tlm(&s, 100);  /* fires (interval elapsed) */
    EXPECT_EQ(g_tlm_count, 2, "tlm_rate: fires after interval");
}

static void test_tlm_payload_target(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    s.target_omega_dps = 25.5f;  /* → target_x10 = 255 */
    g_tlm_count = 0;

    steering_pid_send_tlm(&s, 0);
    EXPECT_EQ(g_last_tlm.target_x10, 255, "tlm_payload: target_x10 correct");
}

static void test_tlm_payload_actual(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 10.0f);
    steering_pid_update(&s, 12.3f, 0.001f);
    g_tlm_count = 0;

    steering_pid_send_tlm(&s, 0);
    /* actual = 12.3 deg/s → actual_x10 = 123 */
    EXPECT_EQ(g_last_tlm.actual_x10, 123, "tlm_payload: actual_x10 correct");
}

static void test_tlm_payload_output(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    s.output = 150;
    g_tlm_count = 0;

    steering_pid_send_tlm(&s, 0);
    EXPECT_EQ(g_last_tlm.output, 150, "tlm_payload: output correct");
}

static void test_tlm_payload_enabled_flag(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_enabled(&s, true);
    g_tlm_count = 0;
    steering_pid_send_tlm(&s, 0);
    EXPECT_EQ(g_last_tlm.enabled, 1u, "tlm_payload: enabled=1 when on");

    steering_pid_init(&s);
    steering_pid_set_enabled(&s, false);
    steering_pid_send_tlm(&s, 0);
    EXPECT_EQ(g_last_tlm.enabled, 0u, "tlm_payload: enabled=0 when off");
}

static void test_tlm_payload_pad_zero(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    g_tlm_count = 0;
    steering_pid_send_tlm(&s, 0);
    EXPECT_EQ(g_last_tlm._pad, 0u, "tlm_payload: _pad = 0");
}

static void test_tlm_target_x10_negative(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    s.target_omega_dps = -30.7f;  /* → target_x10 = -307 */
    g_tlm_count = 0;
    steering_pid_send_tlm(&s, 0);
    EXPECT_EQ(g_last_tlm.target_x10, -307, "tlm_payload: negative target_x10");
}

static void test_tlm_int16_clamp_positive(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    s.target_omega_dps = 4000.0f;  /* × 10 = 40000 > INT16_MAX */
    g_tlm_count = 0;
    steering_pid_send_tlm(&s, 0);
    EXPECT_EQ(g_last_tlm.target_x10, 32767, "tlm_clamp: target_x10 clamped to INT16_MAX");
}

static void test_tlm_int16_clamp_negative(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    s.target_omega_dps = -4000.0f;  /* × 10 = -40000 < INT16_MIN */
    g_tlm_count = 0;
    steering_pid_send_tlm(&s, 0);
    EXPECT_EQ(g_last_tlm.target_x10, (int16_t)-32768, "tlm_clamp: target_x10 clamped to INT16_MIN");
}

static void test_balance_not_disturbed_by_small_step(void)
{
    /*
     * A balance bot is most sensitive to sudden changes in steer_cmd.
     * Verify that a large target step at t=0 only increments output by
     * STEER_RAMP_RATE_PER_MS per millisecond — not a step change.
     * This confirms the rate limiter protects the balance PID.
     */
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 100.0f);   /* large yaw demand */

    /* 10ms at 1kHz */
    for (int i = 0; i < 10; i++) {
        int16_t pre  = s.output;
        int16_t post = steering_pid_update(&s, 0.0f, 0.001f);
        int16_t step = post - pre;
        EXPECT_TRUE(step <= STEER_RAMP_RATE_PER_MS, "balance_protect: step bounded each tick");
        EXPECT_TRUE(step >= -STEER_RAMP_RATE_PER_MS, "balance_protect: neg step bounded each tick");
    }
}

static void test_derivative_on_error_change(void)
{
    /*
     * When error changes suddenly (e.g. actual_omega jumps), the derivative
     * term should add a braking contribution.
     * With STEER_KD=0.05 and a 10 deg/s²-equivalent error rate change:
     *   d_term = 0.05 * (error1 - error0) / dt
     * We just verify output changes when error changes, indicating D is active.
     */
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 20.0f);

    /* Step 1: actual = 0, error = 20 */
    int16_t out1 = steering_pid_update(&s, 0.0f, 0.001f);

    /* Step 2: actual jumps to 20, error = 0. D will oppose. */
    int16_t out2 = steering_pid_update(&s, 20.0f, 0.001f);

    /* out2 should be less than out1 (derivative braking as error drops) */
    EXPECT_TRUE(out2 <= out1, "derivative: braking when error shrinks");
}

static void test_reset_then_restart(void)
{
    steering_pid_t s;
    steering_pid_init(&s);
    steering_pid_set_target(&s, 50.0f);
    for (int i = 0; i < 100; i++) {
        steering_pid_update(&s, 0.0f, 0.001f);
    }

    steering_pid_reset(&s);
    EXPECT_EQ  (s.output,            0,    "reset_restart: output 0");
    EXPECT_NEAR(s.integral,  0.0f, 1e-6f,  "reset_restart: integral 0");
    EXPECT_NEAR(s.target_omega_dps, 0.0f, 1e-6f, "reset_restart: target 0");

    /* After reset, new target should work cleanly */
    steering_pid_set_target(&s, 10.0f);
    int16_t out = steering_pid_update(&s, 0.0f, 0.001f);
    EXPECT_TRUE(out > 0, "reset_restart: positive output after fresh start");
}

static void test_tlm_hz_constant(void)
{
    EXPECT_TRUE(STEER_TLM_HZ == 10u, "tlm_hz: STEER_TLM_HZ is 10");
}

static void test_output_max_constant(void)
{
    EXPECT_TRUE(STEER_OUTPUT_MAX == 400, "output_max: STEER_OUTPUT_MAX is 400");
}

/* ---- main ---- */
int main(void)
{
    printf("=== test_steering_pid ===\n\n");

    test_init();
    test_reset();
    test_disabled_returns_zero();
    test_zero_dt_returns_zero();
    test_negative_dt_returns_zero();
    test_proportional_only();
    test_converges_to_setpoint();
    test_output_clamped_to_max();
    test_output_clamped_negative();
    test_anti_windup();
    test_anti_windup_negative();
    test_rate_limiter_single_step();
    test_rate_limiter_larger_dt();
    test_rate_limiter_decreasing();
    test_zero_error_zero_output();
    test_enable_disable_clears_state();
    test_re_enable();
    test_set_target_ignored_when_disabled();
    test_actual_omega_updated();
    test_get_output_matches_update();
    test_omega_scale_convention();
    test_tlm_first_call_fires();
    test_tlm_rate_limited();
    test_tlm_payload_target();
    test_tlm_payload_actual();
    test_tlm_payload_output();
    test_tlm_payload_enabled_flag();
    test_tlm_payload_pad_zero();
    test_tlm_target_x10_negative();
    test_tlm_int16_clamp_positive();
    test_tlm_int16_clamp_negative();
    test_balance_not_disturbed_by_small_step();
    test_derivative_on_error_change();
    test_reset_then_restart();
    test_tlm_hz_constant();
    test_output_max_constant();

    printf("\nResults: %d passed, %d failed\n", s_pass, s_fail);
    return s_fail ? 1 : 0;
}