/**
 * @file UDPSourceSession.cpp
 * @brief UDPSourceSession implementation.
 */

#include "UDPSourceSession.h"
#include "AdvancedErrorManagement.h"
#include "ConfigurationDatabase.h"
#include "Sleep.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>

namespace StreamHub {

using MARTe::UDPS_SIGNAL_DESC_SIZE;
using MARTe::UDPS_QUANT_NONE;
using MARTe::UDPS_QUANT_UINT8;
using MARTe::UDPS_QUANT_INT8;
using MARTe::UDPS_QUANT_UINT16;
using MARTe::UDPS_QUANT_INT16;
using MARTe::UDPS_TYPECODE_UINT8;
using MARTe::UDPS_TYPECODE_INT8;
using MARTe::UDPS_TYPECODE_UINT16;
using MARTe::UDPS_TYPECODE_INT16;
using MARTe::UDPS_TYPECODE_UINT32;
using MARTe::UDPS_TYPECODE_INT32;
using MARTe::UDPS_TYPECODE_UINT64;
using MARTe::UDPS_TYPECODE_INT64;
using MARTe::UDPS_TYPECODE_FLOAT32;
using MARTe::UDPS_TYPECODE_FLOAT64;
using MARTe::UDPS_TIMEMODE_FIRST_SAMPLE;
using MARTe::UDPS_TIMEMODE_LAST_SAMPLE;
using MARTe::UDPS_TIMEMODE_FULL_ARRAY;
using MARTe::UDPS_NO_TIME_SIGNAL;
using MARTe::UDPS_PUBLISH_ACCUMULATE;
using MARTe::ConfigurationDatabase;

/** Local absolute value (avoids pulling in <math.h>). */
static inline float64 Fabs(float64 x) { return (x < 0.0) ? -x : x; }

/*---------------------------------------------------------------------------*/
/* Constructor / Destructor                                                   */
/*---------------------------------------------------------------------------*/

UDPSourceSession::UDPSourceSession()
    : port_(0u),
      dataPort_(0u),
      maxPoints_(20000u),
      ringTemporal_(1000000u),
      ringScalar_(100000u),
      initialised_(false),
      numSignals_(0u),
      publishMode_(0u),
      configured_(false),
      statSeenFirst_(false),
      statLastCounter_(0u),
      statTotalRx_(0u),
      statTotalLost_(0u),
      ctHead_(0u),
      ctFull_(false),
      statLastRxTicks_(0u),
      statFragCount_(0u),
      statByteCount_(0u),
      hrtFreq_(static_cast<float64>(MARTe::HighResolutionTimer::Frequency())),
      timeScratch_(static_cast<float64 *>(0)),
      valScratch_(static_cast<float64 *>(0)),
      tsBatchScratch_(static_cast<float64 *>(0)),
      timeScratchLen_(0u),
      trigEngine_(static_cast<TriggerEngine *>(0)),
      trigEpochSeen_(0xFFFFFFFFu),
      trigSigIdx_(-1),
      trigElemIdx_(-1),
      recPendingEpoch_(0u),
      recSeenEpoch_(0u) {
    stateStr_ = "disconnected";
    memset(&recCfg_, 0, sizeof(recCfg_));
    recPendingSpec_[0] = '\0';
    strncpy(recActiveSpec_, "all", sizeof(recActiveSpec_) - 1u);
    recActiveSpec_[sizeof(recActiveSpec_) - 1u] = '\0';
    recSpecMutex_.Create();
    ResetCalibration();
}

UDPSourceSession::~UDPSourceSession() {
    (void) Stop();
    delete[] timeScratch_;
    delete[] valScratch_;
    delete[] tsBatchScratch_;
}

void UDPSourceSession::ResetCalibration() {
    for (uint32 i = 0u; i < UDPSS_MAX_SIGNALS; i++) {
        timeSigCalibValid_[i] = false;
        timeSigCalib_[i]      = 0.0;
        timeSigLastValid_[i]  = false;
        timeSigLastTimerS_[i] = 0.0;
        lastPktWallValid_[i]  = false;
        lastPktWallS_[i]      = 0.0;
        accScalarPrevN_[i]    = 0u;
    }
}

/*---------------------------------------------------------------------------*/
/* Initialise / Start / Stop                                                  */
/*---------------------------------------------------------------------------*/

bool UDPSourceSession::Initialise(const char *id, const char *label,
                                   const char *addr, uint16 port,
                                   uint32 maxPts,
                                   const char *mcGroup,
                                   uint16 dataPort) {
    (void) Stop();

    id_    = id;
    label_ = label;
    addr_  = addr;
    port_  = port;
    mcGroup_  = (mcGroup != static_cast<const char *>(0)) ? mcGroup : "";
    dataPort_ = dataPort;
    maxPoints_ = (maxPts > 0u) ? maxPts : 20000u;

    /* Build ConfigurationDatabase for UDPSClient */
    ConfigurationDatabase cfg;
    (void) cfg.Write("ServerAddr", addr);
    (void) cfg.Write("Port", static_cast<uint32>(port));
    if ((mcGroup != static_cast<const char *>(0)) && (strlen(mcGroup) > 0u)) {
        (void) cfg.Write("MulticastGroup", mcGroup);
        if (dataPort > 0u) {
            (void) cfg.Write("DataPort", static_cast<uint32>(dataPort));
        }
    }

    if (!client_.Initialise(cfg)) {
        REPORT_ERROR_STATIC(MARTe::ErrorManagement::FatalError,
                            "UDPSourceSession[%s]: Failed to initialise UDPSClient.", id);
        return false;
    }
    client_.SetListener(this);

    (void) metaMutex_.FastLock();
    numSignals_ = 0u;
    configured_ = false;
    metaMutex_.FastUnLock();

    initialised_ = true;
    return true;
}

bool UDPSourceSession::Start() {
    if (!initialised_) { return false; }
    (void) statsMutex_.FastLock();
    /* Reset all stat accumulators */
    statSeenFirst_   = false;
    statLastCounter_ = 0u;
    statTotalRx_     = 0u;
    statTotalLost_   = 0u;
    ctHead_          = 0u;
    ctFull_          = false;
    statLastRxTicks_ = 0u;
    statFragCount_   = 0u;
    statByteCount_   = 0u;
    stateStr_ = "connecting";
    statsMutex_.FastUnLock();
    return client_.Start();
}

bool UDPSourceSession::Stop() {
    if (!initialised_) { return true; }
    return client_.Stop();
}

/*---------------------------------------------------------------------------*/
/* UDPSClientListener callbacks                                               */
/*---------------------------------------------------------------------------*/

void UDPSourceSession::OnUDPSConnected() {
    ResetCalibration();
    (void) statsMutex_.FastLock();
    stateStr_ = "connected";
    statsMutex_.FastUnLock();
    REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
                        "UDPSourceSession[%s]: Connected.", id_.Buffer());
}

