refactor: move test_helpers, RunningQuality

pasada-lib/include/ssf_filter.h

@@ -6,6 +6,7 @@
 #define PASADASUPERPROJECT_SSF_FILTER_H
 
 #include "iir_filter.h"
+#include <deque>
 
 #define FPS 60
 #define MAX_BPM 300
@@ -53,4 +54,52 @@ public:
     double peek_threshold();
 };
 
+/**
+ * Running signal quality indicator.
+ */
+class RunningQuality {
+protected:
+    // TODO: make it a filter (output proper samples)
+
+    /** template beat is resampled to this #samples */
+    const int BEAT_LEN = 120 /* 2*FPS for 30 bpm lower end */;
+
+    /** threshold for accepting initial beats */
+    const double BEAT_CORR_THR_1 = 0.9;
+    /** threshold for accepting subsequent beats */
+    const double BEAT_CORR_THR_2 = 0.8;
+    /** absolute SSF threshold for accepting any beat */
+    const double SSF_THRESHOLD = 5.0;
+    /** number of recent beats to use for beat template. must be even (alternating feet have different patterns; make it symmetric) */
+    const int    NUM_BEATS = 4;
+
+    std::deque<std::vector<double> > beatTemplates;
+    std::vector<double> beatTemplate;
+    //std::vector<std::pair<int, int> > badBeatRanges;
+    double beatCorrThr2;
+    bool justLocked;
+    int idx;
+
+    /** for debugging only - disable SSF_THRESHOLD */
+    bool disableSsf;
+
+    void addTemplate(std::vector<double>& x);
+
+    void replaceTemplate(std::vector<double>& x);
+
+    virtual void dispatchLocked();
+    virtual void dispatchBeat(int idx, bool good, double posCorr);
+
+public:
+    RunningQuality();
+    explicit RunningQuality(bool disableSsf);
+    virtual ~RunningQuality();
+
+    // note: arg should be an iterator really, but can do later
+    /**
+     * @param beat individual beat accelero signal
+     */
+    void append(std::vector<double> &rawBeat, std::vector<double> &rawSsf);
+};
+
 #endif //PASADASUPERPROJECT_SSF_FILTER_H

pasada-lib/ssf_filter.cpp

@@ -2,9 +2,9 @@
 // Created by david on 03.03.2026.
 //
 
-#include "include/ssf_filter.h"
+#include "ssf_filter.h"
+#include "pd_signal.h"
 #include <limits>
-#include <cmath>
 #include <cassert>
 #include <iostream>
 
@@ -86,3 +86,87 @@ double SsfStepDetector::filter(double ssf) {
 double SsfStepDetector::peek_threshold() {
     return ssf_threshold;
 }
+
+
+void RunningQuality::addTemplate(std::vector<double>& x) {
+    beatTemplates.emplace_back(x);
+    while (beatTemplates.size() > NUM_BEATS) {
+        // sliding window on 'beat_templates', do not use all history
+        beatTemplates.pop_front();
+    }
+    pd_signal::mean(beatTemplate, beatTemplates);
+}
+
+void RunningQuality::replaceTemplate(std::vector<double>& x) {
+    beatTemplates.clear();
+    beatTemplates.emplace_back(x);
+    // essentially just a copy
+    pd_signal::mean(beatTemplate, beatTemplates);
+}
+
+void RunningQuality::dispatchLocked() { /* implement me, add Listener etc. */ }
+void RunningQuality::dispatchBeat(int idx, bool good, double posCorr) { /* implement me, add Listener etc. */ }
+
+RunningQuality::RunningQuality(): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {}
+RunningQuality::RunningQuality(bool disableSsf): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(disableSsf) {}
+RunningQuality::~RunningQuality() {}
+
+// note: arg should be an iterator really, but can do later
+void RunningQuality::append(std::vector<double> &rawBeat, std::vector<double> &rawSsf) {
+    // TODO: should ignore crazy-long and very short beats here. (filter up on beat detector)
+
+    std::vector<double> beat;
+    std::vector<double> ssf;
+    pd_signal::resample(beat, rawBeat, BEAT_LEN);
+    pd_signal::resample(ssf, rawSsf, BEAT_LEN);
+    //std::ranges::copy(rawBeat, std::back_inserter(beat));
+
+    // check ssf at sample 2 (mid-slope of 4 window of ssf)
+    // TODO: param upon SsfFilter.upslope_width/2 instead of hardcoding
+    double checkedSsf = ssf[(int) (2*((double)beat.size())/((double)rawBeat.size()))];
+
+    double corr = std::numeric_limits<double>::quiet_NaN();
+    double posCorr = std::numeric_limits<double>::quiet_NaN();
+    bool goodBeat = false;
+    if (beatTemplates.size() > 0) {
+        corr = pd_signal::crossCorr(ssf, beatTemplate);
+        posCorr = pd_signal::clip(corr, 0.0, 1.0);
+        double corrThreshold = (beatTemplates.size() > 2) ? beatCorrThr2 : BEAT_CORR_THR_1;
+        goodBeat = (corr > corrThreshold) && (checkedSsf > SSF_THRESHOLD || disableSsf);
+    }
+
+    if (beatTemplates.size() == 0) {
+        // cannot correlate the first beat, no template yet
+        std::cerr << "(0) first beat -> addTemplate()" << std::endl;
+        addTemplate(beat);
+        justLocked = false;
+    } else if (beatTemplates.size() <= 2) {
+        // restart if there is no clear correlation between beats
+        if (goodBeat) {
+            std::cerr << "(2) good initial beat -> addTemplate()" << std::endl;
+            addTemplate(beat);
+            if (beatTemplates.size() > 2)
+                justLocked = true; // TODO why not set? wrong compiler optimization? (is it unaware of member change?)
+            //std::cerr << "  (2) beatTemplates.size()=" << beatTemplates.size() << " justLocked=" << ((int) justLocked) << std::endl;
+        } else {
+            std::cerr << "(2) bad initial beat -> replaceTemplate()" << std::endl;
+            replaceTemplate(beat);
+            //badBeatRanges.clear();
+            justLocked = false;
+        }
+    } else {
+        // running mode: collect bad beats, but may be OK not to restart immediately
+
+        std::cerr << "(3) running mode, good=" << ((int) goodBeat) << " justLocked=" << ((int) justLocked) << std::endl;
+        if (goodBeat) {
+            addTemplate(beat);
+        } else {
+            // badBeatRanges.add(s, e)
+            // numNoisy++
+        }
+        // runningCorrs.add(posCorr)
+        if (justLocked) { dispatchLocked(); justLocked = false; }
+        dispatchBeat(idx, goodBeat, posCorr);
+    }
+    idx++;
+}