// =============================================================================
//  test_median.cpp -- Unity host-side tests for MedianFilter<T, N>
// =============================================================================
//
//  Runs on the developer machine via:
//      .venv/Scripts/pio.exe test -e native
//
//  No ESP32 hardware required. Tests the pure-C++ template in
//  src/filters/median.h.
// =============================================================================

#include <unity.h>

// Compile-time switch: when AR_HOST_TEST is defined we drop the Arduino.h
// dependency. The median filter only needs <algorithm>, <array>, <cstddef>.
#include "../../src/filters/median.h"

using arautopilot::filters::MedianFilter;

void setUp() {}
void tearDown() {}

void test_empty_filter_returns_default_constructed_value() {
    MedianFilter<int, 5> f;
    TEST_ASSERT_EQUAL_INT(0, f.median());
    TEST_ASSERT_EQUAL_size_t(0, f.size());
    TEST_ASSERT_FALSE(f.is_full());
}

void test_single_sample_returns_itself() {
    MedianFilter<int, 5> f;
    TEST_ASSERT_EQUAL_INT(42, f.push(42));
    TEST_ASSERT_EQUAL_size_t(1, f.size());
    TEST_ASSERT_FALSE(f.is_full());
}

void test_two_samples_returns_upper_of_pair() {
    MedianFilter<int, 5> f;
    f.push(10);
    // size = 2, median index = 2/2 = 1, so the second element after sort.
    TEST_ASSERT_EQUAL_INT(20, f.push(20));
    TEST_ASSERT_EQUAL_INT(20, f.push(15));   // sorted: 10,15,20 -> 15
    TEST_ASSERT_EQUAL_INT(15, f.median());
}

void test_filter_suppresses_single_spike() {
    // A common scenario: 100 100 99 9999 101 -- the median ignores 9999.
    MedianFilter<int, 5> f;
    f.push(100);
    f.push(100);
    f.push(99);
    f.push(9999);
    f.push(101);
    TEST_ASSERT_TRUE(f.is_full());
    TEST_ASSERT_EQUAL_INT(100, f.median());
}

void test_filter_window_is_circular() {
    MedianFilter<int, 3> f;
    f.push(1);
    f.push(2);
    f.push(3);                  // window = [1,2,3] -> median 2
    TEST_ASSERT_EQUAL_INT(2, f.median());
    f.push(100);                // window = [100,2,3] -> median 3
    TEST_ASSERT_EQUAL_INT(3, f.median());
    f.push(100);                // window = [100,100,3] -> median 100
    TEST_ASSERT_EQUAL_INT(100, f.median());
    f.push(100);                // window = [100,100,100] -> median 100
    TEST_ASSERT_EQUAL_INT(100, f.median());
}

void test_reset_clears_state() {
    MedianFilter<int, 5> f;
    f.push(1);
    f.push(2);
    f.push(3);
    f.reset();
    TEST_ASSERT_EQUAL_size_t(0, f.size());
    TEST_ASSERT_FALSE(f.is_full());
    TEST_ASSERT_EQUAL_INT(0, f.median());
    TEST_ASSERT_EQUAL_INT(42, f.push(42));
}

void test_negative_values() {
    MedianFilter<int16_t, 5> f;
    f.push(-100);
    f.push(-50);
    f.push(0);
    f.push(50);
    f.push(100);
    TEST_ASSERT_EQUAL_INT16(0, f.median());
}

void test_window_of_one_is_identity() {
    MedianFilter<int, 1> f;
    TEST_ASSERT_EQUAL_INT(7, f.push(7));
    TEST_ASSERT_EQUAL_INT(9, f.push(9));
    TEST_ASSERT_TRUE(f.is_full());
}

int main(int /*argc*/, char** /*argv*/) {
    UNITY_BEGIN();
    RUN_TEST(test_empty_filter_returns_default_constructed_value);
    RUN_TEST(test_single_sample_returns_itself);
    RUN_TEST(test_two_samples_returns_upper_of_pair);
    RUN_TEST(test_filter_suppresses_single_spike);
    RUN_TEST(test_filter_window_is_circular);
    RUN_TEST(test_reset_clears_state);
    RUN_TEST(test_negative_values);
    RUN_TEST(test_window_of_one_is_identity);
    return UNITY_END();
}