void UDPSourceSession::OnUDPSDisconnected() {
    (void) statsMutex_.FastLock();
    stateStr_ = "disconnected";
    statsMutex_.FastUnLock();
    REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
                        "UDPSourceSession[%s]: Disconnected.", id_.Buffer());
}

void UDPSourceSession::OnUDPSConfig(const uint8 *payload, uint32 payloadSize) {
    ParseConfigPayload(payload, payloadSize);
}

void UDPSourceSession::OnUDPSData(const uint8 *payload, uint32 payloadSize) {
    ParseDataPayload(payload, payloadSize);
}

/*---------------------------------------------------------------------------*/
/* CONFIG parsing                                                             */
/*---------------------------------------------------------------------------*/

void UDPSourceSession::ParseConfigPayload(const uint8 *payload, uint32 size) {
    if (size < 4u) { return; }

    uint32 numSigs = 0u;
    memcpy(&numSigs, payload, 4u);
    if (numSigs > UDPSS_MAX_SIGNALS) { numSigs = UDPSS_MAX_SIGNALS; }

    uint32 expectedSize = 4u + numSigs * UDPS_SIGNAL_DESC_SIZE + 1u;
    if (size < expectedSize) { return; }

    (void) metaMutex_.FastLock();

    for (uint32 i = 0u; i < numSigs; i++) {
        memcpy(&sigDescs_[i],
               payload + 4u + i * UDPS_SIGNAL_DESC_SIZE,
               UDPS_SIGNAL_DESC_SIZE);
    }
    publishMode_ = payload[4u + numSigs * UDPS_SIGNAL_DESC_SIZE];
    numSignals_  = numSigs;
    configured_  = true;

    metaMutex_.FastUnLock();

    /* Config change invalidates wall-clock calibration (Go hub configSeq). */
    ResetCalibration();

    /* Signal list changed: force trigger-key re-resolution on the next DATA. */
    trigEpochSeen_ = 0xFFFFFFFFu;
    trigSigIdx_    = -1;

    /* (Re)allocate the time-signal decode scratch to the largest element count. */
    uint32 maxElems = 1u;
    for (uint32 i = 0u; i < numSigs; i++) {
        uint32 ne = sigDescs_[i].numRows * sigDescs_[i].numCols;
        if (ne > maxElems) { maxElems = ne; }
    }
    if (maxElems > timeScratchLen_) {
        delete[] timeScratch_;
        delete[] valScratch_;
        delete[] tsBatchScratch_;
        timeScratch_    = new float64[maxElems];
        valScratch_     = new float64[maxElems];
        tsBatchScratch_ = new float64[maxElems];
        timeScratchLen_ = maxElems;
    }

    /* Allocate ring buffers (outside metaMutex to avoid long hold) */
    AllocateRingBuffers();

    /* (Re)configure the recorder: a CONFIG change re-headers a fresh file and
     * adopts any pending subset spec. */
    if (recCfg_.enabled) {
        bool mask[UDPSS_MAX_SIGNALS];
        (void) recSpecMutex_.FastLock();
        strncpy(recActiveSpec_, recPendingSpec_, sizeof(recActiveSpec_) - 1u);
        recActiveSpec_[sizeof(recActiveSpec_) - 1u] = '\0';
        recSeenEpoch_ = recPendingEpoch_;
        recSpecMutex_.FastUnLock();
        ComputeIncludeMask(sigDescs_, numSigs, recActiveSpec_, mask);
        recorder_.Configure(sigDescs_, numSigs, mask);
    }

    REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
                        "UDPSourceSession[%s]: CONFIG received — %u signals.",
                        id_.Buffer(), numSigs);
}

