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

/**
 * @file UDPSourceSession.h
 * @brief One UDPStreamer source connection: wraps UDPSClient, owns ring buffers + stats.
 *
 * Implements UDPSClientListener so it receives CONFIG and DATA callbacks from
 * the UDPSClient thread.  Thread-safe accessors are provided for the StreamHub
 * push thread to read signal data and statistics.
 */

#ifndef STREAMHUB_UDP_SOURCE_SESSION_H_
#define STREAMHUB_UDP_SOURCE_SESSION_H_

#include "UDPSClient.h"
#include "TCPSClient.h"
#include "UDPSProtocol.h"
#include "FastPollingMutexSem.h"
#include "HighResolutionTimer.h"
#include "StreamString.h"
#include "ConfigurationDatabase.h"
#include "SignalRingBuffer.h"
#include "UDPSourceStats.h"
#include "TriggerEngine.h"
#include "BinaryRecorder.h"
#include <string.h>

namespace StreamHub {

using MARTe::uint8;
using MARTe::uint16;
using MARTe::uint32;
using MARTe::uint64;
using MARTe::float64;
using MARTe::StreamString;
using MARTe::UDPSClient;
using MARTe::UDPSClientListener;
using MARTe::UDPSSignalDescriptor;
using MARTe::FastPollingMutexSem;
using MARTe::ConfigurationDatabase;

/** Maximum number of signals per source session. */
static const uint32 UDPSS_MAX_SIGNALS = 256u;

/* Accumulated-scalar dt estimator tuning. */
/** Weight of a new observation; slow enough that one bad gap barely moves it. */
static const float64 UDPSS_DT_EMA_ALPHA = 0.05;
/** Observations outside [lo, hi] x the current estimate are treated as a
 *  mis-counted gap and discarded rather than smoothed in. */
static const float64 UDPSS_DT_ACCEPT_LO = 0.5;
static const float64 UDPSS_DT_ACCEPT_HI = 2.0;

/**
 * @brief Per-sample period of an accumulated scalar packet, robust to loss.
 *
 * An Accumulate producer batches consecutive RT cycles, so the sender-clock
 * gap between two packets' first samples covers exactly as many cycles as the
 * earlier packet carried — but only while nothing is lost in between. Over UDP
 * (and with a producer that can overwrite a batch the sender never took) that
 * assumption fails, and dividing the gap by the previous packet's sample count
 * then inflates the period. The packet's own samples are laid out as
 * base + e*dt, so an inflated dt walks them past their real end and into the
 * span the next packet will claim: samples collide there and leave a hole
 * behind them.
 *
 * The number of packets that went missing is not guessed from the gap — that
 * is circular, and an estimator that infers the cycle count from its own
 * period has a stable fixed point wherever gap/dt is an integer, so a genuine
 * rate change locks it at the old period forever. It comes instead from the
 * UDPS packet counter, which the producer increments once per sent packet. The
 * gap then spans (1 + lost) batches, each assumed to be prevN cycles, and with
 * nothing lost the formula reduces exactly to gap/prevN.
 *
 * @param gap      Sender-clock seconds since the previous packet's first sample.
 *                 Must be > 0.
 * @param prevN    Samples in the previous packet. Must be > 0.
 * @param lost     Packets missing between the previous packet and this one,
 *                 from the producer's counter.
 * @param[in,out] dtEMA    Smoothed period. Seeded on the first call.
 * @param[in,out] dtValid  False until dtEMA holds an estimate.
 * @return The period to space this packet's samples by.
 */
inline float64 UDPSEstimateAccumDt(const float64 gap, const uint32 prevN,
                                   const uint32 lost, float64 &dtEMA,
                                   bool &dtValid) {
    float64 cycles = static_cast<float64>(prevN) *
                     (1.0 + static_cast<float64>(lost));
    if (cycles < 1.0) {
        cycles = 1.0;
    }
    const float64 dtObs = gap / cycles;

    if (!dtValid) {
        dtEMA   = dtObs;
        dtValid = true;
    } else if ((dtObs > (dtEMA * UDPSS_DT_ACCEPT_LO)) &&
               (dtObs < (dtEMA * UDPSS_DT_ACCEPT_HI))) {
        /* Track slow drift, but ignore observations far outside the current
         * estimate: those are the signature of a mis-counted gap, and folding
         * one in would drag the estimate towards the very error it exists to
         * absorb. */
        dtEMA = ((1.0 - UDPSS_DT_EMA_ALPHA) * dtEMA) +
                (UDPSS_DT_EMA_ALPHA * dtObs);
    }
    return dtEMA;
}

/**
 * @brief Pick the tick rate to divide a producer's DATA timestamps by.
 *
 * DATA packets carry the raw value of the producer's high-resolution counter,
 * which is meaningless without the rate it runs at. The rate is published in
 * the CONFIG trailer, after the descriptors and the publish-mode byte. When it
 * is missing — an older producer, or one that could not determine it — the
 * only remaining option is this host's own timer, which is right only while
 * the two machines agree; on x86 that is the TSC frequency, so it is a
 * different number on every model.
 *
 * @param payload    Reassembled CONFIG payload.
 * @param size       Bytes in @p payload.
 * @param numSigs    Signal count already read from the payload, capped to what
 *                   the receiver will store.
 * @param localFreq  This host's HRT frequency, used as the fallback.
 * @return Ticks per second to convert DATA timestamps with; never 0.
 */
inline float64 UDPSConfigHrtFrequency(const uint8 *payload, const uint32 size,
                                      const uint32 numSigs,
                                      const float64 localFreq) {
    const uint32 offset = 4u + (numSigs * MARTe::UDPS_SIGNAL_DESC_SIZE) + 1u;
    if ((payload != NULL_PTR(const uint8 *)) && (size >= (offset + 8u))) {
        uint64 wireFreq = 0u;
        memcpy(&wireFreq, payload + offset, 8u);
        /* Anything below 1 kHz is not a high-resolution timer; the field is
         * either absent, unset, or the payload was mis-parsed, and adopting it
         * would stretch every timestamp far enough to make the trace useless. */
        if (wireFreq >= 1000u) {
            return static_cast<float64>(wireFreq);
        }
    }
    return localFreq;
}

/**
 * @brief One connected UDPStreamer source.
 *
 * Lifecycle: Initialise() → Start() → [running] → Stop()
 * Can be re-Initialised for a different address without destroying the object.
 */
class UDPSourceSession : public UDPSClientListener {
public:

