/*
 * test_slope_estimator.c — Unit tests for slope estimator (Issue #600).
 *
 * Build (host, no hardware):
 *   gcc -I include -I test/stubs -DTEST_HOST -lm \
 *       -o /tmp/test_slope src/slope_estimator.c test/test_slope_estimator.c
 *
 * All tests are self-contained; no HAL, no ADC, no UART required.
 * slope_estimator.c calls jlink_send_slope_tlm() which is stubbed below.
 */

/* ---- Stubs ---- */

/* Prevent jlink.h from pulling in HAL / pid_flash types */
#define JLINK_H
#include <stdint.h>
#include <stdbool.h>

/* Minimal jlink_tlm_slope_t matching the real definition */
typedef struct __attribute__((packed)) {
    int16_t  slope_x100;
    uint8_t  active;
    uint8_t  _pad;
} jlink_tlm_slope_t;

/* Capture last transmitted tlm for inspection */
static jlink_tlm_slope_t g_last_tlm;
static int g_tlm_count = 0;

void jlink_send_slope_tlm(const jlink_tlm_slope_t *tlm)
{
    g_last_tlm = *tlm;
    g_tlm_count++;
}

/* ---- Include implementation directly ---- */
#include "../src/slope_estimator.c"

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

static int g_pass = 0;
static int g_fail = 0;

#define ASSERT(cond, msg) do { \
    if (cond) { g_pass++; } \
    else { g_fail++; printf("FAIL [%s:%d] %s\n", __FILE__, __LINE__, msg); } \
} while(0)

#define ASSERT_NEAR(a, b, tol, msg) \
    ASSERT(fabsf((a)-(b)) <= (tol), msg)

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

static void test_init_zeroes_state(void)
{
    slope_estimator_t se;
    /* dirty the memory first */
    memset(&se, 0xAB, sizeof(se));
    slope_estimator_init(&se);
    ASSERT(se.estimate_deg == 0.0f,  "init: estimate_deg == 0");
    ASSERT(se.enabled == true,        "init: enabled == true");
}

static void test_get_when_disabled_returns_zero(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    /* Force a non-zero estimate by running a few ticks, then disable */
    slope_estimator_update(&se, 10.0f, 0.1f);
    slope_estimator_set_enabled(&se, false);
    ASSERT(slope_estimator_get_deg(&se) == 0.0f,
           "get while disabled must return 0");
}

static void test_update_when_disabled_no_change(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    slope_estimator_set_enabled(&se, false);
    slope_estimator_update(&se, 10.0f, 0.01f);
    ASSERT(se.estimate_deg == 0.0f,
           "update while disabled must not change estimate");
}

static void test_update_zero_dt_no_change(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    slope_estimator_update(&se, 5.0f, 0.0f);
    ASSERT(se.estimate_deg == 0.0f,
           "update with dt=0 must not change estimate");
}

static void test_update_negative_dt_no_change(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    slope_estimator_update(&se, 5.0f, -0.001f);
    ASSERT(se.estimate_deg == 0.0f,
           "update with dt<0 must not change estimate");
}

static void test_single_step_converges_toward_input(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    /* One update with dt=0.01s, pitch=10 deg */
    slope_estimator_update(&se, 10.0f, 0.01f);
    /* alpha = 0.01 / (5.0 + 0.01) ≈ 0.002; new estimate ≈ 0 + 0.002*10 = 0.02 */
    ASSERT(se.estimate_deg > 0.0f,
           "estimate should move toward positive pitch");
    ASSERT(se.estimate_deg < 10.0f,
           "estimate should not jump to full pitch in one step");
}

static void test_iir_reaches_63pct_in_one_tau(void)
{
    /* Run estimator at 1 kHz for exactly SLOPE_TAU_S seconds */
    slope_estimator_t se;
    slope_estimator_init(&se);
    const float pitch   = 10.0f;
    const float dt      = 0.001f;
    const int   steps   = (int)(SLOPE_TAU_S / dt);  /* 5000 steps */
    for (int i = 0; i < steps; i++) {
        slope_estimator_update(&se, pitch, dt);
    }
    /* IIR should be at ≈63.2% of the step */
    float expected_lo = pitch * 0.60f;
    float expected_hi = pitch * 0.66f;
    ASSERT(se.estimate_deg >= expected_lo && se.estimate_deg <= expected_hi,
           "IIR should reach ~63% of step at t=tau");
}

static void test_iir_reaches_63pct_at_100hz(void)
{
    /* Same tau, but at 100 Hz */
    slope_estimator_t se;
    slope_estimator_init(&se);
    const float pitch   = 8.0f;
    const float dt      = 0.01f;
    const int   steps   = (int)(SLOPE_TAU_S / dt);  /* 500 steps */
    for (int i = 0; i < steps; i++) {
        slope_estimator_update(&se, pitch, dt);
    }
    float expected_lo = pitch * 0.60f;
    float expected_hi = pitch * 0.66f;
    ASSERT(se.estimate_deg >= expected_lo && se.estimate_deg <= expected_hi,
           "IIR 100 Hz: should reach ~63% at t=tau");
}