void UDPSourceSession::AllocateRingBuffers() {
    (void) metaMutex_.FastLock();
    uint32 nSigs = numSignals_;
    uint32 nElems[UDPSS_MAX_SIGNALS];
    for (uint32 i = 0u; i < nSigs; i++) {
        nElems[i] = sigDescs_[i].numRows * sigDescs_[i].numCols;
        if (nElems[i] == 0u) { nElems[i] = 1u; }
    }
    metaMutex_.FastUnLock();

    /* Go hub policy (hub.go): scalar signals get the small ring; any
     * multi-element signal (waveform) gets the large temporal ring. */
    for (uint32 i = 0u; i < nSigs; i++) {
        const uint32 cap = (nElems[i] > 1u) ? ringTemporal_ : ringScalar_;
        (void) rings_[i].Allocate(cap);
    }
}

/*---------------------------------------------------------------------------*/
/* DATA parsing                                                               */
/*---------------------------------------------------------------------------*/

void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
    if (size < 8u) { return; }

    /* Copy metadata under lock */
    (void) metaMutex_.FastLock();
    uint32 nSigs = numSignals_;
    uint8  pm    = publishMode_;
    UDPSSignalDescriptor descs[UDPSS_MAX_SIGNALS];
    if (nSigs > 0u) {
        memcpy(descs, sigDescs_, nSigs * sizeof(UDPSSignalDescriptor));
    }
    metaMutex_.FastUnLock();

    if (nSigs == 0u) { return; }

    uint64 hrtTimestamp = 0u;
    memcpy(&hrtTimestamp, payload, 8u);

    /* Re-resolve the trigger signal key when the trigger config changes. */
    if (trigEngine_ != static_cast<TriggerEngine *>(0)) {
        const uint32 ep = trigEngine_->GetConfigEpoch();
        if (ep != trigEpochSeen_) {
            trigEpochSeen_ = ep;
            ResolveTriggerSignal(descs, nSigs);
        }
    }

    /* Packet arrival wall-clock time — basis of all timestamps (Unix seconds),
     * mirroring the Go hub's DataSample.WallTime semantics. */
    struct timespec tsNow;
    (void) clock_gettime(CLOCK_REALTIME, &tsNow);
    const float64 wallNowS = static_cast<float64>(tsNow.tv_sec) +
                             static_cast<float64>(tsNow.tv_nsec) * 1.0e-9;

    uint32 offset = 8u;
    uint32 numSamples = 1u;

    if (pm == UDPS_PUBLISH_ACCUMULATE) {
        if (offset + 4u > size) { return; }
        memcpy(&numSamples, payload + offset, 4u);
        offset += 4u;
        if (numSamples == 0u) { numSamples = 1u; }
    }

    /* Pass 1: compute each signal's payload offset + element count, bounds-checked. */
    uint32 sigOff[UDPSS_MAX_SIGNALS];
    uint32 sigElems[UDPSS_MAX_SIGNALS];
    uint32 off = offset;
    for (uint32 s = 0u; s < nSigs; s++) {
        const UDPSSignalDescriptor &desc = descs[s];
        uint32 numElements = desc.numRows * desc.numCols;
        if (numElements == 0u) { numElements = 1u; }

        uint32 wireElemBytes = (desc.quantType != UDPS_QUANT_NONE)
                               ? QuantWireBytes(desc.quantType)
                               : MARTe::UDPSTypeCodeByteSize(desc.typeCode);
        if (wireElemBytes == 0u) { return; }

        uint32 elemsToRead = ((pm == UDPS_PUBLISH_ACCUMULATE) && (numElements == 1u))
                             ? numSamples
                             : numElements;

        if (off + elemsToRead * wireElemBytes > size) { return; }
        sigOff[s]   = off;
        sigElems[s] = elemsToRead;
        off += elemsToRead * wireElemBytes;
    }

    /* The decode scratch is sized at CONFIG time to the largest per-signal
     * element count, but an Accumulate packet writes numSamples elements per
     * accumulated scalar — for a lone scalar source that count is 1, so the
     * batch decode would overrun valScratch_/tsBatchScratch_ (heap corruption
     * that scrambled timestamps into the value stream).  Grow the scratch to
     * the largest element count actually present in THIS packet before pass 2.
     * Runs on the receive thread (the only writer of these buffers) and only
     * when the batch first exceeds the current capacity. */
    uint32 maxNeeded = 1u;
    for (uint32 s = 0u; s < nSigs; s++) {
        if (sigElems[s] > maxNeeded) { maxNeeded = sigElems[s]; }
    }
    if (maxNeeded > timeScratchLen_) {
        delete[] timeScratch_;
        delete[] valScratch_;
        delete[] tsBatchScratch_;
        timeScratch_    = new float64[maxNeeded];
        valScratch_     = new float64[maxNeeded];
        tsBatchScratch_ = new float64[maxNeeded];
        timeScratchLen_ = maxNeeded;
    }

    /* Pass 2: decode values and timestamps; write ring buffers.
     * Timestamp logic mirrors Go buildBinaryDataMessageForSource (hub.go). */
    static const float64 kRecalibThresholdS = 2.0;

    for (uint32 s = 0u; s < nSigs; s++) {
        const UDPSSignalDescriptor &desc = descs[s];
        uint32 numElements = desc.numRows * desc.numCols;
        if (numElements == 0u) { numElements = 1u; }
        const uint32 nElems = sigElems[s];

        const bool isFirstLast = (numElements > 1u) &&
            ((desc.timeMode == UDPS_TIMEMODE_FIRST_SAMPLE) ||
             (desc.timeMode == UDPS_TIMEMODE_LAST_SAMPLE));
        const bool isFullArray = (desc.timeMode == UDPS_TIMEMODE_FULL_ARRAY);

        /* Resolve the referenced time signal (if any). */
        const bool hasTimeSig = (desc.timeSignalIdx != UDPS_NO_TIME_SIGNAL) &&
                                (desc.timeSignalIdx < nSigs);
        const uint32 tIdx = hasTimeSig ? desc.timeSignalIdx : 0u;
        const float64 timerToSec = (hasTimeSig &&
                                    (descs[tIdx].typeCode == UDPS_TYPECODE_UINT64))
                                   ? 1.0e-9 : 1.0e-6;

        if (isFirstLast) {
            /* Anchor from time signal (first element) or arrival time. */
            bool anchorIsFirstSample = (desc.timeMode == UDPS_TIMEMODE_FIRST_SAMPLE);
            float64 anchor = wallNowS;
            if (hasTimeSig && (sigElems[tIdx] >= 1u)) {
                float64 tv0 = 0.0;
                DecodeElems(payload, sigOff[tIdx], descs[tIdx], 1u, &tv0);
                const float64 timerS = tv0 * timerToSec;
                const float64 calib = CalibrateTimeSignal(tIdx, timerS, wallNowS);
                anchor = calib + timerS;
            } else {
                anchorIsFirstSample = false;
            }
            /* Batch decode: compute all timestamps and values, then batch write. */
            const float64 dt = (desc.samplingRate > 0.0) ? (1.0 / desc.samplingRate) : 0.0;
            DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
            for (uint32 e = 0u; e < nElems; e++) {
                tsBatchScratch_[e] = anchorIsFirstSample
                    ? (anchor + static_cast<float64>(e) * dt)
                    : (anchor - static_cast<float64>(nElems - 1u - e) * dt);
            }
            WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
        }
        else if (isFullArray) {
            /* Per-element timestamps from the referenced time-signal array.
             * Batch decode + batch write (single ring lock per signal). */
            if (hasTimeSig && (sigElems[tIdx] >= nElems) &&
                (nElems <= timeScratchLen_)) {
                DecodeElems(payload, sigOff[tIdx], descs[tIdx], nElems, timeScratch_);
                const float64 timer0S = timeScratch_[0] * timerToSec;
                const float64 calib = CalibrateTimeSignal(tIdx, timer0S, wallNowS);
                DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
                for (uint32 e = 0u; e < nElems; e++) {
                    tsBatchScratch_[e] = calib + timeScratch_[e] * timerToSec;
                }
                WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
            } else {
                /* No time signal: all samples get arrival wall time. */
                DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
                for (uint32 e = 0u; e < nElems; e++) {
                    tsBatchScratch_[e] = wallNowS;
                }
                WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
            }
        }
        else if (numElements == 1u) {
            if (nElems <= 1u) {
                /* Plain scalar: arrival wall time (Go n==1 case). */
                const float64 val = DecodeOneElem(payload, sigOff[s], desc, 0u);
                WriteSample(s, 0u, wallNowS, val);
            }
            else {
                /* Accumulated scalar: reconstruct per-sample timestamps from
                 * the packet's embedded sender HRT (the acquisition time of the
                 * oldest sample), NOT the packet arrival time.  The kernel
                 * frequently delivers several queued datagrams in one burst, so
                 * two packets can be processed microseconds apart even though
                 * each represents ~10 ms of signal; arrival-time interpolation
                 * then crams a packet's samples into that tiny gap, rendering
                 * the trace as a sawtooth ("chainsaw").  The sender HRT is
                 * immune to this because it is sampled at acquisition.
                 *
                 * hrtTimestamp is the HRT counter of sample 0; hrtFreq_ (the
                 * local HRT frequency, identical to the sender on the same
                 * host) converts it to seconds, then a one-time calibration
                 * maps the sender clock onto wall-clock. */
                const float64 hrt0Sec = static_cast<float64>(hrtTimestamp) /
                                        hrtFreq_;
                if ((!timeSigCalibValid_[s]) ||
                    (Fabs((timeSigCalib_[s] + hrt0Sec) - wallNowS) >
                     kRecalibThresholdS)) {
                    timeSigCalib_[s]      = wallNowS - hrt0Sec;
                    timeSigCalibValid_[s] = true;
                }

                /* Per-sample dt: samplingRate if present, else derive it from
                 * the sender-HRT gap to the previous packet divided by that
                 * packet's sample count (the flushes carry contiguous RT
                 * cycles, so this is exactly one cycle period). */
                float64 dt;
                if (desc.samplingRate > 0.0) {
                    dt = 1.0 / desc.samplingRate;
                } else if (lastPktWallValid_[s] && (accScalarPrevN_[s] > 0u) &&
                           (hrt0Sec > lastPktWallS_[s])) {
                    dt = (hrt0Sec - lastPktWallS_[s]) /
                         static_cast<float64>(accScalarPrevN_[s]);
                } else {
                    dt = 1.0e-3; /* 1 kHz default until the gap is known */
                }

                const float64 base = timeSigCalib_[s] + hrt0Sec;
                DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
                for (uint32 e = 0u; e < nElems; e++) {
                    tsBatchScratch_[e] = base + static_cast<float64>(e) * dt;
                }
                WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);

                lastPktWallS_[s]     = hrt0Sec; /* sender seconds for next dt */
                lastPktWallValid_[s] = true;
                accScalarPrevN_[s]   = nElems;
            }
        }
        else {
            /* Packed TIMEMODE_PACKET array: interpolate per-element timestamps
             * from the inter-packet wall-clock gap (Go default case). The very
             * first packet is skipped to avoid poisoning the ring with
             * wrongly-spaced timestamps.
             *
             * Unlike the Go hub (which extrapolates forward from arrival time
             * and overlaps the next packet under jitter), elements span
             * (lastPktWall, wallNow]: the samples were acquired before the
             * packet arrived, and this keeps ring time strictly monotonic. */
            if (lastPktWallValid_[s] && (wallNowS >= lastPktWallS_[s])) {
                float64 dt;
                float64 base;
                if (wallNowS > lastPktWallS_[s]) {
                    dt   = (wallNowS - lastPktWallS_[s]) /
                           static_cast<float64>(nElems);
                    base = lastPktWallS_[s];
                } else {
                    dt   = (desc.samplingRate > 0.0)
                           ? (1.0 / desc.samplingRate) : 1.0e-6;
                    base = lastPktWallS_[s] - static_cast<float64>(nElems) * dt;
                }
                DecodeElems(payload, sigOff[s], desc, nElems, valScratch_);
                for (uint32 e = 0u; e < nElems; e++) {
                    tsBatchScratch_[e] = base + static_cast<float64>(e + 1u) * dt;
                }
                WriteSampleBatch(s, tsBatchScratch_, valScratch_, nElems);
            }
            lastPktWallS_[s]     = wallNowS;
            lastPktWallValid_[s] = true;
        }
    }

    /* Binary recorder: adopt any pending subset spec, then capture the packet
     * verbatim (native types, lossless) for disk recording. */
    if (recCfg_.enabled) {
        if (recPendingEpoch_ != recSeenEpoch_) {
            bool mask[UDPSS_MAX_SIGNALS];
            (void) recSpecMutex_.FastLock();
            strncpy(recActiveSpec_, recPendingSpec_, sizeof(recActiveSpec_) - 1u);
            recActiveSpec_[sizeof(recActiveSpec_) - 1u] = '\0';
            recSeenEpoch_ = recPendingEpoch_;
            recSpecMutex_.FastUnLock();
            ComputeIncludeMask(descs, nSigs, recActiveSpec_, mask);
            recorder_.Configure(descs, nSigs, mask);
        }
        recorder_.CapturePacket(payload, sigOff, sigElems, pm, numSamples);
    }
}