    UDPSourceSession();
    virtual ~UDPSourceSession();

    /**
     * @brief Configure and allocate this session.
     * @param id        Short identifier string (used as source ID in JSON/binary frames).
     * @param label     Human-readable label.
     * @param addr      Server IPv4 address string.
     * @param port      UDPStreamer port.
     * @param maxPts    Ring buffer capacity per signal.
     * @param mcGroup   Multicast group (empty string = unicast).
     * @param dataPort  Multicast data port (0 = port+1).
     * @param transport "udp" (default) or "tcp". TCP ignores mcGroup/dataPort.
     * @param streamName TCP only: stream to subscribe to on a TCPStreamService
     *                   ("" = plain TCPStreamer, no subscription).
     * @param external  Optional receiver owned by the caller (one stream of a
     *                  multiplexed TCPSClient, see TCPSClient::GetStream()). When set the
     *                  session does not create or configure its own client; call
     *                  DetachClient() before the receiver is destroyed.
     * @return true on success.
     */
    bool Initialise(const char *id, const char *label,
                    const char *addr, uint16 port, uint32 maxPts,
                    const char *mcGroup = "", uint16 dataPort = 0u,
                    const char *transport = "udp", const char *streamName = "",
                    MARTe::UDPSClientI *external = static_cast<MARTe::UDPSClientI *>(0));

    /** @brief Stop and forget an external receiver (no-op for built-in receivers). */
    void DetachClient();

    /** @brief Start the UDPSClient background thread. */
    bool Start();

    /** @brief Stop the UDPSClient background thread. */
    bool Stop();

    /* ---- UDPSClientListener callbacks (UDPSClient thread) -------------- */
    virtual void OnUDPSConfig(const uint8 *payload, uint32 payloadSize);
    virtual void OnUDPSData(const uint8 *payload, uint32 payloadSize);
    virtual void OnUDPSFragment(uint32 counter, uint32 nBytes, bool complete);
    virtual void OnUDPSConnected();
    virtual void OnUDPSDisconnected();

