// ============================================================================
// Native Unity tests — wfx_features.cpp  (WaveForge Expose L0/L1 emitter)
//
//   pio test -e native -f test_wfx_features
//
// Strategy: include wfx_features.cpp directly as a single TU (same pattern
// as test_command_handler / test_band_bridge).  Stub out mesh_publish_raw so
// no radio hardware is required.
// ============================================================================

#include <unity.h>
#include <cstdint>
#include <cstring>
#include <cmath>

// ---------------------------------------------------------------------------
// Stubs — replace dependencies that need radio / Arduino runtime
// ---------------------------------------------------------------------------

// mesh_publish_raw stub: records what was sent for inspection
static bool     g_publish_called = false;
static uint8_t  g_publish_buf[80];   // >= WFX_PACKET_SIZE (67)
static size_t   g_publish_len = 0;
static bool     g_publish_ret = true;

bool mesh_publish_raw(const uint8_t* data, size_t len) {
    g_publish_called = true;
    if (len > sizeof(g_publish_buf)) len = sizeof(g_publish_buf);
    memcpy(g_publish_buf, data, len);
    g_publish_len = len;
    return g_publish_ret;
}

// config.h DEFAULT_SENSOR_ID — also defined in the real header, but we
// define it here to avoid pulling in the full config include chain.
#ifndef DEFAULT_SENSOR_ID
#define DEFAULT_SENSOR_ID 0x00000001u
#endif

// Include the implementation under test
#include "../../src/wfx_features.cpp"

// ---------------------------------------------------------------------------
// Helpers
// ---------------------------------------------------------------------------

// Build a sine-modulated 128-bin spectrum:
//   x[k] = 0.5 + 0.4 * sin(2*pi*k*freq_bins / N)
// This gives a non-flat, non-uniform distribution with known non-zero variance.
static void make_sine_spectrum(float* spec, size_t N, float freq_bins) {
    for (size_t k = 0; k < N; k++) {
        float phase = 2.0f * 3.14159265f * (float)k * freq_bins / (float)N;
        spec[k] = 0.5f + 0.4f * sinf(phase);
    }
}

// ---------------------------------------------------------------------------
// setUp / tearDown
// ---------------------------------------------------------------------------
void setUp(void) {
    wfx_features_init();
    g_publish_called = false;
    g_publish_len    = 0;
    g_publish_ret    = true;
    memset(g_publish_buf, 0, sizeof(g_publish_buf));  // sizeof = 80
}

void tearDown(void) {}

// ---------------------------------------------------------------------------
// Test: compute returns false with empty observation buffer
// ---------------------------------------------------------------------------
void test_compute_fails_with_no_observations(void) {
    float spectrum[128];
    make_sine_spectrum(spectrum, 128, 4.0f);

    WfxFeatureVec v;
    memset(&v, 0, sizeof(v));

    bool ok = wfx_features_compute(spectrum, 128, &v);
    TEST_ASSERT_FALSE(ok);
}

// ---------------------------------------------------------------------------
// Test: c20 is non-zero for a non-flat spectrum
// ---------------------------------------------------------------------------
void test_c20_nonzero_for_sine_spectrum(void) {
    // Prime the observation buffer so compute() doesn't early-exit
    wfx_features_observe_rssi_packet(-70, 10, 1000, 0);

    float spectrum[128];
    make_sine_spectrum(spectrum, 128, 4.0f);  // 4 cycles across 128 bins

    WfxFeatureVec v;
    memset(&v, 0, sizeof(v));

    bool ok = wfx_features_compute(spectrum, 128, &v);
    TEST_ASSERT_TRUE(ok);

    // Variance of a sine wave 0.5 + 0.4*sin() is 0.4^2/2 = 0.08
    TEST_ASSERT_FLOAT_WITHIN(0.02f, 0.08f, v.c20_real);

    // Imaginary part must be exactly zero (real-valued proxy signal)
    TEST_ASSERT_FLOAT_WITHIN(1e-6f, 0.0f, v.c20_imag);
}