float64 UDPSourceSession::DecodeOneElem(const uint8 *payload, uint32 off,
                                        const UDPSSignalDescriptor &desc,
                                        uint32 e) const {
    const uint32 wireElemBytes = (desc.quantType != UDPS_QUANT_NONE)
                                 ? QuantWireBytes(desc.quantType)
                                 : MARTe::UDPSTypeCodeByteSize(desc.typeCode);
    const uint8 *ptr = payload + off + e * wireElemBytes;
    if (desc.quantType != UDPS_QUANT_NONE) {
        const float64 raw = DecodeRawValue(ptr,
            (desc.quantType == UDPS_QUANT_UINT8)  ? UDPS_TYPECODE_UINT8  :
            (desc.quantType == UDPS_QUANT_INT8)   ? UDPS_TYPECODE_INT8   :
            (desc.quantType == UDPS_QUANT_UINT16) ? UDPS_TYPECODE_UINT16 :
                                                    UDPS_TYPECODE_INT16);
        return DequantizeValue(raw, desc.quantType, desc.rangeMin, desc.rangeMax, false);
    }
    return DecodeRawValue(ptr, desc.typeCode);
}

void UDPSourceSession::DecodeElems(const uint8 *payload, uint32 off,
                                   const UDPSSignalDescriptor &desc,
                                   uint32 nElems, float64 *out) const {
    for (uint32 e = 0u; e < nElems; e++) {
        out[e] = DecodeOneElem(payload, off, desc, e);
    }
}

