// example_template_pid.cpp
// Demonstrating compile-time PID computation
// No STM32 hardware required — pure C++ template practice

#include <cstdio>
#include <cmath>
#include <type_traits>
#include <array>

// ============================================================
// PID Controller Template
// ============================================================
template<typename T, bool USE_DERIVATIVE = true>
class Pid {
    static_assert(std::is_floating_point<T>::value,
                  "PID requires floating point type");

public:
    constexpr Pid(T kp, T ki, T kd)
        : kp_(kp), ki_(ki), kd_(kd)
        , integral_(0), prev_error_(0)
    {}

    T Update(T setpoint, T measurement, T dt) noexcept {
        T error = setpoint - measurement;

        // P
        T p_term = kp_ * error;

        // I (with anti-windup)
        integral_ += error * dt;
        integral_ = Clamp(integral_, -100.0f, 100.0f);
        T i_term = ki_ * integral_;

        // D (on measurement change, avoids derivative kick)
        T d_term = 0;
        if constexpr (USE_DERIVATIVE) {
            T measurement_derivative = (measurement - prev_measurement_) / dt;
            d_term = kd_ * (-measurement_derivative);
        }

        prev_error_ = error;
        prev_measurement_ = measurement;

        return Clamp(p_term + i_term + d_term, -1000.0f, 1000.0f);
    }

    void Reset() noexcept {
        integral_ = 0;
        prev_error_ = 0;
    }

    T Output() const noexcept { return 0; }

private:
    static constexpr T Clamp(T value, T min, T max) noexcept {
        return (value < min) ? min : (value > max) ? max : value;
    }

    T kp_, ki_, kd_;
    T integral_, prev_error_, prev_measurement_ = 0;
};

// ============================================================
// Moving Average Filter Template
// ============================================================
template<typename T, size_t WINDOW>
class MovingAverage {
public:
    T Update(T input) noexcept {
        sum_ -= buffer_[index_];
        sum_ += input;
        buffer_[index_] = input;
        index_ = (index_ + 1) % WINDOW;
        if (count_ < WINDOW) count_++;
        return sum_ / static_cast<T>(count_);
    }

    void Reset() noexcept {
        buffer_.fill(0);
        sum_ = 0;
        count_ = 0;
        index_ = 0;
    }

private:
    std::array<T, WINDOW> buffer_{};
    size_t index_ = 0;
    size_t count_ = 0;
    T sum_ = 0;
};

// ============================================================
// Compile-time Math
// ============================================================
namespace math {
    template<typename T>
    constexpr T Clamp(T value, T min, T max) noexcept {
        return (value < min) ? min : (value > max) ? max : value;
    }

    template<typename T>
    constexpr T Map(T x, T in_min, T in_max, T out_min, T out_max) noexcept {
        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
    }

    template<typename T, size_t N>
    constexpr T Average(const T (&arr)[N]) noexcept {
        T sum = 0;
        for (size_t i = 0; i < N; i++)
            sum += arr[i];
        return sum / N;
    }
}

// ============================================================
// Test / Demo
// ============================================================
int main() {
    printf("=== C++ Template PID Demo ===\n\n");

    // PID with derivative
    Pid<float, true> pid(30.0f, 0.5f, 1.5f);

    // Simulate step response
    float measurement = 0;
    float dt = 0.005f;  // 200Hz

    printf("Time(s)\tSetpoint\tMeasurement\tOutput\n");
    for (int i = 0; i < 200; i++) {
        float t = i * dt;
        float sp = (t < 0.5f) ? 0.0f : 30.0f;  // Step at t=0.5s
        float output = pid.Update(sp, measurement, dt);

        // Simple plant model (1st order + inertia)
        measurement += (output * 0.1f - measurement) * dt * 5.0f;

        if (i % 10 == 0) {
            printf("%.3f\t%.1f\t\t%.1f\t\t%.1f\n",
                   t, sp, measurement, output);
        }
    }

    // Compile-time constant computed
    constexpr float clamped = math::Clamp(150.0f, 0.0f, 100.0f);
    printf("\nCompile-time clamp: %f (should be 100)\n", clamped);

    constexpr int sensor_values[] = {1024, 1050, 980, 1010, 1030};
    constexpr float avg = math::Average(sensor_values);
    printf("Compile-time average: %f\n", avg);

    // Moving average filter demo
    MovingAverage<float, 4> mavg;
    float noisy_data[] = {10.0f, 20.0f, 30.0f, 40.0f, 50.0f, 60.0f};
    printf("\nMoving average filter:\n");
    for (float v : noisy_data) {
        printf("In: %.0f → Out: %.1f\n", v, mavg.Update(v));
    }

    // Type-safety check: uncommenting this won't compile
    // Pid<int> bad_pid(1, 2, 3);  // static_assert will fire!

    printf("\n=== Done ===\n");
    return 0;
}