// ---------------------------------------------------------------------------
// Test: c40 is non-zero for a non-Gaussian spectrum
// ---------------------------------------------------------------------------
void test_c40_nonzero_for_sine_spectrum(void) {
    wfx_features_observe_rssi_packet(-65, 8, 2000, 0);

    float spectrum[128];
    make_sine_spectrum(spectrum, 128, 4.0f);

    WfxFeatureVec v;
    memset(&v, 0, sizeof(v));

    bool ok = wfx_features_compute(spectrum, 128, &v);
    TEST_ASSERT_TRUE(ok);

    // A sinusoidal distribution is decidedly non-Gaussian; kurtosis != 0
    TEST_ASSERT_TRUE(fabsf(v.c40_real) > 1e-4f);
    TEST_ASSERT_FLOAT_WITHIN(1e-6f, 0.0f, v.c40_imag);
}

// ---------------------------------------------------------------------------
// Test: occupancy is in [0, 1]
// ---------------------------------------------------------------------------
void test_occupancy_in_unit_interval(void) {
    // Inject 20 packet observations on band 0
    for (int i = 0; i < 20; i++) {
        wfx_features_observe_rssi_packet(-80, 5, (uint32_t)(i * 100), 0);
    }

    float spectrum[128];
    make_sine_spectrum(spectrum, 128, 2.0f);

    WfxFeatureVec v;
    memset(&v, 0, sizeof(v));

    bool ok = wfx_features_compute(spectrum, 128, &v);
    TEST_ASSERT_TRUE(ok);

    TEST_ASSERT_TRUE(v.occupancy >= 0.0f);
    TEST_ASSERT_TRUE(v.occupancy <= 1.0f);

    // 20 packets out of 64-slot buffer → 20/64 ≈ 0.3125
    TEST_ASSERT_FLOAT_WITHIN(0.02f, 20.0f / 64.0f, v.occupancy);
}

// ---------------------------------------------------------------------------
// Test: jitter is non-negative
// ---------------------------------------------------------------------------
void test_jitter_nonnegative(void) {
    // Inject packets with irregular spacing
    uint32_t ts[] = {0, 100, 250, 260, 500, 510, 900};
    for (size_t i = 0; i < 7; i++) {
        wfx_features_observe_rssi_packet(-75, 12, ts[i], 0);
    }

    float spectrum[128];
    make_sine_spectrum(spectrum, 128, 3.0f);

    WfxFeatureVec v;
    memset(&v, 0, sizeof(v));

    bool ok = wfx_features_compute(spectrum, 128, &v);
    TEST_ASSERT_TRUE(ok);

    TEST_ASSERT_TRUE(v.ia_jitter_ms >= 0.0f);
}

// ---------------------------------------------------------------------------
// Test: c21 matches m2 (for real-valued signal, C21 = E[|x|^2] = m2)
// ---------------------------------------------------------------------------
void test_c21_equals_mean_power(void) {
    wfx_features_observe_rssi_packet(-60, 15, 5000, 1);  // band 1

    float spectrum[128];
    make_sine_spectrum(spectrum, 128, 4.0f);

    // Manually compute E[x^2]
    float m2 = 0.0f;
    for (int k = 0; k < 128; k++) m2 += spectrum[k] * spectrum[k];
    m2 /= 128.0f;

    WfxFeatureVec v;
    memset(&v, 0, sizeof(v));

    bool ok = wfx_features_compute(spectrum, 128, &v);
    TEST_ASSERT_TRUE(ok);

    TEST_ASSERT_FLOAT_WITHIN(1e-5f, m2, v.c21);
}