    /* ---- Thread-safe accessors (push thread) --------------------------- */

    /** @return Signal count (0 until first CONFIG received). */
    uint32 GetNumSignals() const;

    /**
     * @brief Get a copy of one signal's descriptor.
     * @return false if idx is out of range or no CONFIG received yet.
     */
    bool GetSignalDescriptor(uint32 idx, UDPSSignalDescriptor &desc) const;

    /**
     * @brief Get publish mode (UDPS_PUBLISH_STRICT/ACCUMULATE/DECIMATE).
     */
    uint8 GetPublishMode() const;

    /**
     * @brief Copy the last (up to) n points from signal idx into tOut/vOut.
     * @return Number of points written.
     */
    uint32 ReadSignalLast(uint32 idx, uint32 n,
                          float64 *tOut, float64 *vOut) const;

    /**
     * @brief Copy points written after @p cursor (monotonic write counter) into tOut/vOut.
     * Updates @p cursor. Clamps to the oldest available point on overrun.
     * @return Number of points written (capped by maxOut).
     */
    uint32 ReadSignalSince(uint32 idx, MARTe::uint64 &cursor,
                           float64 *tOut, float64 *vOut, uint32 maxOut) const;

    /**
     * @brief Copy all ring-buffer points for signal idx in the time window [t0,t1].
     * @return Number of points written (capped by maxOut).
     */
    uint32 ReadSignalRange(uint32 idx, float64 t0, float64 t1,
                           float64 *tOut, float64 *vOut, uint32 maxOut) const;

    /** @brief Thread-safe snapshot of current statistics. */
    UDPSourceStats GetStats() const;

    /** @return Session identifier string. */
    StreamString GetId() const;

    /** @return Human-readable label. */
    StreamString GetLabel() const;

    /** @return Server address (for "sources" broadcast). */
    StreamString GetAddr() const;

    /** @return Server port. */
    uint16 GetPort() const;

    /** @return Multicast group ("" = unicast). */
    StreamString GetMulticastGroup() const;

    /** @return Multicast data port (0 = port+1 default). */
    uint16 GetDataPort() const;

    /** @return "udp" or "tcp". */
    StreamString GetTransport() const;

    /** @return TCP stream name ("" = none). */
    StreamString GetStreamName() const;

    /** @return true after the first CONFIG packet has been fully parsed. */
    bool IsConfigured() const;

    /** @return true if the session has been Initialised (but may not be Started). */
    bool IsInitialised() const;

    /** @brief Update the live-read point cap (does not resize ring buffers). */
    bool SetMaxPoints(uint32 maxPts);

    /**
     * @brief Set per-signal ring capacities (applied at the next CONFIG).
     * @param temporal Capacity for multi-element (waveform) signals.
     * @param scalar   Capacity for scalar signals.
     */
    void SetRingCapacities(uint32 temporal, uint32 scalar);

    /**
     * @brief Grow every ring so it can retain at least @p seconds of history.
     *
     * The required capacity is seconds × the rate measured from the ring
     * itself (count / time span), because most sources advertise
     * samplingRate = 0. Signals whose ring has not filled enough to measure a
     * rate are left alone; the caller is expected to retry.
     *
     * @param seconds Retention target.
     * @param maxPts  Per-signal ceiling, so a multi-Msps source cannot be
     *                asked to allocate an unbounded amount of memory.
     * @return true if at least one ring was enlarged.
     */
    bool GrowRingsForSeconds(float64 seconds, uint32 maxPts);

    /** @return Largest ring capacity currently allocated in this session. */
    uint32 GetMaxRingCapacity() const;

    /**
     * @brief Newest timestamp this source has produced on its *own* clock, or
     * 0 when it publishes no producer-timed signal (or has no data yet).
     *
     * Only signals that reference a time signal count: PACKET-timed signals
     * are stamped on arrival and so live in the hub's wall-clock domain, not
     * the producer's, even when they come from the very same source.  A source
     * free-running on its own clock sits seconds away from wall time and drifts,
     * so anything waiting for a capture window to fill must compare against
     * this, never clock_gettime().
     */
    float64 ProducerNewestTime() const;

