fix: RunningQuality must add ssf, not add beat

google-tests/CMakeLists.txt

@@ -12,6 +12,7 @@ add_executable(Google_Tests_run
         test1.cpp
         test2.cpp
         test3.cpp
+        test_helpers.cpp
 )
 
 file(COPY test1/data1.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
@@ -23,6 +24,8 @@ file(COPY test1/iir_t1_y.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test1)
 file(COPY test2/ssf_t2_acc.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test2)
 file(COPY test2/ssf_t2_y_ref.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test2)
 
+file(COPY test3/ssf_t3_acc.npy DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/test3)
+
 target_link_libraries(Google_Tests_run pasada)
 #target_include_directories(Google_Tests_run PRIVATE "${CMAKE_CURRENT_SOURCE_DIR}/pasada-lib/include")
 

google-tests/test2.cpp

@@ -6,56 +6,13 @@
 #include <vector>
 #include "iir_filter.h"
 #include "ssf_filter.h"
+#include "test_helpers.h"
 #include <cmath>
 #include <limits>
 
 #define FPS 60
 #define MAX_BPM 300
 
-template <typename T> static std::vector<double> apply_filter(T& filter, std::vector<double>& x) {
-    std::vector<double> y;
-    y.resize(x.size());
-    for (int i = 0; i < x.size(); i++) {
-        y[i] = filter.filter(x[i]);
-    }
-    return y;
-}
-
-static void npy_save(std::string path, std::vector<double>& x) {
-    npy::npy_data_ptr<double> d;
-    d.data_ptr = x.data();
-    d.shape = {(unsigned long) x.size()};
-    npy::write_npy(path, d);
-}
-
-static std::vector<double> fetch_y_axis(npy::npy_data<double>& acc) {
-    // TODO: later on, we should use a vector projection towards gravity
-    std::vector<double> signal;
-    const size_t rows_real = acc.shape[0];
-#if DEBUG_IIR == 1
-    const size_t rows = 5;
-#else
-    const size_t rows = acc.shape[0];
-#endif
-    int stride = 3;
-    int offset = 1; // [x,y,z] per row - fetch y
-    signal.resize(rows);
-    if (acc.fortran_order) {
-        stride = 1;
-        offset = (int) rows_real;
-    }
-    /*
-    std::cout << "is_fortran=" << acc.fortran_order << std::endl;
-    for (size_t i = 0; i < 10; i++) {
-        std::cout << "acc.data[" << i << "]=" << acc.data[i] << std::endl;
-    }
-    */
-    for (int i = 0; i < rows; i++) {
-        signal[i] = acc.data[i * stride + offset];
-    }
-    return signal;
-}
-
 TEST(HelloTest, Zong_SSF_Stage1) {
     npy::npy_data acc = npy::read_npy<double>("test2/ssf_t2_acc.npy");
     npy::npy_data y_ref = npy::read_npy<double>("test2/ssf_t2_y_ref.npy");
@@ -166,16 +123,6 @@ TEST(HelloTest, Zong_SSF_Stage2) {
     npy_save("test2/ssf_t2_ssf.npy", ssf);
 }
 
-/** Returns the ssf_threshold as the filter output for debugging. */
-class DebugSsfStepDetectorThreshold : public SsfStepDetector {
-public:
-    DebugSsfStepDetectorThreshold(size_t len_refr) : SsfStepDetector(len_refr) {}
-    double filter(double val) {
-        this->SsfStepDetector::filter(val);
-        return peek_threshold();
-    }
-};
-
 TEST(HelloTest, Zong_SSF_Stage3) {
     npy::npy_data acc = npy::read_npy<double>("test2/ssf_t2_acc.npy");
 

google-tests/test3.cpp

@@ -2,9 +2,14 @@
 // Created by david on 04.03.2026.
 //
 #include <gtest/gtest.h>
+#include "npy.hpp"
 //#include <utility>
 #include <deque>
+#include <iomanip>
+
 #include "pd_signal.h"
+#include "ssf_filter.h"
+#include "test_helpers.h"
 
 using namespace pd_signal;
 
@@ -28,6 +33,41 @@ TEST(SignalTest, interp_t1) {
     }
 }
 
+TEST(SignalTest, cross_corr_t1) {
+    std::vector<double> sig {0.9, 1.5, 2.0, 3.0, 5.0, 4.0, 1.0, 0.5, 0.3, 0.2};
+    double corr = pd_signal::crossCorr(sig, sig);
+    ASSERT_NEAR(1.0, corr, 1e-7);
+}
+
+TEST(SignalTest, cross_corr_t2) {
+    std::vector<double> x {0.9, 1.5, 2.0, 3.0, 5.0, 4.0, 1.0, 0.5, 0.3, 0.2};
+    std::vector<double> y {0.4, 0.7, 0.9, 1.5, 2.5, 2.0, 0.5, 0.25, 0.15, 0.1};
+    double corr = pd_signal::crossCorr(x, y);
+    ASSERT_NEAR(0.999, corr, 1e-3);
+}
+
+TEST(SignalTest, resample_t1) {
+    std::vector<double> x   {0.9,      1.5,       2.0,      3.0,      5.0,      4.0,      1.0,       0.5,      0.3,       0.2};
+    std::vector<double> y_e {0.9, 1.2, 1.5, 1.75, 2.0, 2.5, 3.0, 4.0, 5.0, 4.5, 4.0, 2.5, 1.0, 0.75, 0.5, 0.4, 0.3, 0.25, 0.2};
+
+    std::vector<double> t;
+    linspace(t, 0, (double) x.size()-1, y_e.size(), false);
+    // interp t=0.000 0.500 1.000 1.500 2.000 2.500 3.000 3.500 4.000 4.500 5.000 5.500 6.000 6.500 7.000 7.500 8.000 8.500 9.000
+    /*
+    std::cout << "interp t=";
+    for (size_t n = 0; n < t.size(); n++) {
+        std::cout << std::fixed << std::setw(5) << std::setprecision(3) << t[n] << " ";
+    }
+    std::cout << std::endl;
+    */
+
+    std::vector<double> y;
+    pd_signal::resample(y, x, y_e.size());
+
+    double corr = pd_signal::crossCorr(y, y_e);
+    ASSERT_NEAR(1.0, corr, 1e-3);
+}
+
 TEST(SignalTest, ranges) {
     const double abs_error = 1e-5;
     std::vector<double> i;
@@ -38,130 +78,14 @@ TEST(SignalTest, ranges) {
     ASSERT_NEAR(2.0, i[2], abs_error);
 }
 
-/**
- * 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) {
-        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 replaceTemplate(std::vector<double>& x) {
-        beatTemplates.clear();
-        beatTemplates.emplace_back(x);
-        // essentially just a copy
-        pd_signal::mean(beatTemplate, beatTemplates);
-    }
-
-    virtual void dispatchLocked() { /* implement me, add Listener etc. */ }
-    virtual void dispatchBeat(int idx, bool good, double posCorr) { /* implement me, add Listener etc. */ }
-
-public:
-    RunningQuality(): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(false) {}
-    explicit RunningQuality(bool disableSsf): beatCorrThr2(BEAT_CORR_THR_2), justLocked(false), idx(0), disableSsf(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) {
-        // TODO: should ignore crazy-long and very short beats here. (filter up on beat detector)
-
-        std::vector<double> beat;
-        std::vector<double> ssf;
-        resample(beat, rawBeat, BEAT_LEN);
-        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++;
-    }
-};
-
 class DebugRunningQuality : public RunningQuality {
 protected:
     virtual void dispatchLocked() { locked = true; }
-    virtual void dispatchBeat(int idx, bool good, double posCorr) { corrs.push_back(posCorr); }
+    virtual void dispatchBeat(int idx, bool good, double posCorr) { goods.push_back(good); corrs.push_back(posCorr); }
 
     bool locked;
     std::vector<double> corrs;
+    std::vector<bool> goods;
 
 public:
     DebugRunningQuality(): locked(false) {}
@@ -169,6 +93,7 @@ public:
     virtual ~DebugRunningQuality() {}
     bool isLocked() { return locked; }
     std::vector<double> getCorrs() { return corrs; }
+    std::vector<bool> getGoods() { return goods; }
     std::vector<double> getBeatTemplate() { return this->beatTemplate; }
 };
 
@@ -213,3 +138,81 @@ TEST(SignalTest, RunningQuality_t1) {
     //ASSERT_NEAR(0.7, sqi.getBeatTemplate()[4], 1e-6); // nb. resampled!
     ASSERT_NEAR(num/norm, sqi.getCorrs()[0], 1e-3);
 }
+
+TEST(SignalTest, RunningQuality_t2) {
+    npy::npy_data acc = npy::read_npy<double>("test3/ssf_t3_acc.npy");
+
+    std::vector<double> signal = fetch_y_axis(acc);
+
+#if (FPS != 60)
+#error "FPS must currently be 60, as highpass taps are pre-computed for that value"
+#endif
+
+    // TODO: SQI: cehck input file
+    // TODO: SQI: print debug values corr,idx, checkedSsf
+
+    // Butterworth filter: order=5, fc=0.5, fs=60, btype='highpass'
+    std::vector b {0.91875845, -4.59379227,  9.18758454, -9.18758454,  4.59379227, -0.91875845};
+    std::vector a {1.        , -4.83056552,  9.33652742, -9.02545247,  4.36360803, -0.8441171};
+    IirFilter filter(b, a);
+
+    //std::cerr << "before stage 1" << std::endl;
+
+    // Stage 1: high-pass
+    auto y = apply_filter(filter, signal);
+
+    Filt f_neg(1, 0, 0, std::vector {-1.0});
+    auto y_neg = apply_filter(f_neg, y);
+
+    //std::cerr << "before stage 2" << std::endl;
+
+    // Stage 2: sum slope function
+    const size_t upslope_width = 4;
+    SsfFilter f_ssf(upslope_width);
+    auto ssf = apply_filter(f_ssf, y_neg);
+
+    //std::cerr << "before stage 3" << std::endl;
+
+    // Stage 3: threshold detection
+    const size_t len_refr = (size_t) (FPS / (MAX_BPM / 60));
+    DebugSsfStepDetectorThreshold f_ssd_thr(len_refr);
+    auto ssf_threshold = apply_filter(f_ssd_thr, ssf);
+
+    //std::cerr << "before writing results 1 and doing step detection" << std::endl;
+
+    npy_save("test2/ssf_t3_ssf_threshold.npy", ssf_threshold);
+
+    SsfStepDetector f_ssd(len_refr);
+    auto steps = apply_filter(f_ssd, ssf);
+
+    //std::cerr << "before writing results 2" << std::endl;
+
+    npy_save("test2/ssf_t3_steps.npy", steps);
+
+    // Debug SQI
+
+    DebugRunningQuality sqi;
+
+    std::vector<double> beat_buf;
+    std::vector<double> ssf_buf;
+    // y, ssf
+    size_t N = y.size();
+    for (size_t i = 0; i < N; i++) {
+        if (steps[i] == 1.0) {
+            sqi.append(beat_buf, ssf_buf);
+            beat_buf.clear();
+            ssf_buf.clear();
+        }
+        beat_buf.push_back(y[i]);
+        ssf_buf.push_back(ssf[i]);
+    }
+
+    EXPECT_TRUE(sqi.isLocked());
+    EXPECT_TRUE(sqi.getCorrs().size() > 50);
+
+    std::vector<double> corrs(sqi.getCorrs());
+    npy_save("test3/ssf_t3_sqi_corrs.npy", corrs);
+
+    std::vector<bool> goods(sqi.getGoods());
+    npy_save("test3/ssf_t3_sqi_goods.npy", goods);
+}

google-tests/test_helpers.cpp

@@ -0,0 +1,53 @@
+//
+// Created by david on 11.03.2026.
+//
+
+#include "test_helpers.h"
+
+void npy_save(std::string path, std::vector<double>& x) {
+    npy::npy_data_ptr<double> d;
+    d.data_ptr = x.data();
+    d.shape = {(unsigned long) x.size()};
+    npy::write_npy(path, d);
+}
+void npy_save(std::string path, std::vector<bool>& x) {
+    npy::npy_data_ptr<int> d;
+    std::vector<int> xx(x.begin(), x.end());
+    d.data_ptr = xx.data();
+    d.shape = {(unsigned long) x.size()};
+    npy::write_npy(path, d);
+}
+
+std::vector<double> fetch_y_axis(npy::npy_data<double>& acc) {
+    // TODO: later on, we should use a vector projection towards gravity
+    std::vector<double> signal;
+    const size_t rows_real = acc.shape[0];
+#if DEBUG_IIR == 1
+    const size_t rows = 5;
+#else
+    const size_t rows = acc.shape[0];
+#endif
+    int stride = 3;
+    int offset = 1; // [x,y,z] per row - fetch y
+    signal.resize(rows);
+    if (acc.fortran_order) {
+        stride = 1;
+        offset = (int) rows_real;
+    }
+    /*
+    std::cout << "is_fortran=" << acc.fortran_order << std::endl;
+    for (size_t i = 0; i < 10; i++) {
+        std::cout << "acc.data[" << i << "]=" << acc.data[i] << std::endl;
+    }
+    */
+    for (int i = 0; i < rows; i++) {
+        signal[i] = acc.data[i * stride + offset];
+    }
+    return signal;
+}
+
+DebugSsfStepDetectorThreshold::DebugSsfStepDetectorThreshold(size_t len_refr) : SsfStepDetector(len_refr) {}
+double DebugSsfStepDetectorThreshold::filter(double val) {
+    this->SsfStepDetector::filter(val);
+    return peek_threshold();
+}

google-tests/test_helpers.h

@@ -0,0 +1,34 @@
+//
+// Created by david on 11.03.2026.
+//
+
+#ifndef PASADASUPERPROJECT_TEST_HELPERS_H
+#define PASADASUPERPROJECT_TEST_HELPERS_H
+
+#include "npy.hpp"
+#include "ssf_filter.h"
+#include <string>
+#include <vector>
+
+template <typename T> static std::vector<double> apply_filter(T& filter, std::vector<double>& x) {
+    std::vector<double> y;
+    y.resize(x.size());
+    for (int i = 0; i < x.size(); i++) {
+        y[i] = filter.filter(x[i]);
+    }
+    return y;
+}
+
+void npy_save(std::string path, std::vector<double>& x);
+void npy_save(std::string path, std::vector<bool>& x);
+
+std::vector<double> fetch_y_axis(npy::npy_data<double>& acc);
+
+/** Returns the ssf_threshold as the filter output for debugging. */
+class DebugSsfStepDetectorThreshold : public SsfStepDetector {
+public:
+    DebugSsfStepDetectorThreshold(size_t len_refr);
+    double filter(double val);
+};
+
+#endif //PASADASUPERPROJECT_TEST_HELPERS_H

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/pd_signal.cpp

@@ -90,7 +90,7 @@ void interp(std::vector<double>& y, std::vector<double>& x, std::vector<double>&
 void resample(std::vector<double> &out, std::vector<double> &x, int beat_len) {
     std::vector<double> t;
     std::vector<double> i;
-    linspace(t, 0, (double) x.size(), beat_len, false);
+    linspace(t, 0, (double) (x.size()-1), beat_len, false);
     linspace(i, 0, (int) (x.size()-1), (int) x.size(), false);
     interp(out, t, i, x);
 }

pasada-lib/ssf_filter.cpp

@@ -2,10 +2,11 @@
 // 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 <iomanip>
 #include <iostream>
 
 static std::vector<double> make_ones(size_t sw) {
@@ -86,3 +87,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(ssf);
+        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(ssf);
+            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 idx=" << idx << " -> replaceTemplate() corr=" << std::fixed << std::setw(7) << std::setprecision(4) << corr << " checkedSsf=" << checkedSsf << std::endl;
+            replaceTemplate(ssf);
+            //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(ssf);
+        } else {
+            // badBeatRanges.add(s, e)
+            // numNoisy++
+        }
+        // runningCorrs.add(posCorr)
+        if (justLocked) { dispatchLocked(); justLocked = false; }
+        dispatchBeat(idx, goodBeat, posCorr);
+    }
+    idx++;
+}