/*---------------------------------------------------------------------------*/
/* Value decode helpers                                                       */
/*---------------------------------------------------------------------------*/

float64 UDPSourceSession::DecodeRawValue(const uint8 *ptr, uint8 typeCode) const {
    switch (typeCode) {
        case UDPS_TYPECODE_UINT8: {
            uint8 v = 0u; memcpy(&v, ptr, 1u); return static_cast<float64>(v);
        }
        case UDPS_TYPECODE_INT8: {
            MARTe::int8 v = 0; memcpy(&v, ptr, 1u); return static_cast<float64>(v);
        }
        case UDPS_TYPECODE_UINT16: {
            uint16 v = 0u; memcpy(&v, ptr, 2u); return static_cast<float64>(v);
        }
        case UDPS_TYPECODE_INT16: {
            MARTe::int16 v = 0; memcpy(&v, ptr, 2u); return static_cast<float64>(v);
        }
        case UDPS_TYPECODE_UINT32: {
            uint32 v = 0u; memcpy(&v, ptr, 4u); return static_cast<float64>(v);
        }
        case UDPS_TYPECODE_INT32: {
            MARTe::int32 v = 0; memcpy(&v, ptr, 4u); return static_cast<float64>(v);
        }
        case UDPS_TYPECODE_UINT64: {
            uint64 v = 0u; memcpy(&v, ptr, 8u); return static_cast<float64>(v);
        }
        case UDPS_TYPECODE_INT64: {
            MARTe::int64 v = 0; memcpy(&v, ptr, 8u); return static_cast<float64>(v);
        }
        case UDPS_TYPECODE_FLOAT32: {
            float v = 0.0f; memcpy(&v, ptr, 4u); return static_cast<float64>(v);
        }
        case UDPS_TYPECODE_FLOAT64: {
            float64 v = 0.0; memcpy(&v, ptr, 8u); return v;
        }
        default: return 0.0;
    }
}