    /**
     * @brief Attach the (shared) hub trigger engine.
     * Every decoded sample of the trigger's configured signal — resolved
     * against this session's id and signal names per config epoch — is fed
     * to TriggerEngine::CheckSample from the receive thread.
     */
    void SetTriggerEngine(TriggerEngine *engine);

    /* ---- Binary recorder control -------------------------------------- */

    /** @brief Set the recorder configuration; initialises the recorder when
     *  enabled. Must be called before Start(). */
    void SetRecorderConfig(const RecorderConfig &cfg);

    /** @brief Override the recorded signal subset for this run (any thread).
     *  Adopted on the receive thread via an epoch check. */
    void SetRecorderSignals(const char *spec);

    /** @brief Arm the recorder (any thread). */
    void RequestRecArm();

    /** @brief Disarm the recorder (any thread). */
    void RequestRecDisarm();

    /** @brief Drive recorder disk I/O (push thread). */
    void RecorderFlushTick(uint32 nowSec);

    /** @brief Recorder status snapshot (push thread). */
    void GetRecorderInfo(bool &recording, char *file, uint32 fileSz,
                         uint64 &bytesWritten, uint64 &rowsWritten,
                         uint64 &droppedRows, uint64 &freeMB) const;

    /** @return true if the recorder is enabled for this session. */
    bool IsRecorderEnabled() const { return recCfg_.enabled; }

private:

    /** @brief Build the per-signal include mask from a subset spec
     *  ("all" or comma-separated "src:sig" keys). Receive thread only. */
    void ComputeIncludeMask(const UDPSSignalDescriptor *descs, uint32 n,
                            const char *spec, bool *maskOut);

    /* DATA payload parsing */
    void ParseConfigPayload(const uint8 *payload, uint32 size);
    /**
     * @param lostPackets DATA packets missing immediately before this one, from
     *        the producer's counter; the accumulated-scalar period estimate
     *        needs it to know how many cycles the sender-clock gap spans.
     */
    void ParseDataPayload(const uint8 *payload, uint32 size,
                          uint32 lostPackets);
    void AllocateRingBuffers();

    /** @brief Invalidate all wall-clock calibration state (receive thread only). */
    void ResetCalibration();

    /**
     * @brief Re-resolve the trigger signal key against this session
     * (receive thread only; called when the trigger config epoch changes).
     */
    void ResolveTriggerSignal(const UDPSSignalDescriptor *descs, uint32 nSigs);

    /** @brief Ring write + trigger edge check for element @p e of signal @p s. */
    inline void WriteSample(uint32 s, uint32 e, float64 t, float64 v) {
        rings_[s].Write(t, v);
        if ((trigSigIdx_ >= 0) && (static_cast<uint32>(trigSigIdx_) == s)) {
            if ((trigElemIdx_ < 0) || (static_cast<uint32>(trigElemIdx_) == e)) {
                trigEngine_->CheckSample(t, v);
            }
        }
    }

    /** @brief Batch ring write for signal @p s, with optional trigger check.
     *  Uses WriteBatch internally (single lock acquisition for all samples). */
    inline void WriteSampleBatch(uint32 s, const float64 *t, const float64 *v,
                                 uint32 n) {
        rings_[s].WriteBatch(t, v, n);
        /* Trigger check must still run per-sample for the watched signal. */
        if ((trigSigIdx_ >= 0) && (static_cast<uint32>(trigSigIdx_) == s) &&
            (trigEngine_ != static_cast<TriggerEngine *>(0))) {
            for (uint32 e = 0u; e < n; e++) {
                if ((trigElemIdx_ < 0) || (static_cast<uint32>(trigElemIdx_) == e)) {
                    trigEngine_->CheckSample(t[e], v[e]);
                }
            }
        }
    }

