//
// Created by david on 02.03.2026.
//

#include "iir_filter.h"
#include <iostream>

#ifndef DEBUG_IIR
#define DEBUG_IIR 0
#endif

#if (DEBUG_IIR == 1)
#define DEBUG_PRINT(expr) do { expr; } while (0)
#else
#define DEBUG_PRINT(expr) while(0) { expr; }
#endif

Buf::Buf(size_t N): N(N), n(0) {
    data.resize(N);
    data.assign(N, 0.0);
}
void Buf::push(double val) {
    data[n] = val;
    n = (n+1) % N;
}

Filt::Filt(size_t N, size_t shift, size_t offset, std::vector<double> taps): Buf(N), shift(shift), offset(offset), taps(taps) {
    if (taps.size() != N) throw std::invalid_argument("taps.size() != N");
}
double Filt::filter(double val) {
    this->push(val);
    return this->peek();
}
double Filt::peek() {
    double sum = 0;
    for (size_t i = offset; i < this->N; i++) {
        //size_t n = (this->n - i + shift - 1) % this->size; // unsigned % size ... bad if u is negative
        size_t n = (this->N + this->n - i + shift - 1) % this->N;
        DEBUG_PRINT(std::cout << " t[" << i << "] * v[" << n << "]" << std::endl);
        sum += this->data[n] * this->taps[i];
    }
    return sum;
}
void Filt::push(double val) {
    Buf::push(val);
}
void Filt::prime(double val) {
    data.assign(this->N, val);
}
size_t Filt::size() {
    return this->N;
}

IirFilter::IirFilter(std::vector<double> b, std::vector<double> a) : x(b.size(), 0, 0, b), y(a.size(), 1, 1, a) {
    if (b.size() != a.size()) throw std::invalid_argument("b.size() != a.size()");
}
double IirFilter::filter(double val) {
    DEBUG_PRINT(std::cout << "x.filter(" << val << ")" << std::endl);
    double xv = x.filter(val);
    DEBUG_PRINT(std::cout << "xv=" << xv << std::endl);
    DEBUG_PRINT(std::cout << "y.peek()" << std::endl);
    double yv = y.peek();
    DEBUG_PRINT(std::cout << "yv=" << yv << std::endl);
    DEBUG_PRINT(std::cout << "---" << std::endl);
    double yo = xv - yv;
    y.push(yo);
    return yo;
}