//
// Created by david on 03.03.2026.
//

#include "include/ssf_filter.h"
#include <limits>
#include <cmath>
#include <cassert>

static std::vector<double> make_ones(size_t sw) {
    std::vector<double> ones;
    ones.resize(sw);
    ones.assign(sw, 1.0);
    return ones;
}

SsfFilter::SsfFilter(size_t upslope_width) :
    sw(upslope_width),
    // Filt(N, shift, offset, taps)
    f_delta_u(2, 0, 0, std::vector<double> {1.0, -1.0}),
    f_window(upslope_width, 0, 0, make_ones(upslope_width))
{}
double SsfFilter::filter(double val) {
    double du = f_delta_u.filter(val);
    double duc = std::max(0.0, du);
    double ssf = f_window.filter(duc);
    return ssf;
}


SsfStepDetector::SsfStepDetector(size_t len_refr) :
    LEN_INIT((size_t) (3.0 * FPS)), // initial window length for ssf_threshold
    LEN_TH_WIN((size_t) (3.0 * FPS)), // subsequent window length for ssf_threshold
    num_samples(0),
    ssf_threshold(std::numeric_limits<double>::infinity()),
    len_refr(len_refr), n_refr(0), is_refr(false),
    nm1_ssf(0.0),
    f_ssf_mean(LEN_TH_WIN, 0, 0, make_ones(LEN_TH_WIN))
{
    assert (LEN_INIT >= LEN_TH_WIN && "LEN_INIT < LEN_TH_WIN, check normalization of initial ssf_threshold");
}
double SsfStepDetector::filter(double val) {
    double ssf_mean = f_ssf_mean.filter(val) / ((double) LEN_TH_WIN);
    double rv = 0.0;
    if (num_samples >= LEN_INIT) {
        // initial and subsequent threshold setting.
        ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers
    }
    // threshold crossing detection
    bool is_txing = nm1_ssf < ssf_threshold && val >= ssf_threshold;
    // refractory period reset
    if (num_samples - n_refr >= len_refr) is_refr = false;
    // transition and not in refractory period? detected a step.
    if (is_txing && !is_refr) {
        rv = 1.0;
        is_refr = true;
        n_refr = num_samples;
    }
    nm1_ssf = val;
    num_samples++;
    return rv;
}
double SsfStepDetector::peek_threshold() {
    return ssf_threshold;
}