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

#include "ssf_filter.h"
#include "pd_signal.h"
#include <limits>
#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;
}


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++;
}