// =============================================================================
//  median.h -- fixed-size median filter
// =============================================================================
//
//  Templated median filter that holds the last N samples in a ring buffer and
//  returns the median of the current window in O(N log N) (sort-copy). N is
//  expected to be small (3-9), so the cost is negligible at 100 Hz.
//
//  Used by:
//      - hal/rudder_sensor.cpp  (5-sample median to suppress ADC spikes)
//
//  Host-testable: depends only on the standard library. The Unity tests in
//  test/test_filters/ exercise it on the developer machine.
// =============================================================================

#pragma once

#include <algorithm>
#include <array>
#include <cstddef>

namespace arautopilot::filters {

template <typename T, std::size_t N>
class MedianFilter {
    static_assert(N >= 1, "MedianFilter window must be at least 1");
    static_assert(N % 2 == 1, "MedianFilter window must be odd for an unambiguous median");

   public:
    MedianFilter() = default;

    /// Push one sample. Until N samples have been pushed the filter
    /// returns the median over only the samples received so far.
    T push(T sample) {
        buffer_[write_idx_] = sample;
        write_idx_ = (write_idx_ + 1) % N;
        if (count_ < N) {
            ++count_;
        }
        return median();
    }

    /// Median of the current window (without pushing). Returns T{} if
    /// no samples have been pushed yet.
    T median() const {
        if (count_ == 0) {
            return T{};
        }
        std::array<T, N> sorted{};
        for (std::size_t i = 0; i < count_; ++i) {
            sorted[i] = buffer_[i];
        }
        std::sort(sorted.begin(), sorted.begin() + count_);
        return sorted[count_ / 2];
    }

    /// Number of valid samples currently stored (0..N).
    std::size_t size() const { return count_; }

    /// True once the window has been filled at least once.
    bool is_full() const { return count_ == N; }

    /// Forget all samples; next push() starts fresh.
    void reset() {
        write_idx_ = 0;
        count_ = 0;
        buffer_.fill(T{});
    }

   private:
    std::array<T, N> buffer_{};
    std::size_t write_idx_{0};
    std::size_t count_{0};
};

}  // namespace arautopilot::filters