float64 UDPSourceSession::DequantizeValue(float64 rawOrQuant, uint8 quantType,
                                           float64 rangeMin, float64 rangeMax,
                                           bool /*isRaw*/) const {
    float64 range = rangeMax - rangeMin;
    switch (quantType) {
        case UDPS_QUANT_UINT8:
            return rangeMin + (rawOrQuant / 255.0) * range;
        case UDPS_QUANT_INT8:
            return rangeMin + ((rawOrQuant + 127.0) / 254.0) * range;
        case UDPS_QUANT_UINT16:
            return rangeMin + (rawOrQuant / 65535.0) * range;
        case UDPS_QUANT_INT16:
            return rangeMin + ((rawOrQuant + 32767.0) / 65534.0) * range;
        default:
            return rawOrQuant;
    }
}

uint32 UDPSourceSession::QuantWireBytes(uint8 quantType) const {
    switch (quantType) {
        case UDPS_QUANT_UINT8:  return 1u;
        case UDPS_QUANT_INT8:   return 1u;
        case UDPS_QUANT_UINT16: return 2u;
        case UDPS_QUANT_INT16:  return 2u;
        default:                return 0u;
    }
}

/*---------------------------------------------------------------------------*/
/* Statistics (port of Go SourceStat.RecordFragment, stats.go)                */
/*---------------------------------------------------------------------------*/

void UDPSourceSession::OnUDPSFragment(uint32 counter, uint32 nBytes,
                                      bool complete) {
    const uint64 nowTicks = MARTe::HighResolutionTimer::Counter();

    (void) statsMutex_.FastLock();

    statFragCount_++;
    statByteCount_ += nBytes;

    if (!complete) {
        statsMutex_.FastUnLock();
        return;
    }

    statTotalRx_++;
    if (statSeenFirst_) {
        const uint32 delta = counter - statLastCounter_; /* wraps correctly */
        if (delta > 1u) {
            statTotalLost_ += static_cast<uint64>(delta - 1u);
        }
    } else {
        statSeenFirst_ = true;
    }
    statLastCounter_ = counter;

    if (statLastRxTicks_ != 0u) {
        const float64 hrtFreq =
            static_cast<float64>(MARTe::HighResolutionTimer::Frequency());
        const uint32 idx = ctHead_;
        ctRing_[idx]   = static_cast<float64>(nowTicks - statLastRxTicks_) / hrtFreq;
        fragRing_[idx] = statFragCount_;
        byteRing_[idx] = statByteCount_;
        ctHead_ = (ctHead_ + 1u) % kStatRingSize;
        if (ctHead_ == 0u) { ctFull_ = true; }
    }
    statLastRxTicks_ = nowTicks;
    statFragCount_   = 0u;
    statByteCount_   = 0u;

    statsMutex_.FastUnLock();
}

/*---------------------------------------------------------------------------*/
/* Thread-safe read accessors                                                 */
/*---------------------------------------------------------------------------*/

uint32 UDPSourceSession::GetNumSignals() const {
    (void) metaMutex_.FastLock();
    uint32 n = numSignals_;
    metaMutex_.FastUnLock();
    return n;
}

bool UDPSourceSession::GetSignalDescriptor(uint32 idx, UDPSSignalDescriptor &desc) const {
    (void) metaMutex_.FastLock();
    bool ok = (configured_ && idx < numSignals_);
    if (ok) { memcpy(&desc, &sigDescs_[idx], sizeof(UDPSSignalDescriptor)); }
    metaMutex_.FastUnLock();
    return ok;
}

uint8 UDPSourceSession::GetPublishMode() const {
    (void) metaMutex_.FastLock();
    uint8 pm = publishMode_;
    metaMutex_.FastUnLock();
    return pm;
}

