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

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

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()),
    ssf_threshold_nm1(std::numeric_limits<double>::infinity()),
    f_ssf_threshold_smoothing(6, 0, 0, make_ones(6)),
    len_refr(len_refr), n_refr(0), is_refr(false),
    ssf_nm1(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 ssf) {
    double ssf_mean = f_ssf_mean.filter(ssf) / ((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
    // 'is_prev_lower' fixes a glitch where a falling threshold leads to undetected crossings
    bool is_prev_lower = ssf_nm1 < ssf_threshold || ssf_nm1 < ssf_threshold_nm1;
    bool is_cur_higher = ssf >= ssf_threshold;
    bool is_txing = is_prev_lower && is_cur_higher;
    // 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;
    }
    if (num_samples == LEN_INIT) {
        // initial threshold setting
        ssf_threshold = 3.0 * ssf_mean * 0.99; // see Zong 2003 for the magic numbers
        //std::cerr << "before prime()" << std::endl;
        f_ssf_threshold_smoothing.prime(ssf_threshold);
    } else if (num_samples > LEN_TH_WIN) {
        //std::cerr << "adaptive threshold setting" << std::endl;
        // adaptive threshold setting
        // +2 is half the window size
        // TODO: param upon SsfFilter.upslope_width/2 instead of hardcoding -- also f_ssf_threshold_smoothing(), nb. should be even number
        if (num_samples == n_refr + 2) {
            //std::cerr << "setting adaptive threshold setting" << std::endl;
            ssf_threshold_nm1 = ssf_threshold;
            // the ssf peak comes 3 samples (half-window + 1 sample) after the crossing
            ssf_threshold = f_ssf_threshold_smoothing.filter(ssf) / ((double) f_ssf_threshold_smoothing.size()) * 0.6;
        }
    }
    ssf_nm1 = ssf;
    num_samples++;
    return rv;
}
double SsfStepDetector::peek_threshold() {
    return ssf_threshold;
}