    /**
     * @brief Resolve (and maintain) the wall-clock calibration offset for time
     * signal @p tIdx given the first decoded timer value @p timer0S of the
     * current packet and the arrival wall time @p wallNowS.
     *
     * Snaps the offset to wallNowS − timer0S only when there is a genuine
     * discontinuity in the source: the first packet, or a backward jump of the
     * source clock (a looping/rewinding producer).
     *
     * A source that free-runs on its own clock also *drifts* against wall time,
     * without any discontinuity.  Snapping that away would shift the whole
     * published timeline in one step and so tear a hole of exactly the drift
     * into a stream that is in fact continuous, which is worse than the drift
     * itself.  Past kRecalibThresholdS the offset is therefore slewed instead:
     * nudged towards wall time by at most kMaxSlewFraction of the packet's own
     * duration, so the seam can never exceed a fraction of one packet.
     *
     * @return the calibration offset to add to timer-seconds for this signal.
     */
    inline float64 CalibrateTimeSignal(uint32 tIdx, float64 timer0S,
                                       float64 wallNowS) {
        static const float64 kRecalibThresholdS = 2.0;
        static const float64 kMaxSlewFraction   = 0.1;
        const bool reset = timeSigLastValid_[tIdx] &&
                           (timer0S < timeSigLastTimerS_[tIdx]);
        if ((!timeSigCalibValid_[tIdx]) || reset) {
            timeSigCalib_[tIdx]      = wallNowS - timer0S;
            timeSigCalibValid_[tIdx] = true;
        }
        else {
            const float64 drift = (timeSigCalib_[tIdx] + timer0S) - wallNowS;
            const float64 absDrift = (drift < 0.0) ? -drift : drift;
            if (absDrift > kRecalibThresholdS) {
                const float64 pktSpan = timer0S - timeSigLastTimerS_[tIdx];
                const float64 maxStep = pktSpan * kMaxSlewFraction;
                float64 step = -drift;
                if (step >  maxStep) { step =  maxStep; }
                if (step < -maxStep) { step = -maxStep; }
                timeSigCalib_[tIdx] += step;
            }
        }
        timeSigLastTimerS_[tIdx] = timer0S;
        timeSigLastValid_[tIdx]  = true;
        return timeSigCalib_[tIdx];
    }

    /**
     * @brief Decode @p nElems consecutive elements of signal @p desc starting
     * at payload offset @p off into @p out (dequantised physical values).
     */
    void DecodeElems(const uint8 *payload, uint32 off,
                     const UDPSSignalDescriptor &desc,
                     uint32 nElems, float64 *out) const;

    /** @brief Decode element @p e of signal @p desc at payload offset @p off. */
    float64 DecodeOneElem(const uint8 *payload, uint32 off,
                          const UDPSSignalDescriptor &desc, uint32 e) const;

    /* Value decoding helpers */
    float64 DecodeRawValue(const uint8 *ptr, uint8 typeCode) const;
    float64 DequantizeValue(float64 rawOrQuant, uint8 quantType,
                            float64 rangeMin, float64 rangeMax,
                            bool isRaw) const;
    uint32  QuantWireBytes(uint8 quantType) const;

    /* Configuration (set by Initialise, immutable afterwards) */
    StreamString id_;
    StreamString label_;
    StreamString addr_;
    uint16       port_;
    StreamString mcGroup_;   ///< Multicast group ("" = unicast)
    uint16       dataPort_;  ///< Multicast data port (0 = port+1)
    StreamString transport_; ///< "udp" or "tcp"
    StreamString streamName_;///< TCPStreamService stream name ("" = none)
    uint32       maxPoints_;
    uint32       ringTemporal_;  ///< Ring capacity for multi-element signals
    uint32       ringScalar_;    ///< Ring capacity for scalar signals
    bool         initialised_;

    /* Receivers (each owns a background thread); client_ points at the active one. */
    UDPSClient   udpClient_;
    MARTe::TCPSClient tcpClient_;
    MARTe::UDPSClientI *client_;
    bool                external_;   ///< client_ is owned by the caller

    /* Signal metadata — protected by metaMutex_ */
    mutable FastPollingMutexSem metaMutex_;
    UDPSSignalDescriptor        sigDescs_[UDPSS_MAX_SIGNALS];
    SignalRingBuffer             rings_[UDPSS_MAX_SIGNALS];
    uint32                      numSignals_;
    uint8                       publishMode_;
    bool                        configured_;

