//
// Created by david on 15.03.2026.
//

#include "step_detector.h"

StepDetector::StepDetector(double fps, StepListener *listener, bool debug) :
    listener(listener),
    f_neg(1, 0, 0, std::vector<double> {-1.0}),
    f_ssf(fps),
    f_ssd(fps),
    f_sqi(fps),
    debug(debug)
{}

#if (FPS != 60)
#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
#endif

void StepDetector::filter(std::vector<float> values) {
    // TODO: later on, we should use a vector projection towards gravity
    auto s1 = (double) values[1]; // take y-axis value for now
    auto s2 = f_neg.filter(s1);
    auto s3 = f_ssf.filter(s2);
    auto s4 = f_ssd.filter(s3);
    auto q5 = f_sqi.filter(s2, s3, s4);
    if (debug) {
        buf_ssd.push_back(s4);
        buf_sqi.push_back(q5);
        buf_out.push_back(s4 * (q5 > 0.0 ? 1.0 : 0.0));
    }
    // is step, step quality is OK, and we have a listener?
    if(s4 > 0.0 && q5 > 0.0 && listener != nullptr) {
        listener->playBeat();
    }
}

std::vector<double> StepDetector::getBufSsd() { return buf_ssd; }
std::vector<double> StepDetector::getBufSqi() { return buf_sqi; }
std::vector<double> StepDetector::getBufOut() { return buf_out; }

void StepDetector::primeFilters(double fps, std::vector<double> sig) {
    const size_t N_INIT = SsfStepDetector::initial_samples(fps);
    // initialize: feed for priming the filters
    for (size_t i = 0; i < N_INIT; i++) {
        const auto a_i = static_cast<float>(sig[i]);
        filter(std::vector<float> {0.0f, a_i, 0.0f});
    }
    // clear debug buffers
    buf_ssd.clear();
    buf_sqi.clear();
    buf_out.clear();
}