MARTe-Integrated-Components/Source/Applications/StreamHub/SignalRingBuffer.h

/**
 * @file SignalRingBuffer.h
 * @brief Thread-safe fixed-capacity circular buffer of (float64 time, float64 value) pairs.
 *
 * Write() is called from the UDPSClient receive thread; Read/ReadRange are called
 * from the StreamHub push thread.  A FastPollingMutexSem protects all operations.
 */

#ifndef STREAMHUB_SIGNAL_RING_BUFFER_H_
#define STREAMHUB_SIGNAL_RING_BUFFER_H_

#include "CompilerTypes.h"
#include "FastPollingMutexSem.h"
#include <string.h>

namespace StreamHub {

using MARTe::uint32;
using MARTe::float64;
using MARTe::FastPollingMutexSem;

/**
 * @brief Circular buffer of (time, value) float64 pairs.
 *
 * When full, Write() silently overwrites the oldest entry.
 */
class SignalRingBuffer {
public:

    SignalRingBuffer();
    ~SignalRingBuffer();

    /**
     * @brief Allocate buffer for @p maxPts points.
     * Frees any previously allocated buffer.
     * @return true on success.
     */
    bool Allocate(uint32 maxPts);

    /**
     * @brief Write one (t, v) pair.  Called from the receive thread.
     */
    void Write(float64 t, float64 v);

    /**
     * @brief Copy the last (oldest-to-newest) min(n, count) points into tOut/vOut.
     * @return Number of points actually written.
     */
    uint32 Read(float64 *tOut, float64 *vOut, uint32 n) const;

    /**
     * @brief Copy all points with time in [t0, t1] into tOut/vOut (up to maxOut).
     * Uses binary search — requires monotonically non-decreasing time values.
     * @return Number of points actually written.
     */
    uint32 ReadRange(float64 t0, float64 t1,
                     float64 *tOut, float64 *vOut, uint32 maxOut) const;

    /**
     * @brief Copy points written after @p cursor (a previously returned
     * TotalWritten() value) into tOut/vOut, oldest first.  On overrun the
     * read is clamped to the oldest available point.  @p cursor is advanced
     * by the number of points consumed (capped by @p maxOut).
     * @return Number of points actually written.
     */
    uint32 ReadSince(MARTe::uint64 &cursor,
                     float64 *tOut, float64 *vOut, uint32 maxOut) const;

    /** @return Total number of points ever written (monotonic). */
    MARTe::uint64 TotalWritten() const;

    /** @return Current number of stored points (≤ capacity). */
    uint32 Count() const;

