MARTe-Integrated-Components/Client/streamhub/SignalBuffer.h

/**
 * @file SignalBuffer.h
 * @brief Per-signal ring buffer with LTTB decimation.
 *
 * Header-only. Uses STL (this is not a MARTe2 component).
 */

#pragma once

#include <vector>
#include <cstddef>
#include <cmath>
#include <algorithm>

namespace StreamHubClient {

/**
 * @brief Thread-safe circular buffer of (time, value) float64 pairs.
 */
struct SignalBuffer {
    explicit SignalBuffer(size_t cap = 20000)
        : capacity(cap), head(0), count(0) {
        t.resize(cap);
        v.resize(cap);
    }

    void setCapacity(size_t cap) {
        t.assign(cap, 0.0);
        v.assign(cap, 0.0);
        capacity = cap;
        head     = 0;
        count    = 0;
    }

    /** Append a (time, value) pair; overwrites oldest when full. */
    void push(double time, double val) {
        t[head] = time;
        v[head] = val;
        head    = (head + 1) % capacity;
        if (count < capacity) { count++; }
    }

    /**
     * @brief Copy the last `n` points into tOut/vOut (oldest → newest order).
     * @return Number of points written.
     */
    size_t readLast(size_t n, std::vector<double>& tOut, std::vector<double>& vOut) const {
        size_t actual = std::min(n, count);
        if (actual == 0) { return 0; }
        tOut.resize(actual);
        vOut.resize(actual);

        size_t startIdx = (head + capacity - actual) % capacity;
        for (size_t i = 0; i < actual; i++) {
            size_t idx  = (startIdx + i) % capacity;
            tOut[i] = t[idx];
            vOut[i] = v[idx];
        }
        return actual;
    }

    /**
     * @brief Read all points in [t0, t1] into tOut/vOut.
     *
     * Assumes points were pushed in non-decreasing time order (true for the
     * StreamHub live stream) and binary-searches the logical [0,count) index
     * space so the cost is O(log count + window) rather than O(count) — this
     * keeps per-frame rendering cheap even with a million-point ring.
     * @return Number of points written.
     */
    size_t readRange(double t0, double t1,
                     std::vector<double>& tOut, std::vector<double>& vOut) const {
        tOut.clear();
        vOut.clear();
        if (count == 0) { return 0; }

        const size_t startIdx = (head + capacity - count) % capacity;
        /* time at logical index i (0 = oldest, count-1 = newest) */
        // first index with t >= t0
        size_t lo = 0, hi = count;
        while (lo < hi) {
            size_t mid = lo + (hi - lo) / 2;
            if (t[(startIdx + mid) % capacity] < t0) { lo = mid + 1; }
            else { hi = mid; }
        }
        const size_t begin = lo;
        // first index with t > t1
        lo = begin; hi = count;
        while (lo < hi) {
            size_t mid = lo + (hi - lo) / 2;
            if (t[(startIdx + mid) % capacity] <= t1) { lo = mid + 1; }
            else { hi = mid; }
        }
        const size_t end = lo;
        if (end <= begin) { return 0; }

        tOut.reserve(end - begin);
        vOut.reserve(end - begin);
        for (size_t i = begin; i < end; i++) {
            size_t idx = (startIdx + i) % capacity;
            tOut.push_back(t[idx]);
            vOut.push_back(v[idx]);
        }
        return tOut.size();
    }

    size_t size() const { return count; }

    void clear() { head = 0; count = 0; }

    size_t capacity;
    std::vector<double> t;
    std::vector<double> v;
    size_t head;
    size_t count;
};

/*---------------------------------------------------------------------------*/
/* LTTB — Largest Triangle Three Buckets                                     */
/*---------------------------------------------------------------------------*/

/**
 * @brief Decimate tIn/vIn to at most `threshold` points using LTTB.
 *
 * Always preserves first and last points.
 * @return Number of output points.
 */
inline size_t LTTBDecimate(
    const std::vector<double>& tIn, const std::vector<double>& vIn,
    std::vector<double>& tOut, std::vector<double>& vOut,
    size_t threshold)
{
    const size_t nIn = tIn.size();
    if (nIn <= threshold || threshold < 2) {
        tOut = tIn;
        vOut = vIn;
        return nIn;
    }

    tOut.resize(threshold);
    vOut.resize(threshold);

    tOut[0] = tIn[0];
    vOut[0] = vIn[0];
    tOut[threshold - 1] = tIn[nIn - 1];
    vOut[threshold - 1] = vIn[nIn - 1];

    /* Bucket size (middle threshold-2 buckets cover points 1..nIn-2) */
    double bucketSize = static_cast<double>(nIn - 2) / static_cast<double>(threshold - 2);

    size_t a = 0; /* index of last selected point */
    for (size_t i = 0; i < threshold - 2; i++) {
        /* Calculate average of next bucket */
        size_t avgRangeStart = static_cast<size_t>((i + 1) * bucketSize) + 1;
        size_t avgRangeEnd   = static_cast<size_t>((i + 2) * bucketSize) + 1;
        if (avgRangeEnd >= nIn) { avgRangeEnd = nIn - 1; }

        double avgT = 0.0, avgV = 0.0;
        size_t avgLen = avgRangeEnd - avgRangeStart;
        for (size_t j = avgRangeStart; j < avgRangeEnd; j++) {
            avgT += tIn[j];
            avgV += vIn[j];
        }
        if (avgLen > 0) {
            avgT /= static_cast<double>(avgLen);
            avgV /= static_cast<double>(avgLen);
        }

        /* Select point in current bucket with max triangle area */
        size_t rangeStart = static_cast<size_t>(i * bucketSize) + 1;
        size_t rangeEnd   = static_cast<size_t>((i + 1) * bucketSize) + 1;
        if (rangeEnd >= nIn) { rangeEnd = nIn - 1; }

        double maxArea  = -1.0;
        size_t maxIdx   = rangeStart;
        for (size_t j = rangeStart; j < rangeEnd; j++) {
            double area = std::fabs(
                (tIn[a] - avgT) * (vIn[j] - vIn[a]) -
                (tIn[a] - tIn[j]) * (avgV - vIn[a])
            ) * 0.5;
            if (area > maxArea) {
                maxArea = area;
                maxIdx  = j;
            }
        }
        tOut[i + 1] = tIn[maxIdx];
        vOut[i + 1] = vIn[maxIdx];
        a = maxIdx;
    }
    return threshold;
}

} /* namespace StreamHubClient */