    /* Statistics — protected by statsMutex_ (port of Go SourceStat, stats.go) */
    static const uint32 kStatRingSize = 512u;
    mutable FastPollingMutexSem statsMutex_;
    StreamString stateStr_;                 ///< Connection state string
    bool         statSeenFirst_;            ///< First DATA counter seen
    uint32       statLastCounter_;          ///< Last DATA packet counter
    uint64       statTotalRx_;              ///< Complete DATA packets
    uint64       statTotalLost_;            ///< Lost packets (counter gaps)
    float64      ctRing_[kStatRingSize];    ///< Cycle times (s)
    uint32       fragRing_[kStatRingSize];  ///< Datagrams per cycle
    uint32       byteRing_[kStatRingSize];  ///< Raw bytes per cycle
    uint32       ctHead_;                   ///< Next write index
    bool         ctFull_;                   ///< Ring has wrapped
    uint64       statLastRxTicks_;          ///< HRT ticks at last complete DATA
    uint32       statFragCount_;            ///< Per-cycle datagram accumulator
    uint32       statByteCount_;            ///< Per-cycle byte accumulator

    /* Wall-clock calibration of time signals (receive thread only).
     * timeSigCalib_[i] holds the offset (wall − timer·scale) for time signal i,
     * mirroring the Go hub's timeSigCalib map (hub.go). */
    float64  timeSigCalib_[UDPSS_MAX_SIGNALS];
    bool     timeSigCalibValid_[UDPSS_MAX_SIGNALS];

    /* Previous packet's first time-signal value (seconds) per time signal —
     * used to detect a backward reset of a looping/rewinding source clock
     * (e.g. a FileReader with EOF = "Rewind").  When the new packet's clock
     * jumps backward the calibration is re-anchored to the current wall time
     * so the published timeline stays monotonic instead of overwriting the
     * previous pass's window (which renders as periodic gaps). */
    float64  timeSigLastTimerS_[UDPSS_MAX_SIGNALS];
    bool     timeSigLastValid_[UDPSS_MAX_SIGNALS];

    /* Last packet arrival wall time per signal — used to interpolate per-element
     * timestamps for packed TIMEMODE_PACKET arrays (Go hub lastPktNs).
     * For accumulated scalars this instead holds the previous packet's
     * sender-clock seconds (HRT-derived), used to derive the per-sample dt. */
    float64  lastPktWallS_[UDPSS_MAX_SIGNALS];
    bool     lastPktWallValid_[UDPSS_MAX_SIGNALS];

    /* Accumulated-scalar timing: the producer's HRT counter frequency and the
     * previous packet's sample count, used to reconstruct per-sample timestamps
     * from the embedded sender HRT instead of the (UDP burst-sensitive) packet
     * arrival time.  Seeded from this host's timer and replaced by the rate the
     * producer publishes in CONFIG; see UDPSConfigHrtFrequency. */
    float64  hrtFreq_;
    uint32   accScalarPrevN_[UDPSS_MAX_SIGNALS];

    /* Per-signal state of UDPSEstimateAccumDt (see above): the smoothed
     * per-sample period for accumulated scalars whose descriptor carries no
     * SamplingRate. */
    float64  accScalarDtEMA_[UDPSS_MAX_SIGNALS];
    bool     accScalarDtValid_[UDPSS_MAX_SIGNALS];

    /* Scratch buffers for decoding arrays (receive thread only). */
    float64 *timeScratch_;    ///< Time values scratch
    float64 *valScratch_;     ///< Data values scratch
    float64 *tsBatchScratch_; ///< Timestamps scratch for batch write
    uint32   timeScratchLen_;

    /* Trigger hookup (resolution cache is receive-thread only). */
    TriggerEngine *trigEngine_;     ///< Shared hub trigger (may be NULL)
    uint32         trigEpochSeen_;  ///< Last resolved trigger config epoch
    MARTe::int32   trigSigIdx_;     ///< Watched signal index (-1 = none)
    MARTe::int32   trigElemIdx_;    ///< Watched element index (-1 = all)

    /* Binary recorder (capture on receive thread, I/O on push thread). */
    BinaryRecorder recorder_;
    RecorderConfig recCfg_;                 ///< Recorder config (enabled flag gates use)
    mutable FastPollingMutexSem recSpecMutex_;
    char           recPendingSpec_[1024];   ///< Subset spec awaiting adoption
    char           recActiveSpec_[1024];    ///< Currently applied subset spec
    volatile uint32 recPendingEpoch_;       ///< Bumped when the spec changes
    uint32         recSeenEpoch_;            ///< Last spec epoch adopted (receive thread)
};

} /* namespace StreamHub */

#endif /* STREAMHUB_UDP_SOURCE_SESSION_H_ */