uint32 UDPSourceSession::ReadSignalLast(uint32 idx, uint32 n,
                                         float64 *tOut, float64 *vOut) const {
    if (idx >= UDPSS_MAX_SIGNALS) { return 0u; }
    return rings_[idx].Read(tOut, vOut, n);
}

uint32 UDPSourceSession::ReadSignalSince(uint32 idx, MARTe::uint64 &cursor,
                                          float64 *tOut, float64 *vOut,
                                          uint32 maxOut) const {
    if (idx >= UDPSS_MAX_SIGNALS) { return 0u; }
    return rings_[idx].ReadSince(cursor, tOut, vOut, maxOut);
}

uint32 UDPSourceSession::ReadSignalRange(uint32 idx, float64 t0, float64 t1,
                                          float64 *tOut, float64 *vOut,
                                          uint32 maxOut) const {
    if (idx >= UDPSS_MAX_SIGNALS) { return 0u; }
    return rings_[idx].ReadRange(t0, t1, tOut, vOut, maxOut);
}

UDPSourceStats UDPSourceSession::GetStats() const {
    /* Port of Go SourceStat.Snapshot (stats.go). */
    UDPSourceStats si;

    (void) statsMutex_.FastLock();

    si.state         = stateStr_;
    si.totalReceived = statTotalRx_;
    si.totalLost     = statTotalLost_;

    const uint32 n = ctFull_ ? kStatRingSize : ctHead_;
    if (n == 0u) {
        statsMutex_.FastUnLock();
        return si;
    }

    float64 sum = 0.0;
    float64 sumSq = 0.0;
    float64 minV = 1.0e308;
    float64 maxV = 0.0;
    uint64  fragSum = 0u;
    uint64  byteSum = 0u;
    for (uint32 i = 0u; i < n; i++) {
        const float64 v = ctRing_[i];
        sum   += v;
        sumSq += v * v;
        if (v < minV) { minV = v; }
        if (v > maxV) { maxV = v; }
        fragSum += fragRing_[i];
        byteSum += byteRing_[i];
    }
    const float64 fn  = static_cast<float64>(n);
    const float64 avg = sum / fn;
    float64 variance  = (sumSq / fn) - (avg * avg);
    if (variance < 0.0) { variance = 0.0; }
    /* sqrt without <math.h>: Newton iterations (variance ≥ 0) */
    float64 stdv = variance;
    if (stdv > 0.0) {
        float64 x = variance;
        for (uint32 it = 0u; it < 32u; it++) {
            x = 0.5 * (x + variance / x);
        }
        stdv = x;
    }

    si.cycleAvgMs    = avg * 1.0e3;
    si.cycleStdMs    = stdv * 1.0e3;
    si.cycleMinMs    = minV * 1.0e3;
    si.cycleMaxMs    = maxV * 1.0e3;
    si.rateHz        = (avg > 0.0) ? (1.0 / avg) : 0.0;
    si.rateStdHz     = (avg > 0.0) ? (stdv / (avg * avg)) : 0.0;
    si.fragsPerCycle = static_cast<float64>(fragSum) / fn;
    si.bytesPerCycle = static_cast<float64>(byteSum) / fn;

    /* 20-bin histogram over [minV, maxV] */
    si.cycleHistMin = minV * 1.0e3;
    si.cycleHistMax = maxV * 1.0e3;
    si.histValid    = true;
    const float64 span = maxV - minV;
    for (uint32 i = 0u; i < n; i++) {
        uint32 bin;
        if (span > 0.0) {
            bin = static_cast<uint32>(((ctRing_[i] - minV) / span) *
                                      static_cast<float64>(UDPS_STAT_HIST_BINS));
            if (bin >= UDPS_STAT_HIST_BINS) { bin = UDPS_STAT_HIST_BINS - 1u; }
        } else {
            bin = UDPS_STAT_HIST_BINS / 2u;
        }
        si.cycleHist[bin]++;
    }

    statsMutex_.FastUnLock();
    return si;
}

StreamString UDPSourceSession::GetId() const { return id_; }

StreamString UDPSourceSession::GetLabel() const { return label_; }

StreamString UDPSourceSession::GetAddr() const { return addr_; }

uint16 UDPSourceSession::GetPort() const { return port_; }

StreamString UDPSourceSession::GetMulticastGroup() const { return mcGroup_; }

uint16 UDPSourceSession::GetDataPort() const { return dataPort_; }

bool UDPSourceSession::IsConfigured() const {
    (void) metaMutex_.FastLock();
    bool c = configured_;
    metaMutex_.FastUnLock();
    return c;
}

bool UDPSourceSession::IsInitialised() const { return initialised_; }

bool UDPSourceSession::SetMaxPoints(uint32 maxPts) {
    if (maxPts == 0u) { return false; }
    maxPoints_ = maxPts;
    return true;
}

void UDPSourceSession::SetRingCapacities(uint32 temporal, uint32 scalar) {
    if (temporal > 0u) { ringTemporal_ = temporal; }
    if (scalar > 0u)   { ringScalar_   = scalar; }
}

void UDPSourceSession::SetTriggerEngine(TriggerEngine *engine) {
    trigEngine_    = engine;
    trigEpochSeen_ = 0xFFFFFFFFu;
    trigSigIdx_    = -1;
    trigElemIdx_   = -1;
}

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