static void test_positive_clamp(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    /* Drive with a huge pitch for a long time — estimate must clamp at SLOPE_MAX_DEG */
    for (int i = 0; i < 100000; i++) {
        slope_estimator_update(&se, 90.0f, 0.001f);
    }
    ASSERT_NEAR(se.estimate_deg, SLOPE_MAX_DEG, 0.001f,
                "estimate must clamp at +SLOPE_MAX_DEG");
}

static void test_negative_clamp(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    for (int i = 0; i < 100000; i++) {
        slope_estimator_update(&se, -90.0f, 0.001f);
    }
    ASSERT_NEAR(se.estimate_deg, -SLOPE_MAX_DEG, 0.001f,
                "estimate must clamp at -SLOPE_MAX_DEG");
}

static void test_reset_zeroes_estimate(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    /* Build up some estimate */
    for (int i = 0; i < 1000; i++) {
        slope_estimator_update(&se, 12.0f, 0.001f);
    }
    ASSERT(se.estimate_deg > 0.1f, "pre-reset: estimate should be non-zero");
    slope_estimator_reset(&se);
    ASSERT(se.estimate_deg == 0.0f, "post-reset: estimate should be zero");
    /* enabled state must be preserved */
    ASSERT(se.enabled == true, "reset must not change enabled flag");
}

static void test_reset_preserves_disabled_state(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    slope_estimator_set_enabled(&se, false);
    slope_estimator_reset(&se);
    ASSERT(se.enabled == false, "reset must not re-enable disabled estimator");
}

static void test_balance_setpoint_compensation(void)
{
    /*
     * Simulate: robot on 5-deg slope, pitch = 5 deg, slope estimate converges.
     * After convergence, tilt_corrected = pitch - slope ≈ 0.
     * Verify that the effective PID error (setpoint=0, tilt_corrected≈0) is near 0.
     */
    slope_estimator_t se;
    slope_estimator_init(&se);
    const float slope_deg = 5.0f;
    const float dt        = 0.001f;
    const int   steps     = (int)(3.0f * SLOPE_TAU_S / dt);  /* 3 tau = ~95% converged */

    for (int i = 0; i < steps; i++) {
        /* Robot holds steady on slope — pitch stays constant */
        slope_estimator_update(&se, slope_deg, dt);
    }

    float est   = slope_estimator_get_deg(&se);
    float tilt_corrected = slope_deg - est;

    /* After 3 tau, estimate should be within 5% of slope */
    ASSERT(est >= slope_deg * 0.90f, "3-tau estimate >= 90% of slope");
    /* tilt_corrected should be close to zero — minimal PID error */
    ASSERT(fabsf(tilt_corrected) < slope_deg * 0.10f,
           "corrected tilt error < 10% of slope after 3 tau");
}

static void test_fast_tilt_not_tracked(void)
{
    /*
     * Simulate: robot starts balanced (pitch≈0), then tips to 20 deg suddenly
     * for 100 ms (balance intervention), then returns to 0.
     * The slope estimate should not move significantly.
     */
    slope_estimator_t se;
    slope_estimator_init(&se);
    const float dt = 0.001f;

    /* 2 s of stable balancing at pitch ≈ 0 */
    for (int i = 0; i < 2000; i++) {
        slope_estimator_update(&se, 0.0f, dt);
    }
    float before = se.estimate_deg;

    /* 100 ms spike to 20 deg (fast tilt) */
    for (int i = 0; i < 100; i++) {
        slope_estimator_update(&se, 20.0f, dt);
    }
    /* Return to 0 for another 100 ms */
    for (int i = 0; i < 100; i++) {
        slope_estimator_update(&se, 0.0f, dt);
    }
    float after = se.estimate_deg;

    /* 200ms / 5000ms = 4% of tau — estimate should move < 1 deg */
    ASSERT(fabsf(after - before) < 1.0f,
           "fast tilt transient should not significantly move slope estimate");
}

static void test_slope_change_tracks_slowly(void)
{
    /* Robot transitions from flat (0°) to 10° slope at t=0.
     * After 1 tau, estimate should be ~63% of 10 = ~6.3 deg. */
    slope_estimator_t se;
    slope_estimator_init(&se);
    const float dt     = 0.001f;
    const int   steps  = (int)(SLOPE_TAU_S / dt);

    for (int i = 0; i < steps; i++) {
        slope_estimator_update(&se, 10.0f, dt);
    }
    ASSERT(se.estimate_deg >= 6.0f && se.estimate_deg <= 6.5f,
           "slope change: 1-tau estimate should be 6.0-6.5 deg");
}

