//
// Created by david on 17.05.2026.
//

#include "pd_resamp.h"
#include "pd_signal.h"

Resampler::Resampler(): N(INITIAL_SAMPLES+1), n(0), m(0), initialized(false), read_valid(false), fs(0.0) {
    times.resize(N);
    data.resize(N);
    times.assign(N, 0.0);
    data.assign(N, 0.0);
}

void Resampler::push(double ts, double val) {
    // i: previous write position
    auto i = static_cast<size_t>((static_cast<int>(n) - 1 + static_cast<int>(N)) % static_cast<int>(N));
    auto im = static_cast<size_t>((static_cast<int>(m) - 1 + static_cast<int>(N)) % static_cast<int>(N));
    if (ts < times[i]) throw std::invalid_argument("we expect ts to be time-ascending");
    // j: current write position
    auto j = n;
    times[n] = ts;
    data[n] = val;
    n = (n+1) % N;
    // note: we do not currently handle overrun (assume caller keeps contract)
    if (n == INITIAL_SAMPLES && !initialized) {
        compute_fs();
        read_valid = true;
        return; // returns INITIAL_SAMPLES all at once
        // ??? need to compute 'dt' etc. for last sample! -> fall through
    }
    // once initialized, we skip ahead as much as possible - avoid buffering too much
    double dt = ts - times[im];
    double dtr = dt * 1e-9 * fs;
    if (dtr < 0) { throw std::invalid_argument("dt is negative"); }
    // case 1: 'ts' is less than next sample
    if (dtr < 0.99) {
        // drop samples (cannot be bothered with interpolation):
        // practice shows that Android initially provides a high sampling rate, which subsequently drops later on,
        // so we simply skip the implementation here.
        read_valid = false;
        return;
    }
    // case 2: 'ts' is exactly next sample
    if (0.99 < dtr && dtr < 1.01) {
        m = j; // skip directly to actual sample
        read_valid = true;
        return;
    }
    // case 3: 'ts' skips samples
    if (dtr >= 1.01) {
        auto ts_nm1 = times[i];
        // x = ts[n-1] + np.linspace(1.0, dtr, int(np.round(dtr)), endpoint=True) / fs * 1e9
        // xp = [ts[n-1], ts[n]]
        // fp = [a[n-1], a[n]]
        // y = np.interp(x, xp, fp)
        std::vector<double> y;
        std::vector<double> x;
        std::vector<double> xp { times[i], ts };
        std::vector<double> fp { data[i], val };
        pd_signal::linspace(x, 1.0, dtr, static_cast<int>(round(dtr)), false);
        for (auto& e : x) e = ts_nm1 + e / fs * 1e9;
        pd_signal::interp(y, x, xp, fp);
        // write to data[_ : n]
        int s = static_cast<int>(x.size());
        auto p0 = static_cast<size_t>((static_cast<int>(n) - s + static_cast<int>(N)) % static_cast<int>(N));
        for (int p = 0; p < s; p++) {
            data[(p0 + p) % N] = y[p];
        }
        m = p0; // provides round(dtr) samples output, interpolated
        read_valid = true;
        return;
    }
}

bool Resampler::peek() {
    if (!initialized) { return false; }
    if (!read_valid) { return false; }
    return n != m;
}

double Resampler::get() {
    if (!initialized) { throw std::runtime_error("not initialized"); }
    if (n == m) { throw std::runtime_error("empty buffer"); }
    double val = data[m];
    m = (m+1) % N;
    return val;
}

double Resampler::get_fs() const {
    return fs;
}

void Resampler::compute_fs() {
    // compute 'fs' according to first INITIAL_SAMPLES
    // we ignore 'n' as this is only ever called once per Resampler lifetime, and assume 'times' has been filled from 0 on
    std::vector<double> delta_times;
    pd_signal::diff(delta_times, times);
    delta_times.resize(INITIAL_SAMPLES-1); // trim off trailing buffer slot (which is not filled)
    double mean_dt = pd_signal::mean(delta_times);
    fs = 1e9 / mean_dt;
    initialized = true;
}