    /** @brief Discard all stored points. */
    void Clear();

private:
    float64            *tBuf;
    float64            *vBuf;
    uint32              capacity;
    uint32              head;   /* next write position */
    uint32              count;  /* number of valid entries */
    MARTe::uint64       totalWritten; /* monotonic write counter */
    mutable FastPollingMutexSem mutex;
};

/*---------------------------------------------------------------------------*/
/* Inline implementation                                                      */
/*---------------------------------------------------------------------------*/

inline SignalRingBuffer::SignalRingBuffer()
    : tBuf(static_cast<float64 *>(0)),
      vBuf(static_cast<float64 *>(0)),
      capacity(0u),
      head(0u),
      count(0u),
      totalWritten(0u) {
}

inline SignalRingBuffer::~SignalRingBuffer() {
    delete[] tBuf;
    delete[] vBuf;
}

inline bool SignalRingBuffer::Allocate(uint32 maxPts) {
    if (maxPts == 0u) { return false; }

    /* Allocate outside the lock, swap under it (readers may be active). */
    float64 *newT = new float64[maxPts];
    float64 *newV = new float64[maxPts];
    if ((newT == static_cast<float64 *>(0)) ||
        (newV == static_cast<float64 *>(0))) {
        delete[] newT;
        delete[] newV;
        return false;
    }

    (void) mutex.FastLock();
    float64 *oldT = tBuf;
    float64 *oldV = vBuf;
    tBuf = newT;
    vBuf = newV;
    capacity = maxPts;
    head = 0u;
    count = 0u;
    totalWritten = 0u;
    mutex.FastUnLock();

    delete[] oldT;
    delete[] oldV;
    return true;
}

inline void SignalRingBuffer::Write(float64 t, float64 v) {
    (void) mutex.FastLock();
    if (capacity > 0u) {
        tBuf[head] = t;
        vBuf[head] = v;
        head = (head + 1u) % capacity;
        if (count < capacity) { count++; }
        totalWritten++;
    }
    mutex.FastUnLock();
}

inline uint32 SignalRingBuffer::Read(float64 *tOut, float64 *vOut, uint32 n) const {
    if ((capacity == 0u) || (count == 0u)) { return 0u; }
    (void) mutex.FastLock();

    uint32 avail = count;
    if (n > avail) { n = avail; }

    /* Oldest entry index: (head - count + capacity) % capacity */
    uint32 start = (head + capacity - avail) % capacity;
    /* We want the last n entries: skip (avail - n) entries */
    uint32 skip = avail - n;
    start = (start + skip) % capacity;

    for (uint32 i = 0u; i < n; i++) {
        uint32 idx = (start + i) % capacity;
        tOut[i] = tBuf[idx];
        vOut[i] = vBuf[idx];
    }

    mutex.FastUnLock();
    return n;
}

inline uint32 SignalRingBuffer::ReadRange(float64 t0, float64 t1,
                                          float64 *tOut, float64 *vOut,
                                          uint32 maxOut) const {
    (void) mutex.FastLock();
    const uint32 avail = count;
    if ((capacity == 0u) || (avail == 0u) || (t1 < t0)) {
        mutex.FastUnLock();
        return 0u;
    }

    const uint32 oldest = (head + capacity - avail) % capacity;

    /* Binary search over the logically-ordered ring (time is monotonically
     * non-decreasing per ring).  Port of Go ringbuf.go readRange. */

    /* lo = first logical index with t >= t0 */
    uint32 lo = 0u;
    {
        uint32 a = 0u;
        uint32 b = avail;
        while (a < b) {
            const uint32 mid = a + ((b - a) >> 1);
            const float64 tm = tBuf[(oldest + mid) % capacity];
            if (tm < t0) { a = mid + 1u; } else { b = mid; }
        }
        lo = a;
    }
    /* hi = first logical index with t > t1 */
    uint32 hi = lo;
    {
        uint32 a = lo;
        uint32 b = avail;
        while (a < b) {
            const uint32 mid = a + ((b - a) >> 1);
            const float64 tm = tBuf[(oldest + mid) % capacity];
            if (tm <= t1) { a = mid + 1u; } else { b = mid; }
        }
        hi = a;
    }

    uint32 nOut = hi - lo;
    if (nOut > maxOut) { nOut = maxOut; }
    for (uint32 i = 0u; i < nOut; i++) {
        const uint32 idx = (oldest + lo + i) % capacity;
        tOut[i] = tBuf[idx];
        vOut[i] = vBuf[idx];
    }

    mutex.FastUnLock();
    return nOut;
}

inline uint32 SignalRingBuffer::ReadSince(MARTe::uint64 &cursor,
                                          float64 *tOut, float64 *vOut,
                                          uint32 maxOut) const {
    (void) mutex.FastLock();

    if (cursor > totalWritten) {
        /* Ring was reset (re-Allocate) — restart from current position. */
        cursor = totalWritten;
    }
    MARTe::uint64 pending = totalWritten - cursor;
    if ((capacity == 0u) || (pending == 0u) || (maxOut == 0u)) {
        mutex.FastUnLock();
        return 0u;
    }
    /* Overrun: oldest retained point is totalWritten - count. */
    if (pending > static_cast<MARTe::uint64>(count)) {
        pending = static_cast<MARTe::uint64>(count);
        cursor  = totalWritten - pending;
    }

    uint32 n = (pending > static_cast<MARTe::uint64>(maxOut))
               ? maxOut : static_cast<uint32>(pending);

    /* Logical index of the first pending point, relative to the oldest. */
    const uint32 avail  = count;
    const uint32 oldest = (head + capacity - avail) % capacity;
    const uint32 firstLogical =
        avail - static_cast<uint32>(pending);

    for (uint32 i = 0u; i < n; i++) {
        const uint32 idx = (oldest + firstLogical + i) % capacity;
        tOut[i] = tBuf[idx];
        vOut[i] = vBuf[idx];
    }
    cursor += static_cast<MARTe::uint64>(n);

    mutex.FastUnLock();
    return n;
}

inline MARTe::uint64 SignalRingBuffer::TotalWritten() const {
    (void) mutex.FastLock();
    const MARTe::uint64 tw = totalWritten;
    mutex.FastUnLock();
    return tw;
}

inline uint32 SignalRingBuffer::Count() const {
    (void) mutex.FastLock();
    uint32 c = count;
    mutex.FastUnLock();
    return c;
}

inline void SignalRingBuffer::Clear() {
    (void) mutex.FastLock();
    head = 0u;
    count = 0u;
    mutex.FastUnLock();
}

} /* namespace StreamHub */

#endif /* STREAMHUB_SIGNAL_RING_BUFFER_H_ */