static void test_symmetry_negative_slope(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    const float dt    = 0.001f;
    const int   steps = (int)(SLOPE_TAU_S / dt);

    for (int i = 0; i < steps; i++) {
        slope_estimator_update(&se, -8.0f, dt);
    }
    /* Should be ~63% of -8 */
    ASSERT(se.estimate_deg <= -4.0f && se.estimate_deg >= -5.5f,
           "negative slope: estimate should converge symmetrically");
}

static void test_enable_disable_toggle(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);

    /* Build estimate */
    for (int i = 0; i < 2000; i++) {
        slope_estimator_update(&se, 10.0f, 0.001f);
    }
    float val = se.estimate_deg;
    ASSERT(val > 0.1f, "estimate built before disable");

    /* Disable: estimate frozen */
    slope_estimator_set_enabled(&se, false);
    slope_estimator_update(&se, 10.0f, 0.001f);
    ASSERT(se.estimate_deg == val, "estimate frozen while disabled");

    /* get returns 0 when disabled */
    ASSERT(slope_estimator_get_deg(&se) == 0.0f, "get returns 0 while disabled");

    /* Re-enable: estimate resumes */
    slope_estimator_set_enabled(&se, true);
    slope_estimator_update(&se, 10.0f, 0.001f);
    ASSERT(se.estimate_deg > val, "estimate resumes after re-enable");
}

static void test_tlm_rate_limiting(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);

    g_tlm_count = 0;

    /* Call at 1000 Hz for 3 seconds → should send 3 TLMs (at 1 Hz rate limit) */
    for (uint32_t ms = 0; ms < 3000; ms++) {
        slope_estimator_send_tlm(&se, ms);
    }
    /* First call at ms=0 sends, then 1000ms, 2000ms → 3 total */
    ASSERT(g_tlm_count == 3, "TLM rate-limited to SLOPE_TLM_HZ (1 Hz)");
}

static void test_tlm_payload_encoding(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);

    /* Force a known estimate */
    se.estimate_deg = 7.5f;
    se.enabled = true;

    g_tlm_count = 0;
    g_last_tlm.slope_x100 = 0;

    /* Trigger a TLM by advancing time enough */
    slope_estimator_send_tlm(&se, 0u);

    ASSERT(g_tlm_count == 1, "TLM sent");
    ASSERT(g_last_tlm.slope_x100 == 750, "7.5 deg -> slope_x100 = 750");
    ASSERT(g_last_tlm.active == 1u, "active flag set when enabled");
}

static void test_tlm_disabled_active_flag(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    slope_estimator_set_enabled(&se, false);
    se.estimate_deg = 3.0f;

    g_tlm_count = 0;
    slope_estimator_send_tlm(&se, 0u);

    ASSERT(g_tlm_count == 1, "TLM sent when disabled");
    ASSERT(g_last_tlm.active == 0u, "active flag clear when disabled");
    /* slope_x100 reflects stored estimate but get() returns 0 */
    ASSERT(g_last_tlm.slope_x100 == 300, "slope_x100 encodes stored estimate");
}

static void test_tlm_clamped_to_int16(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    /* Manually set beyond int16 range for test */
    se.estimate_deg = 400.0f;  /* 400 * 100 = 40000, within int16 */
    se.enabled = true;
    g_tlm_count = 0;
    slope_estimator_send_tlm(&se, 0u);
    ASSERT(g_last_tlm.slope_x100 == 32767, "large estimate clamped to INT16_MAX");
}

static void test_multiple_resets_idempotent(void)
{
    slope_estimator_t se;
    slope_estimator_init(&se);
    slope_estimator_reset(&se);
    slope_estimator_reset(&se);
    ASSERT(se.estimate_deg == 0.0f, "multiple resets: still zero");
    ASSERT(se.enabled == true, "multiple resets: still enabled");
}

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

    test_init_zeroes_state();
    test_get_when_disabled_returns_zero();
    test_update_when_disabled_no_change();
    test_update_zero_dt_no_change();
    test_update_negative_dt_no_change();
    test_single_step_converges_toward_input();
    test_iir_reaches_63pct_in_one_tau();
    test_iir_reaches_63pct_at_100hz();
    test_positive_clamp();
    test_negative_clamp();
    test_reset_zeroes_estimate();
    test_reset_preserves_disabled_state();
    test_balance_setpoint_compensation();
    test_fast_tilt_not_tracked();
    test_slope_change_tracks_slowly();
    test_symmetry_negative_slope();
    test_enable_disable_toggle();
    test_tlm_rate_limiting();
    test_tlm_payload_encoding();
    test_tlm_disabled_active_flag();
    test_tlm_clamped_to_int16();
    test_multiple_resets_idempotent();

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