void UDPSourceSession::SetRecorderConfig(const RecorderConfig &cfg) {
    recCfg_ = cfg;
    if (!cfg.enabled) { return; }
    recorder_.Init(cfg, id_.Buffer());
    (void) recSpecMutex_.FastLock();
    strncpy(recPendingSpec_, cfg.signals, sizeof(recPendingSpec_) - 1u);
    recPendingSpec_[sizeof(recPendingSpec_) - 1u] = '\0';
    strncpy(recActiveSpec_, cfg.signals, sizeof(recActiveSpec_) - 1u);
    recActiveSpec_[sizeof(recActiveSpec_) - 1u] = '\0';
    recPendingEpoch_++;
    recSpecMutex_.FastUnLock();
}

void UDPSourceSession::SetRecorderSignals(const char *spec) {
    if (spec == static_cast<const char *>(0)) { return; }
    (void) recSpecMutex_.FastLock();
    strncpy(recPendingSpec_, spec, sizeof(recPendingSpec_) - 1u);
    recPendingSpec_[sizeof(recPendingSpec_) - 1u] = '\0';
    recPendingEpoch_++;
    recSpecMutex_.FastUnLock();
}

void UDPSourceSession::RequestRecArm() {
    if (recCfg_.enabled) { recorder_.RequestArm(); }
}

void UDPSourceSession::RequestRecDisarm() {
    if (recCfg_.enabled) { recorder_.RequestDisarm(); }
}

void UDPSourceSession::RecorderFlushTick(uint32 nowSec) {
    if (recCfg_.enabled) { recorder_.FlushTick(nowSec); }
}

void UDPSourceSession::GetRecorderInfo(bool &recording, char *file, uint32 fileSz,
                                       uint64 &bytesWritten, uint64 &rowsWritten,
                                       uint64 &droppedRows, uint64 &freeMB) const {
    recorder_.GetInfo(recording, file, fileSz, bytesWritten, rowsWritten,
                      droppedRows, freeMB);
}

void UDPSourceSession::ComputeIncludeMask(const UDPSSignalDescriptor *descs,
                                          uint32 n, const char *spec,
                                          bool *maskOut) {
    if ((spec == static_cast<const char *>(0)) || (spec[0] == '\0') ||
        (strcmp(spec, "all") == 0)) {
        for (uint32 i = 0u; i < n; i++) { maskOut[i] = true; }
        return;
    }
    for (uint32 i = 0u; i < n; i++) {
        char key[160];
        (void) snprintf(key, sizeof(key), "%s:%s", id_.Buffer(), descs[i].name);
        const size_t klen = strlen(key);
        maskOut[i] = false;
        const char *p = spec;
        while (*p != '\0') {
            while ((*p == ',') || (*p == ' ')) { p++; }
            const char *start = p;
            while ((*p != '\0') && (*p != ',')) { p++; }
            size_t tlen = static_cast<size_t>(p - start);
            while ((tlen > 0u) && (start[tlen - 1u] == ' ')) { tlen--; }
            if ((tlen == klen) && (strncmp(start, key, klen) == 0)) {
                maskOut[i] = true;
                break;
            }
        }
    }
}

void UDPSourceSession::ResolveTriggerSignal(const UDPSSignalDescriptor *descs,
                                            uint32 nSigs) {
    trigSigIdx_  = -1;
    trigElemIdx_ = -1;
    if (trigEngine_ == static_cast<TriggerEngine *>(0)) { return; }

    TriggerConfig cfg = trigEngine_->GetConfig();
    const char *key = cfg.signalKey.Buffer();
    if ((key == static_cast<const char *>(0)) || (key[0] == '\0')) { return; }

    /* Key format: "src:sig" or "src:sig[i]" */
    const char *colon = strchr(key, ':');
    if (colon == static_cast<const char *>(0)) { return; }

    /* Source id must match this session */
    const uint32 srcLen = static_cast<uint32>(colon - key);
    const char  *myId   = id_.Buffer();
    if ((strlen(myId) != srcLen) || (strncmp(myId, key, srcLen) != 0)) {
        return;
    }

    /* Split optional "[i]" element suffix */
    char sigName[128];
    strncpy(sigName, colon + 1, sizeof(sigName) - 1u);
    sigName[sizeof(sigName) - 1u] = '\0';
    MARTe::int32 elemIdx = -1;
    char *bracket = strchr(sigName, '[');
    if (bracket != static_cast<char *>(0)) {
        elemIdx  = static_cast<MARTe::int32>(strtol(bracket + 1,
                                                    static_cast<char **>(0), 10));
        *bracket = '\0';
    }

    for (uint32 s = 0u; s < nSigs; s++) {
        if (strcmp(descs[s].name, sigName) == 0) {
            trigSigIdx_  = static_cast<MARTe::int32>(s);
            trigElemIdx_ = elemIdx;
            REPORT_ERROR_STATIC(MARTe::ErrorManagement::Information,
                "UDPSourceSession[%s]: trigger watching signal '%s' (idx %u, elem %d).",
                id_.Buffer(), sigName, s, elemIdx);
            break;
        }
    }
}

} /* namespace StreamHub */