// ---------------------------------------------------------------------------
// Test: publish builds a 64-byte packet with correct msg_type and CRC
// ---------------------------------------------------------------------------
void test_publish_packet_format(void) {
    WfxFeatureVec v;
    memset(&v, 0, sizeof(v));
    v.sensor_id    = 0xDEADBEEFu;
    v.timestamp_ms = 12345u;
    v.c20_real     = 0.08f;
    v.occupancy    = 0.3125f;
    v.band_id      = 0;

    bool ok = wfx_features_publish(v);
    TEST_ASSERT_TRUE(ok);
    TEST_ASSERT_TRUE(g_publish_called);
    // Packet layout: 1 (msg_type) + 4 (sensor_id) + 60 (WfxFeatureVec) + 2 (CRC) = 67
    TEST_ASSERT_EQUAL_size_t(67u, g_publish_len);

    // msg_type
    TEST_ASSERT_EQUAL_UINT8(0x10u, g_publish_buf[0]);

    // sensor_id big-endian
    TEST_ASSERT_EQUAL_UINT8(0xDE, g_publish_buf[1]);
    TEST_ASSERT_EQUAL_UINT8(0xAD, g_publish_buf[2]);
    TEST_ASSERT_EQUAL_UINT8(0xBE, g_publish_buf[3]);
    TEST_ASSERT_EQUAL_UINT8(0xEF, g_publish_buf[4]);

    // CRC verification (same algo as in wfx_features.cpp; covers bytes [0..64])
    auto crc16 = [](const uint8_t* d, size_t n) -> uint16_t {
        uint16_t crc = 0xFFFFu;
        for (size_t i = 0; i < n; i++) {
            crc ^= (uint16_t)d[i] << 8;
            for (int b = 0; b < 8; b++) {
                crc = (crc & 0x8000u) ? (uint16_t)((crc << 1) ^ 0x1021u)
                                      : (uint16_t)(crc << 1);
            }
        }
        return crc;
    };

    uint16_t expected_crc = crc16(g_publish_buf, 65);
    uint16_t got_crc = ((uint16_t)g_publish_buf[65] << 8) | g_publish_buf[66];
    TEST_ASSERT_EQUAL_UINT16(expected_crc, got_crc);
}

// ---------------------------------------------------------------------------
// Test: publish returns false when mesh_publish_raw fails
// ---------------------------------------------------------------------------
void test_publish_propagates_tx_failure(void) {
    g_publish_ret = false;

    WfxFeatureVec v;
    memset(&v, 0, sizeof(v));

    bool ok = wfx_features_publish(v);
    TEST_ASSERT_FALSE(ok);
    TEST_ASSERT_TRUE(g_publish_called);
}

// ---------------------------------------------------------------------------
// Test: observe_rssi_packet wraps correctly at PACKET_BUF_SIZE
// ---------------------------------------------------------------------------
void test_rolling_buffer_wraps(void) {
    // Fill beyond the 64-slot ring buffer
    for (int i = 0; i < 70; i++) {
        wfx_features_observe_rssi_packet(-80, 5, (uint32_t)(i * 50), 0);
    }

    float spectrum[128];
    make_sine_spectrum(spectrum, 128, 4.0f);

    WfxFeatureVec v;
    memset(&v, 0, sizeof(v));

    bool ok = wfx_features_compute(spectrum, 128, &v);
    TEST_ASSERT_TRUE(ok);

    // occupancy should be capped at 1.0 (full buffer)
    TEST_ASSERT_FLOAT_WITHIN(1e-5f, 1.0f, v.occupancy);

    // RSSI stats should still be valid (no crash, reasonable range)
    TEST_ASSERT_TRUE(v.rssi_mean < 0.0f);   // all RSSI values were negative
    TEST_ASSERT_TRUE(v.rssi_std >= 0.0f);
}

// ---------------------------------------------------------------------------
// Test: flat spectrum produces zero variance (c20 ≈ 0)
// ---------------------------------------------------------------------------
void test_flat_spectrum_zero_variance(void) {
    wfx_features_observe_rssi_packet(-70, 10, 1000, 0);

    float spectrum[128];
    for (int k = 0; k < 128; k++) spectrum[k] = 0.5f;  // perfectly flat

    WfxFeatureVec v;
    memset(&v, 0, sizeof(v));

    bool ok = wfx_features_compute(spectrum, 128, &v);
    TEST_ASSERT_TRUE(ok);

    TEST_ASSERT_FLOAT_WITHIN(1e-6f, 0.0f, v.c20_real);
}

// ---------------------------------------------------------------------------
// main
// ---------------------------------------------------------------------------
int main(int, char**) {
    UNITY_BEGIN();

    RUN_TEST(test_compute_fails_with_no_observations);
    RUN_TEST(test_c20_nonzero_for_sine_spectrum);
    RUN_TEST(test_c40_nonzero_for_sine_spectrum);
    RUN_TEST(test_occupancy_in_unit_interval);
    RUN_TEST(test_jitter_nonnegative);
    RUN_TEST(test_c21_equals_mean_power);
    RUN_TEST(test_publish_packet_format);
    RUN_TEST(test_publish_propagates_tx_failure);
    RUN_TEST(test_rolling_buffer_wraps);
    RUN_TEST(test_flat_spectrum_zero_variance);

    return UNITY_END();
}
