/**
 * @file UDPStreamer.cpp
 * @brief Source file for class UDPStreamer
 * @date 13/05/2026
 * @author Martino Ferrari
 *
 * @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
 * the Development of Fusion Energy ('Fusion for Energy').
 * Licensed under the EUPL, Version 1.1 or - as soon they will be approved
 * by the European Commission - subsequent versions of the EUPL (the "Licence")
 * You may not use this work except in compliance with the Licence.
 * You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
 *
 * @warning Unless required by applicable law or agreed to in writing,
 * software distributed under the Licence is distributed on an "AS IS"
 * basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
 * or implied. See the Licence permissions and limitations under the Licence.
 *
 * @details This source file contains the definition of all the methods for
 * the class UDPStreamer (public, protected, and private). Be aware that some
 * methods, such as those inline could be defined on the header file, instead.
 */

#define DLL_API

/*---------------------------------------------------------------------------*/
/*                         Standard header includes                          */
/*---------------------------------------------------------------------------*/
#include <sys/select.h>
#include <unistd.h>

/*---------------------------------------------------------------------------*/
/*                         Project header includes                           */
/*---------------------------------------------------------------------------*/
#include "AdvancedErrorManagement.h"
#include "ConfigurationDatabase.h"
#include "EmbeddedThreadI.h"
#include "GlobalObjectsDatabase.h"
#include "HighResolutionTimer.h"
#include "MemoryMapSynchronisedOutputBroker.h"
#include "MemoryOperationsHelper.h"
#include "Sleep.h"
#include "Threads.h"
#include "UDPStreamer.h"

/*---------------------------------------------------------------------------*/
/*                           Static definitions                              */
/*---------------------------------------------------------------------------*/

namespace MARTe {

/** Default port used when none is specified. */
static const uint16 UDPS_DEFAULT_PORT = 44500u;

/** Default data port offset: dataPort = port + this value when DataPort is not specified. */
static const uint16 UDPS_DEFAULT_DATA_PORT_OFFSET = 1u;

/** Maximum pending TCP connections on the listener backlog. */
static const int32 UDPS_TCP_MAX_CONNECTIONS = 4;

/** Default max payload per UDP datagram (bytes). */
static const uint32 UDPS_DEFAULT_MAX_PAYLOAD = 1400u;

/** Minimum MaxPayloadSize: header + at least 1 byte of payload. */
static const uint32 UDPS_MIN_PAYLOAD = UDPS_HEADER_SIZE + 1u;

/** Server socket receive timeout in milliseconds.
 *  Set to 0 for a pure non-blocking poll so the send loop can keep pace with
 *  the RT thread regardless of its frequency.  Client commands (CONNECT /
 *  DISCONNECT) are still caught on the very next loop iteration. */
static const uint32 UDPS_RECV_TIMEOUT_MS = 0u;

/** EventSem wait timeout in milliseconds for the data loop. */
static const uint32 UDPS_DATA_WAIT_MS = 10u;

/** Bytes prepended to each DATA payload for the HRT packet timestamp. */
static const uint32 UDPS_TIMESTAMP_BYTES = 8u;

/*---------------------------------------------------------------------------*/
/*                           Method definitions                              */
/*---------------------------------------------------------------------------*/

UDPStreamer::UDPStreamer() :
        MemoryDataSourceI(),
        EmbeddedServiceMethodBinderI(),
        executor(*this) {
    port             = UDPS_DEFAULT_PORT;
    maxPayloadSize   = UDPS_DEFAULT_MAX_PAYLOAD;
    cpuMask          = 0xFFFFFFFFu;
    stackSize        = THREADS_DEFAULT_STACKSIZE;
    publishMode      = UDPStreamerPublishStrict;
    minRefreshRate   = 0.0;
    flushPeriodTicks = 0u;
    numSigs          = 0u;
    signalInfos      = NULL_PTR(UDPStreamerSignalInfo *);
    readyBuffer      = NULL_PTR(uint8 *);
    scratchBuffer    = NULL_PTR(uint8 *);
    wireBuffer       = NULL_PTR(uint8 *);
    totalSrcBytes    = 0u;
    totalWireBytes   = 0u;
    syncTimestamp    = 0u;
    packetCounter    = 0u;
    maxBatchCount        = 0u;
    singleCycleWireBytes = 0u;
    fixedWireBytes       = 0u;
    lastPublishTs        = 0u;
    accumBuffer          = NULL_PTR(uint8 *);
    accumTimestamps      = NULL_PTR(uint64 *);
    accumFill            = 0u;
    readyTimestamps      = NULL_PTR(uint64 *);
    scratchTimestamps    = NULL_PTR(uint64 *);
    readyFill            = 0u;
    decimateRatio        = 1u;
    decimateCounter      = 0u;

    if (!dataSem.Create()) {
        REPORT_ERROR(ErrorManagement::FatalError, "Could not create EventSem.");
    }
    bufMutex.Create(false);
}

/*lint -e{1551} Destructor must guarantee thread and socket cleanup. */
UDPStreamer::~UDPStreamer() {
    /* Unblock the background thread's dataSem wait so it can exit */
    (void) dataSem.Post();

    if (executor.GetStatus() != EmbeddedThreadI::OffState) {
        if (!executor.Stop()) {
            REPORT_ERROR(ErrorManagement::Warning,
                         "First Stop() attempt failed; retrying.");
            if (!executor.Stop()) {
                REPORT_ERROR(ErrorManagement::FatalError,
                             "Could not stop background thread.");
            }
        }
    }

    (void) server.Stop();

    HeapI *heapAccum = GlobalObjectsDatabase::Instance()->GetStandardHeap();
    if (accumBuffer != NULL_PTR(uint8 *)) {
        heapAccum->Free(reinterpret_cast<void *&>(accumBuffer));
    }
    if (accumTimestamps != NULL_PTR(uint64 *)) {
        delete[] accumTimestamps;
        accumTimestamps = NULL_PTR(uint64 *);
    }
    if (readyTimestamps != NULL_PTR(uint64 *)) {
        delete[] readyTimestamps;
        readyTimestamps = NULL_PTR(uint64 *);
    }
    if (scratchTimestamps != NULL_PTR(uint64 *)) {
        delete[] scratchTimestamps;
        scratchTimestamps = NULL_PTR(uint64 *);
    }

    if (signalInfos != NULL_PTR(UDPStreamerSignalInfo *)) {
        delete[] signalInfos;
        signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
    }

    HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
    if (readyBuffer != NULL_PTR(uint8 *)) {
        heap->Free(reinterpret_cast<void *&>(readyBuffer));
    }
    if (scratchBuffer != NULL_PTR(uint8 *)) {
        heap->Free(reinterpret_cast<void *&>(scratchBuffer));
    }
    if (wireBuffer != NULL_PTR(uint8 *)) {
        heap->Free(reinterpret_cast<void *&>(wireBuffer));
    }

    (void) dataSem.Close();
}

bool UDPStreamer::Initialise(StructuredDataI &data) {
    bool ok = MemoryDataSourceI::Initialise(data);

    if (ok) {
        if (!data.Read("Port", port)) {
            port = UDPS_DEFAULT_PORT;
            REPORT_ERROR(ErrorManagement::Information,
                         "Port not specified; using default %u.",
                         static_cast<uint32>(port));
        }
        if (port <= 1024u) {
            REPORT_ERROR(ErrorManagement::Warning,
                         "Port %u is in the privileged range (<= 1024).",
                         static_cast<uint32>(port));
        }
    }

    if (ok) {
        if (!data.Read("MaxPayloadSize", maxPayloadSize)) {
            maxPayloadSize = UDPS_DEFAULT_MAX_PAYLOAD;
            REPORT_ERROR(ErrorManagement::Information,
                         "MaxPayloadSize not specified; using default %u.",
                         maxPayloadSize);
        }
        if (maxPayloadSize < UDPS_MIN_PAYLOAD) {
            REPORT_ERROR(ErrorManagement::ParametersError,
                         "MaxPayloadSize %u is too small (minimum %u).",
                         maxPayloadSize, UDPS_MIN_PAYLOAD);
            ok = false;
        }
    }

    if (ok) {
        uint32 cpuMaskIn = 0xFFFFFFFFu;
        if (!data.Read("CPUMask", cpuMaskIn)) {
            REPORT_ERROR(ErrorManagement::Information,
                         "CPUMask not specified; using 0xFFFFFFFF.");
        }
        cpuMask = cpuMaskIn;
    }

    if (ok) {
        if (!data.Read("StackSize", stackSize)) {
            stackSize = THREADS_DEFAULT_STACKSIZE;
            REPORT_ERROR(ErrorManagement::Information,
                         "StackSize not specified; using MARTe2 default %u.",
                         stackSize);
        }
        if (stackSize == 0u) {
            REPORT_ERROR(ErrorManagement::ParametersError, "StackSize must be > 0.");
            ok = false;
        }
    }

    if (ok) {
        StreamString publishStr = "";
        (void) data.Read("PublishingMode", publishStr);
        if ((publishStr.Size() == 0u) || (publishStr == "Strict")) {
            publishMode = UDPStreamerPublishStrict;
        }
        else if (publishStr == "Accumulate") {
            publishMode = UDPStreamerPublishAccumulate;
        }
        else if (publishStr == "Decimate") {
            publishMode = UDPStreamerPublishDecimate;
        }
        else {
            REPORT_ERROR(ErrorManagement::ParametersError,
                         "Unknown PublishingMode '%s'. Allowed: Strict|Accumulate|Decimate.",
                         publishStr.Buffer());
            ok = false;
        }
    }

    if (ok && (publishMode == UDPStreamerPublishAccumulate)) {
        /* MinRefreshRate controls the time-based flush: flush when
         * (now - lastPublishTs) >= flushPeriodTicks, or when adding one more
         * sample would overflow MaxPayloadSize.  Whichever fires first. */
        if (!data.Read("MinRefreshRate", minRefreshRate) || (minRefreshRate <= 0.0)) {
            REPORT_ERROR(ErrorManagement::ParametersError,
                         "MinRefreshRate > 0 is required when PublishingMode = Accumulate.");
            ok = false;
        }
        else {
            float64 hrtFreq  = static_cast<float64>(HighResolutionTimer::Frequency());
            flushPeriodTicks = static_cast<uint64>(hrtFreq / minRefreshRate);
            REPORT_ERROR(ErrorManagement::Information,
                         "Accumulate mode: MinRefreshRate=%.1f Hz, flushPeriodTicks=%llu.",
                         minRefreshRate,
                         static_cast<unsigned long long>(flushPeriodTicks));
        }
    }

    if (ok && (publishMode == UDPStreamerPublishDecimate)) {
        /* Ratio: send 1 packet every Ratio Synchronise() calls. */
        uint32 ratio = 0u;
        if (!data.Read("Ratio", ratio) || (ratio == 0u)) {
            REPORT_ERROR(ErrorManagement::ParametersError,
                         "Ratio >= 1 is required when PublishingMode = Decimate.");
            ok = false;
        }
        else {
            decimateRatio = ratio;
            if (decimateRatio == 1u) {
                REPORT_ERROR(ErrorManagement::Warning,
                             "Decimate mode with Ratio=1 is equivalent to Strict mode.");
            }
            REPORT_ERROR(ErrorManagement::Information,
                         "Decimate mode: Ratio=%u (1 packet per %u RT cycle(s)).",
                         decimateRatio, decimateRatio);
        }
    }

    if (ok) {
        // Build server config with already-resolved Port and MaxPayloadSize so
        // UDPSServer::Initialise() always sees them, even when defaults were used.
        ConfigurationDatabase serverCfg;
        (void) serverCfg.Write("Port", static_cast<uint32>(port));
        (void) serverCfg.Write("MaxPayloadSize", maxPayloadSize);
        // Forward optional multicast / timeout params if present in caller's data.
        StreamString mcGroup;
        if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) {
            (void) serverCfg.Write("MulticastGroup", mcGroup.Buffer());
            uint32 dp = 0u;
            if (data.Read("DataPort", dp)) {
                (void) serverCfg.Write("DataPort", dp);
            }
        }
        uint32 clientTimeout = 0u;
        if (data.Read("ClientTimeout", clientTimeout)) {
            (void) serverCfg.Write("ClientTimeout", clientTimeout);
        }
        ok = server.Initialise(serverCfg);
    }

    return ok;
}

bool UDPStreamer::SetConfiguredDatabase(StructuredDataI &data) {
    bool ok = MemoryDataSourceI::SetConfiguredDatabase(data);
    if (!ok) {
        return false;
    }

    numSigs = GetNumberOfSignals();
    if (numSigs == 0u) {
        REPORT_ERROR(ErrorManagement::ParametersError,
                     "At least one signal must be defined.");
        return false;
    }

    signalInfos = new UDPStreamerSignalInfo[numSigs];
    /* Local array to hold time-signal names (resolved to indices in pass 3) */
    StreamString *timeSignalNames = new StreamString[numSigs];

    /* --- Pass 1: populate from the MARTe2 framework APIs --- */
    totalSrcBytes  = 0u;
    totalWireBytes = UDPS_TIMESTAMP_BYTES;

    for (uint32 i = 0u; i < numSigs && ok; i++) {
        StreamString sigName;
        ok = GetSignalName(i, sigName);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::FatalError,
                         "Could not get name for signal %u.", i);
            break;
        }
        signalInfos[i].name           = sigName;
        signalInfos[i].type           = GetSignalType(i);
        signalInfos[i].numDimensions  = 0u;
        signalInfos[i].numElements    = 1u;
        signalInfos[i].numRows        = 1u;
        signalInfos[i].numCols        = 1u;
        signalInfos[i].quantType      = UDPStreamerQuantNone;
        signalInfos[i].rangeMin       = 0.0;
        signalInfos[i].rangeMax       = 1.0;
        signalInfos[i].timeMode       = UDPStreamerTimePacket;
        signalInfos[i].samplingRate   = 0.0;
        signalInfos[i].timeSignalIdx  = UDPS_NO_TIME_SIGNAL;
        signalInfos[i].unit           = "";
        signalInfos[i].srcByteSize    = 0u;
        signalInfos[i].wireByteSize   = 0u;
        signalInfos[i].bufferOffset   = 0u;
        timeSignalNames[i]            = "";

        uint8 ndims = 0u;
        (void) GetSignalNumberOfDimensions(i, ndims);
        signalInfos[i].numDimensions = ndims;

        uint32 nelems = 1u;
        (void) GetSignalNumberOfElements(i, nelems);
        signalInfos[i].numElements = nelems;
        signalInfos[i].numCols     = nelems;

        uint32 bsz = 0u;
        (void) GetSignalByteSize(i, bsz);
        signalInfos[i].srcByteSize  = bsz;
        signalInfos[i].bufferOffset = totalSrcBytes;
        totalSrcBytes += bsz;
    }

    /* --- Pass 2: read custom per-signal fields from signalsDatabase ---
     * Note: DataSourceI::AddSignals() leaves signalsDatabase positioned at the
     * "Signals" node. We must reset to root before navigating.              */
    if (ok) {
        (void) signalsDatabase.MoveToRoot();
        bool moved = signalsDatabase.MoveRelative("Signals");
        if (!moved) {
            REPORT_ERROR(ErrorManagement::FatalError,
                         "Could not navigate to Signals in signalsDatabase.");
            ok = false;
        }
    }

    for (uint32 i = 0u; i < numSigs && ok; i++) {
        bool moved = signalsDatabase.MoveRelative(signalInfos[i].name.Buffer());
        if (!moved) {
            /* Signal added by framework with no user-configured custom fields */
            continue;
        }

        /* Unit */
        StreamString unit = "";
        (void) signalsDatabase.Read("Unit", unit);
        signalInfos[i].unit = unit;

        /* Range */
        (void) signalsDatabase.Read("RangeMin", signalInfos[i].rangeMin);
        (void) signalsDatabase.Read("RangeMax", signalInfos[i].rangeMax);

        /* QuantizedType */
        StreamString quantStr = "";
        if (signalsDatabase.Read("QuantizedType", quantStr)) {
            if (quantStr == "uint8") {
                signalInfos[i].quantType = UDPStreamerQuantUint8;
            }
            else if (quantStr == "int8") {
                signalInfos[i].quantType = UDPStreamerQuantInt8;
            }
            else if (quantStr == "uint16") {
                signalInfos[i].quantType = UDPStreamerQuantUint16;
            }
            else if (quantStr == "int16") {
                signalInfos[i].quantType = UDPStreamerQuantInt16;
            }
            else if (quantStr == "none") {
                signalInfos[i].quantType = UDPStreamerQuantNone;
            }
            else {
                REPORT_ERROR(ErrorManagement::ParametersError,
                             "Signal %s: unknown QuantizedType '%s'. "
                             "Allowed: none|uint8|int8|uint16|int16.",
                             signalInfos[i].name.Buffer(), quantStr.Buffer());
                ok = false;
            }
            if (ok && (signalInfos[i].quantType != UDPStreamerQuantNone)) {
                TypeDescriptor td = signalInfos[i].type;
                bool isFloat = ((td == Float32Bit) || (td == Float64Bit));
                if (!isFloat) {
                    REPORT_ERROR(ErrorManagement::ParametersError,
                                 "Signal %s: QuantizedType only supported for "
                                 "float32/float64 signals.",
                                 signalInfos[i].name.Buffer());
                    ok = false;
                }
            }
        }

        /* TimeMode */
        if (ok) {
            StreamString timeModeStr;
            (void) signalsDatabase.Read("TimeMode", timeModeStr);
            if (timeModeStr.Size() == 0u) {
                timeModeStr = "PacketTime";
            }
            if (timeModeStr == "PacketTime") {
                signalInfos[i].timeMode = UDPStreamerTimePacket;
            }
            else if (timeModeStr == "FullArray") {
                signalInfos[i].timeMode = UDPStreamerTimeFullArray;
            }
            else if (timeModeStr == "FirstSample") {
                signalInfos[i].timeMode = UDPStreamerTimeFirstSample;
            }
            else if (timeModeStr == "LastSample") {
                signalInfos[i].timeMode = UDPStreamerTimeLastSample;
            }
            else {
                REPORT_ERROR(ErrorManagement::ParametersError,
                             "Signal %s: unknown TimeMode '%s'. "
                             "Allowed: PacketTime|FullArray|FirstSample|LastSample.",
                             signalInfos[i].name.Buffer(), timeModeStr.Buffer());
                ok = false;
            }
        }

        /* TimeSignal (required when TimeMode != PacketTime) */
        if (ok && (signalInfos[i].timeMode != UDPStreamerTimePacket)) {
            StreamString tsName = "";
            if (!signalsDatabase.Read("TimeSignal", tsName)) {
                REPORT_ERROR(ErrorManagement::ParametersError,
                             "Signal %s: TimeSignal must be specified when "
                             "TimeMode != PacketTime.",
                             signalInfos[i].name.Buffer());
                ok = false;
            }
            else {
                timeSignalNames[i] = tsName;
                /* Index resolved in pass 3 */
                signalInfos[i].timeSignalIdx = UDPS_NO_TIME_SIGNAL;
            }
        }

        /* SamplingRate */
        if (ok) {
            (void) signalsDatabase.Read("SamplingRate", signalInfos[i].samplingRate);
            bool needsRate = (signalInfos[i].timeMode == UDPStreamerTimeFirstSample ||
                              signalInfos[i].timeMode == UDPStreamerTimeLastSample);
            if (needsRate && (signalInfos[i].samplingRate <= 0.0)) {
                REPORT_ERROR(ErrorManagement::ParametersError,
                             "Signal %s: SamplingRate > 0 is required for "
                             "FirstSample/LastSample TimeMode.",
                             signalInfos[i].name.Buffer());
                ok = false;
            }
        }

        (void) signalsDatabase.MoveToAncestor(1u);
    }

    if (ok || true) { /* always attempt to restore navigation */
        (void) signalsDatabase.MoveToAncestor(1u);
    }

    /* --- Pass 3: resolve TimeSignal names to signal indices --- */
    for (uint32 i = 0u; i < numSigs && ok; i++) {
        if (signalInfos[i].timeMode == UDPStreamerTimePacket) {
            continue; /* no time signal needed */
        }
        bool found = false;
        for (uint32 j = 0u; j < numSigs; j++) {
            if (signalInfos[j].name == timeSignalNames[i]) {
                signalInfos[i].timeSignalIdx = j;
                found = true;
                break;
            }
        }
        if (!found) {
            REPORT_ERROR(ErrorManagement::ParametersError,
                         "Signal %s: TimeSignal '%s' not found among declared signals.",
                         signalInfos[i].name.Buffer(),
                         timeSignalNames[i].Buffer());
            ok = false;
        }
    }

    delete[] timeSignalNames;
    timeSignalNames = NULL_PTR(StreamString *);

    /* --- Pass 4: validate time-signal dimensions and compute wire sizes --- */
    for (uint32 i = 0u; i < numSigs && ok; i++) {
        /* Initialise accumulated flag: false until pass 5 may flip it */
        signalInfos[i].accumulated = false;

        /* Compute wire byte size per element */
        uint32 elemWireBytes = 0u;
        switch (signalInfos[i].quantType) {
            case UDPStreamerQuantUint8:
            case UDPStreamerQuantInt8:
                elemWireBytes = 1u;
                break;
            case UDPStreamerQuantUint16:
            case UDPStreamerQuantInt16:
                elemWireBytes = 2u;
                break;
            default:
                /* Raw copy: element size = total / numElements */
                if (signalInfos[i].numElements > 0u) {
                    elemWireBytes = signalInfos[i].srcByteSize / signalInfos[i].numElements;
                }
                break;
        }
        signalInfos[i].wireByteSize = elemWireBytes * signalInfos[i].numElements;
        totalWireBytes += signalInfos[i].wireByteSize;

        /* Validate time signal dimensions */
        uint32 tsIdx = signalInfos[i].timeSignalIdx;
        if (tsIdx != UDPS_NO_TIME_SIGNAL) {
            uint32 tsElems = signalInfos[tsIdx].numElements;
            if (signalInfos[i].timeMode == UDPStreamerTimeFullArray) {
                if (tsElems != signalInfos[i].numElements) {
                    REPORT_ERROR(ErrorManagement::ParametersError,
                                 "Signal %s: FullArray TimeMode requires TimeSignal "
                                 "%s to have the same NumberOfElements (%u vs %u).",
                                 signalInfos[i].name.Buffer(),
                                 signalInfos[tsIdx].name.Buffer(),
                                 tsElems, signalInfos[i].numElements);
                    ok = false;
                }
            }
            else if ((signalInfos[i].timeMode == UDPStreamerTimeFirstSample) ||
                     (signalInfos[i].timeMode == UDPStreamerTimeLastSample)) {
                if (tsElems != 1u) {
                    REPORT_ERROR(ErrorManagement::ParametersError,
                                 "Signal %s: FirstSample/LastSample TimeMode requires "
                                 "a scalar TimeSignal (found %u elements).",
                                 signalInfos[i].name.Buffer(), tsElems);
                    ok = false;
                }
            }
        }
    }

    /* --- Pass 5: Accumulate mode setup ---
     *
     * Scalars (numElements == 1) are tagged accumulated = true and auto-assigned
     * a FullArray time reference if a primary time signal exists. numCols / numRows
     * are left at 1 — the actual per-packet element count is determined at runtime
     * and transmitted as a 4-byte numSamples field in the DATA payload header.
     *
     * Compute singleCycleWireBytes (accumulated signals) and fixedWireBytes
     * (non-accumulated arrays that travel once per packet from the most-recent slot).
     * Override totalWireBytes to the maximum possible DATA payload for wireBuffer
     * allocation: 12 + maxBatchCount × singleCycleWireBytes + fixedWireBytes.
     */
    if (ok && (publishMode == UDPStreamerPublishAccumulate)) {

        /* Find primary time signal: prefer Unit="us"/"ns", fall back to first integer scalar */
        uint32 primaryTsIdx = UDPS_NO_TIME_SIGNAL;
        for (uint32 i = 0u; i < numSigs && (primaryTsIdx == UDPS_NO_TIME_SIGNAL); i++) {
            if (signalInfos[i].numElements == 1u) {
                if ((signalInfos[i].unit == "us") || (signalInfos[i].unit == "ns")) {
                    primaryTsIdx = i;
                }
            }
        }
        if (primaryTsIdx == UDPS_NO_TIME_SIGNAL) {
            for (uint32 i = 0u; i < numSigs && (primaryTsIdx == UDPS_NO_TIME_SIGNAL); i++) {
                if (signalInfos[i].numElements == 1u) {
                    TypeDescriptor td = signalInfos[i].type;
                    if ((td == UnsignedInteger32Bit) || (td == UnsignedInteger64Bit) ||
                        (td == SignedInteger32Bit)   || (td == SignedInteger64Bit)) {
                        primaryTsIdx = i;
                    }
                }
            }
        }
        if (primaryTsIdx != UDPS_NO_TIME_SIGNAL) {
            REPORT_ERROR(ErrorManagement::Information,
                         "Accumulate: primary time signal '%s' (idx=%u).",
                         signalInfos[primaryTsIdx].name.Buffer(), primaryTsIdx);
        }

        /* Partition signals into accumulated (scalars) and fixed (arrays).
         * Auto-assign FullArray time mode for scalars that had PacketTime. */
        singleCycleWireBytes = 0u;
        fixedWireBytes       = 0u;
        for (uint32 i = 0u; i < numSigs; i++) {
            if (signalInfos[i].numElements == 1u) {
                signalInfos[i].accumulated = true;
                singleCycleWireBytes += signalInfos[i].wireByteSize; /* = srcByteSize for 1 elem */
                /* Auto-assign time reference for non-primary, non-time scalars */
                if ((i != primaryTsIdx) && (primaryTsIdx != UDPS_NO_TIME_SIGNAL) &&
                    (signalInfos[i].timeMode == UDPStreamerTimePacket)) {
                    signalInfos[i].timeMode      = UDPStreamerTimeFullArray;
                    signalInfos[i].timeSignalIdx = primaryTsIdx;
                }
            }
            else {
                /* Non-scalar: not accumulated; wire size already computed in pass 4 */
                fixedWireBytes += signalInfos[i].wireByteSize;
            }
        }

        if (singleCycleWireBytes == 0u) {
            REPORT_ERROR(ErrorManagement::ParametersError,
                         "Accumulate mode: no scalar signals found to accumulate.");
            ok = false;
        }

        if (ok) {
            /* DATA payload: [8 HRT][4 numSamples][numSamples × singleCycle][fixed] */
            static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u; /* 12 bytes */
            if ((ACCUM_HEADER + singleCycleWireBytes + fixedWireBytes) > maxPayloadSize) {
                REPORT_ERROR(ErrorManagement::ParametersError,
                             "Accumulate mode: even a single sample (%u B) exceeds "
                             "MaxPayloadSize (%u B).",
                             ACCUM_HEADER + singleCycleWireBytes + fixedWireBytes,
                             maxPayloadSize);
                ok = false;
            }
        }

        if (ok) {
            static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u;
            maxBatchCount = (maxPayloadSize - ACCUM_HEADER - fixedWireBytes) / singleCycleWireBytes;
            /* Override totalWireBytes: size of the largest possible DATA payload */
            totalWireBytes = ACCUM_HEADER + maxBatchCount * singleCycleWireBytes + fixedWireBytes;
            REPORT_ERROR(ErrorManagement::Information,
                         "Accumulate mode: singleCycleWireBytes=%u, fixedWireBytes=%u, "
                         "maxBatchCount=%u, maxPayloadSize=%u, totalWireBytes=%u.",
                         singleCycleWireBytes, fixedWireBytes,
                         maxBatchCount, maxPayloadSize, totalWireBytes);
        }
    }

    return ok;
}

bool UDPStreamer::AllocateMemory() {
    bool ok = MemoryDataSourceI::AllocateMemory();
    if (!ok) {
        return false;
    }

    /* stateMemorySize is populated by MemoryDataSourceI::AllocateMemory() */
    if (totalSrcBytes == 0u) {
        totalSrcBytes = stateMemorySize;
    }

    HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();

    /* In Accumulate mode, readyBuffer / scratchBuffer hold maxBatchCount consecutive
     * snapshots instead of a single one. */
    uint32 readyBufSize = (maxBatchCount > 0u) ? (maxBatchCount * totalSrcBytes) : totalSrcBytes;

    /* readyBuffer: copy of signal memory shared with background thread */
    readyBuffer = reinterpret_cast<uint8 *>(heap->Malloc(readyBufSize));
    if (readyBuffer == NULL_PTR(uint8 *)) {
        REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate readyBuffer.");
        return false;
    }
    (void) MemoryOperationsHelper::Set(readyBuffer, 0, readyBufSize);

    /* scratchBuffer: background-thread-private copy for serialization */
    scratchBuffer = reinterpret_cast<uint8 *>(heap->Malloc(readyBufSize));
    if (scratchBuffer == NULL_PTR(uint8 *)) {
        REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate scratchBuffer.");
        return false;
    }
    (void) MemoryOperationsHelper::Set(scratchBuffer, 0, readyBufSize);

    /* wireBuffer: serialized/quantized payload for transmission */
    wireBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalWireBytes));
    if (wireBuffer == NULL_PTR(uint8 *)) {
        REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate wireBuffer.");
        return false;
    }
    (void) MemoryOperationsHelper::Set(wireBuffer, 0, totalWireBytes);

    /* Update buffer offsets to match actual MemoryDataSourceI layout */
    for (uint32 i = 0u; i < numSigs; i++) {
        void *addr = NULL_PTR(void *);
        if (GetSignalMemoryBuffer(i, 0u, addr)) {
            signalInfos[i].bufferOffset =
                static_cast<uint32>(reinterpret_cast<uint8 *>(addr) - memory);
        }
    }

    /* --- Accumulate-mode extra buffers --- */
    if (maxBatchCount > 0u) {
        /* Linear fill buffer: RT thread writes one snapshot per slot (0..maxBatchCount-1) */
        uint32 accumBufSize = maxBatchCount * totalSrcBytes;
        accumBuffer = reinterpret_cast<uint8 *>(heap->Malloc(accumBufSize));
        if (accumBuffer == NULL_PTR(uint8 *)) {
            REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate accumBuffer.");
            return false;
        }
        (void) MemoryOperationsHelper::Set(accumBuffer, 0, accumBufSize);

        /* Per-slot HRT timestamp arrays */
        accumTimestamps   = new uint64[maxBatchCount];
        readyTimestamps   = new uint64[maxBatchCount];
        scratchTimestamps = new uint64[maxBatchCount];
        if ((accumTimestamps   == NULL_PTR(uint64 *)) ||
            (readyTimestamps   == NULL_PTR(uint64 *)) ||
            (scratchTimestamps == NULL_PTR(uint64 *))) {
            REPORT_ERROR(ErrorManagement::FatalError,
                         "Could not allocate timestamp arrays.");
            return false;
        }
        uint32 tsBytes = maxBatchCount * static_cast<uint32>(sizeof(uint64));
        (void) MemoryOperationsHelper::Set(
            reinterpret_cast<uint8 *>(accumTimestamps),   0, tsBytes);
        (void) MemoryOperationsHelper::Set(
            reinterpret_cast<uint8 *>(readyTimestamps),   0, tsBytes);
        (void) MemoryOperationsHelper::Set(
            reinterpret_cast<uint8 *>(scratchTimestamps), 0, tsBytes);

        accumFill = 0u;
        readyFill = 0u;

        REPORT_ERROR(ErrorManagement::Information,
                     "Accumulate buffers: maxBatchCount=%u, accumBufSize=%u B, readyBufSize=%u B.",
                     maxBatchCount, accumBufSize, readyBufSize);
    }

    return true;
}

const char8 *UDPStreamer::GetBrokerName(StructuredDataI &data,
                                        const SignalDirection direction) {
    const char8 *brokerName = "";
    if (direction == OutputSignals) {
        brokerName = "MemoryMapSynchronisedOutputBroker";
    }
    return brokerName;
}

bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
                                   const char8 *const nextStateName) {
    bool ok = true;

    ok = server.Start();

    /* Build the CONFIG payload and cache it in the server so any CONNECT client
     * receives it immediately.  The config is static for the lifetime of this state. */
    if (ok) {
        uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u + 1u;
        HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
        uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
        if (cfgBuf != NULL_PTR(uint8 *)) {
            uint32 cfgPayloadSize = 0u;
            if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) {
                (void) server.SendConfig(cfgBuf, cfgPayloadSize);
            }
            else {
                REPORT_ERROR(ErrorManagement::Warning,
                             "Could not build initial CONFIG payload.");
            }
            heap->Free(reinterpret_cast<void *&>(cfgBuf));
        }
        else {
            REPORT_ERROR(ErrorManagement::Warning,
                         "Could not allocate CONFIG buffer.");
        }
    }

    /* Initialise the flush timestamp so the first Accumulate flush is deferred
     * until MinRefreshRate elapses (not immediately on the first Synchronise). */
    if (ok && (publishMode == UDPStreamerPublishAccumulate)) {
        lastPublishTs = HighResolutionTimer::Counter();
        accumFill     = 0u;
        readyFill     = 0u;
    }

    /* Start the background thread (idempotent; shared by both modes) */
    if (ok && (executor.GetStatus() == EmbeddedThreadI::OffState)) {
        executor.SetName(GetName());
        executor.SetCPUMask(ProcessorType(cpuMask));
        executor.SetStackSize(stackSize);
        ErrorManagement::ErrorType startErr = executor.Start();
        ok = (startErr == ErrorManagement::NoError);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::FatalError,
                         "Could not start background thread.");
        }
    }

    return ok;
}

bool UDPStreamer::Synchronise() {
    /* Capture HRT timestamp as early as possible. */
    uint64 ts = HighResolutionTimer::Counter();

    if (publishMode == UDPStreamerPublishAccumulate) {
        /* --- Accumulate path ---
         *
         * Append this snapshot to the linear accumulation buffer, then check
         * the two flush conditions (from the user spec):
         *
         *   (a) size: accumulate_size + next_sample_size >= MaxPayloadSize
         *             (adding one more would overflow the UDP datagram)
         *   (b) time: expected_next_cycle_time - lastPublishTs >= flushPeriodTicks
         *             approximated as: ts - lastPublishTs >= flushPeriodTicks
         *
         * When either fires, the completed batch is promoted to readyBuffer /
         * readyTimestamps and dataSem is posted.  The background thread sends
         * the ready batch without any additional timer check. */
        bufMutex.FastLock(TTInfiniteWait);
        uint8 *slot = accumBuffer + (accumFill * totalSrcBytes);
        (void) MemoryOperationsHelper::Copy(slot, memory, totalSrcBytes);
        accumTimestamps[accumFill] = ts;
        accumFill++;
        uint32 filled = accumFill;
        bufMutex.FastUnLock();

        /* Check flush conditions (volatile read of lastPublishTs is safe on x86). */
        static const uint32 ACCUM_HEADER = UDPS_TIMESTAMP_BYTES + 4u; /* 12 bytes */
        uint32 curPayload  = ACCUM_HEADER + filled * singleCycleWireBytes + fixedWireBytes;
        uint32 nextPayload = curPayload + singleCycleWireBytes;
        bool   sizeCondition = (nextPayload >= maxPayloadSize);
        bool   timeCondition = ((ts - lastPublishTs) >= flushPeriodTicks);

        if (sizeCondition || timeCondition) {
            bufMutex.FastLock(TTInfiniteWait);
            (void) MemoryOperationsHelper::Copy(
                readyBuffer, accumBuffer, filled * totalSrcBytes);
            (void) MemoryOperationsHelper::Copy(
                reinterpret_cast<uint8 *>(readyTimestamps),
                reinterpret_cast<const uint8 *>(accumTimestamps),
                filled * static_cast<uint32>(sizeof(uint64)));
            readyFill = filled;
            accumFill = 0u;
            bufMutex.FastUnLock();

            /* Reset the time-based deadline (volatile write). */
            lastPublishTs = ts;
            (void) dataSem.Post();
        }
    }
    else if (publishMode == UDPStreamerPublishDecimate) {
        /* --- Decimate path ---
         * Post dataSem only every decimateRatio calls. */
        decimateCounter++;
        if (decimateCounter >= decimateRatio) {
            decimateCounter = 0u;
            bufMutex.FastLock(TTInfiniteWait);
            (void) MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
            syncTimestamp = ts;
            bufMutex.FastUnLock();
            (void) dataSem.Post();
        }
    }
    else {
        /* --- Strict path: post every call --- */
        bufMutex.FastLock(TTInfiniteWait);
        (void) MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
        syncTimestamp = ts;
        bufMutex.FastUnLock();
        (void) dataSem.Post();
    }

    return true;
}

ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
    ErrorManagement::ErrorType ret = ErrorManagement::NoError;

    if (info.GetStage() == ExecutionInfo::StartupStage) {
        const char8 *modeStr = "Strict";
        if (publishMode == UDPStreamerPublishAccumulate) { modeStr = "Accumulate"; }
        else if (publishMode == UDPStreamerPublishDecimate) { modeStr = "Decimate"; }
        REPORT_ERROR(ErrorManagement::Information,
                     "UDPStreamer background thread started (port %u, mode %s).",
                     static_cast<uint32>(port), modeStr);
    }

    if (info.GetStage() == ExecutionInfo::MainStage) {
        /* --- Wait for RT thread to post new data ---
         * ResetWait sleeps the background thread until the RT thread calls
         * Synchronise() and posts dataSem, or until the timeout expires.
         * Doing this FIRST means the thread spends nearly all its time here
         * instead of spinning on the non-blocking select() below.
         * Command latency is bounded by UDPS_DATA_WAIT_MS (acceptable for
         * CONNECT / DISCONNECT). */
        ErrorManagement::ErrorType waitErr =
            dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
        bool dataReady = (waitErr == ErrorManagement::NoError);

        /* --- Poll for incoming control commands (CONNECT / DISCONNECT / ACK) --- */
        server.ServiceClients();

        if (dataReady && server.HasClients()) {
            /* Synchronise() already gates posting dataSem to the correct rate
             * (size/time for Accumulate, every-Nth for Decimate, every call for
             * Strict).  Execute() just sends whatever is in the ready buffers. */
            if (publishMode == UDPStreamerPublishAccumulate) {
                /* --- Accumulate batch send --- */
                uint32 fill = 0u;
                bufMutex.FastLock(TTInfiniteWait);
                fill = readyFill;
                if (fill > 0u) {
                    (void) MemoryOperationsHelper::Copy(
                        scratchBuffer, readyBuffer, fill * totalSrcBytes);
                    (void) MemoryOperationsHelper::Copy(
                        reinterpret_cast<uint8 *>(scratchTimestamps),
                        reinterpret_cast<const uint8 *>(readyTimestamps),
                        fill * static_cast<uint32>(sizeof(uint64)));
                }
                bufMutex.FastUnLock();

                if (fill > 0u) {
                    SerializeAccumulated(scratchBuffer, scratchTimestamps, fill);
                    uint32 sendBytes = UDPS_TIMESTAMP_BYTES + 4u +
                                       fill * singleCycleWireBytes + fixedWireBytes;
                    packetCounter++;
                    if (!server.SendData(packetCounter, wireBuffer, sendBytes)) {
                        REPORT_ERROR(ErrorManagement::Warning,
                                     "Failed to send Accumulate DATA packet (counter=%u).",
                                     packetCounter);
                    }
                }
            }
            else {
                /* --- Single-snapshot send (Strict or Decimate) --- */
                uint64 ts = 0u;
                bufMutex.FastLock(TTInfiniteWait);
                (void) MemoryOperationsHelper::Copy(
                    scratchBuffer, readyBuffer, totalSrcBytes);
                ts = syncTimestamp;
                bufMutex.FastUnLock();

                QuantizeAndSerialize(scratchBuffer, ts);

                packetCounter++;
                if (!server.SendData(packetCounter, wireBuffer, totalWireBytes)) {
                    REPORT_ERROR(ErrorManagement::Warning,
                                 "Failed to send DATA packet (counter=%u).",
                                 packetCounter);
                }
            }
        }
    }

    if (info.GetStage() == ExecutionInfo::TerminationStage) {
        (void) server.Stop();
        REPORT_ERROR(ErrorManagement::Information,
                     "UDPStreamer background thread terminated.");
    }

    return ret;
}

void UDPStreamer::SerializeAccumulated(const uint8 *src,
                                        const uint64 *timestamps,
                                        uint32 numSamples) {
    /* Wire layout (Accumulate mode DATA payload):
     *   [8 bytes]         : HRT of slot 0 (oldest sample)
     *   [4 bytes]         : numSamples (uint32, little-endian)
     *   for each signal:
     *     if accumulated  : numSamples elements (one per slot)
     *     if non-accumulated (array): one copy from the most-recent slot
     */
    uint8 *dst = wireBuffer;

    /* 8-byte packet-level HRT timestamp = timestamp of the first (oldest) sample */
    (void) MemoryOperationsHelper::Copy(dst, &timestamps[0u], UDPS_TIMESTAMP_BYTES);
    dst += UDPS_TIMESTAMP_BYTES;

    /* 4-byte sample count */
    (void) MemoryOperationsHelper::Copy(dst, &numSamples, 4u);
    dst += 4u;

    for (uint32 i = 0u; i < numSigs; i++) {
        if (signalInfos[i].accumulated) {
            /* Scalar: pack one value from each slot in order */
            uint32 elemSrcBytes = signalInfos[i].srcByteSize; /* bytes for one element */

            for (uint32 k = 0u; k < numSamples; k++) {
                const uint8 *slotSrc = src + (k * totalSrcBytes) + signalInfos[i].bufferOffset;

                if (signalInfos[i].quantType == UDPStreamerQuantNone) {
                    (void) MemoryOperationsHelper::Copy(dst, slotSrc, elemSrcBytes);
                    dst += elemSrcBytes;
                }
                else {
                    float64 rawVal = 0.0;
                    if (signalInfos[i].type == Float32Bit) {
                        float32 f32 = 0.0f;
                        (void) MemoryOperationsHelper::Copy(&f32, slotSrc, 4u);
                        rawVal = static_cast<float64>(f32);
                    }
                    else {
                        (void) MemoryOperationsHelper::Copy(&rawVal, slotSrc, 8u);
                    }
                    float64 rMin   = signalInfos[i].rangeMin;
                    float64 rRange = signalInfos[i].rangeMax - rMin;
                    if (rRange == 0.0) { rRange = 1.0; }
                    float64 norm = (rawVal - rMin) / rRange;
                    if (norm < 0.0) { norm = 0.0; }
                    if (norm > 1.0) { norm = 1.0; }
                    switch (signalInfos[i].quantType) {
                        case UDPStreamerQuantUint8: {
                            uint8 q = static_cast<uint8>(norm * 255.0);
                            *dst = q; dst += 1u;
                            break;
                        }
                        case UDPStreamerQuantInt8: {
                            int8 q = static_cast<int8>((norm * 254.0) - 127.0);
                            (void) MemoryOperationsHelper::Copy(dst, &q, 1u);
                            dst += 1u;
                            break;
                        }
                        case UDPStreamerQuantUint16: {
                            uint16 q = static_cast<uint16>(norm * 65535.0);
                            (void) MemoryOperationsHelper::Copy(dst, &q, 2u);
                            dst += 2u;
                            break;
                        }
                        case UDPStreamerQuantInt16: {
                            int16 q = static_cast<int16>((norm * 65534.0) - 32767.0);
                            (void) MemoryOperationsHelper::Copy(dst, &q, 2u);
                            dst += 2u;
                            break;
                        }
                        default: {
                            (void) MemoryOperationsHelper::Copy(dst, slotSrc, elemSrcBytes);
                            dst += elemSrcBytes;
                            break;
                        }
                    }
                }
            }
        }
        else {
            /* Non-accumulated array: send from the most-recent slot */
            const uint8 *slotSrc = src + ((numSamples - 1u) * totalSrcBytes) +
                                   signalInfos[i].bufferOffset;

            if (signalInfos[i].quantType == UDPStreamerQuantNone) {
                (void) MemoryOperationsHelper::Copy(dst, slotSrc, signalInfos[i].srcByteSize);
                dst += signalInfos[i].srcByteSize;
            }
            else {
                float64 rMin   = signalInfos[i].rangeMin;
                float64 rRange = signalInfos[i].rangeMax - rMin;
                if (rRange == 0.0) { rRange = 1.0; }
                bool isSrcFloat32 = (signalInfos[i].type == Float32Bit);
                uint32 nelems     = signalInfos[i].numElements;
                const uint8 *s    = slotSrc;

                for (uint32 e = 0u; e < nelems; e++) {
                    float64 rawVal = 0.0;
                    if (isSrcFloat32) {
                        float32 f32 = 0.0f;
                        (void) MemoryOperationsHelper::Copy(&f32, s, 4u);
                        rawVal = static_cast<float64>(f32);
                        s += 4u;
                    }
                    else {
                        (void) MemoryOperationsHelper::Copy(&rawVal, s, 8u);
                        s += 8u;
                    }
                    float64 norm = (rawVal - rMin) / rRange;
                    if (norm < 0.0) { norm = 0.0; }
                    if (norm > 1.0) { norm = 1.0; }
                    switch (signalInfos[i].quantType) {
                        case UDPStreamerQuantUint8: {
                            uint8 q = static_cast<uint8>(norm * 255.0);
                            *dst = q; dst += 1u;
                            break;
                        }
                        case UDPStreamerQuantInt8: {
                            int8 q = static_cast<int8>((norm * 254.0) - 127.0);
                            (void) MemoryOperationsHelper::Copy(dst, &q, 1u);
                            dst += 1u;
                            break;
                        }
                        case UDPStreamerQuantUint16: {
                            uint16 q = static_cast<uint16>(norm * 65535.0);
                            (void) MemoryOperationsHelper::Copy(dst, &q, 2u);
                            dst += 2u;
                            break;
                        }
                        case UDPStreamerQuantInt16: {
                            int16 q = static_cast<int16>((norm * 65534.0) - 32767.0);
                            (void) MemoryOperationsHelper::Copy(dst, &q, 2u);
                            dst += 2u;
                            break;
                        }
                        default:
                            break;
                    }
                }
            }
        }
    }
}


bool UDPStreamer::BuildConfigPayload(uint8 *buf,
                                     uint32 bufSize,
                                     uint32 &payloadSize) {
    payloadSize = 0u;

    /* 4 bytes: number of signals */
    if ((payloadSize + 4u) > bufSize) {
        return false;
    }
    (void) MemoryOperationsHelper::Copy(buf + payloadSize, &numSigs, 4u);
    payloadSize += 4u;

    for (uint32 i = 0u; i < numSigs; i++) {
        if ((payloadSize + UDPS_SIGNAL_DESC_SIZE) > bufSize) {
            return false;
        }

        uint8 *p = buf + payloadSize;

        /* Name: 64 bytes, zero-padded */
        (void) MemoryOperationsHelper::Set(p, 0, UDPS_MAX_SIGNAL_NAME);
        uint32 nameLen = static_cast<uint32>(signalInfos[i].name.Size());
        if (nameLen >= UDPS_MAX_SIGNAL_NAME) {
            nameLen = UDPS_MAX_SIGNAL_NAME - 1u;
        }
        (void) MemoryOperationsHelper::Copy(p, signalInfos[i].name.Buffer(), nameLen);
        p += UDPS_MAX_SIGNAL_NAME;

        /* Type code: 1 byte */
        *p = TypeDescriptorToCode(signalInfos[i].type);
        p += 1u;

        /* Quant type: 1 byte */
        *p = static_cast<uint8>(signalInfos[i].quantType);
        p += 1u;

        /* numDimensions: 1 byte */
        *p = signalInfos[i].numDimensions;
        p += 1u;

        /* numRows: 4 bytes */
        (void) MemoryOperationsHelper::Copy(p, &signalInfos[i].numRows, 4u);
        p += 4u;

        /* numCols: 4 bytes */
        (void) MemoryOperationsHelper::Copy(p, &signalInfos[i].numCols, 4u);
        p += 4u;

        /* rangeMin: 8 bytes (float64) */
        (void) MemoryOperationsHelper::Copy(p, &signalInfos[i].rangeMin, 8u);
        p += 8u;

        /* rangeMax: 8 bytes (float64) */
        (void) MemoryOperationsHelper::Copy(p, &signalInfos[i].rangeMax, 8u);
        p += 8u;

        /* timeMode: 1 byte */
        *p = static_cast<uint8>(signalInfos[i].timeMode);
        p += 1u;

        /* samplingRate: 8 bytes (float64) */
        (void) MemoryOperationsHelper::Copy(p, &signalInfos[i].samplingRate, 8u);
        p += 8u;

        /* timeSignalIdx: 4 bytes */
        (void) MemoryOperationsHelper::Copy(p, &signalInfos[i].timeSignalIdx, 4u);
        p += 4u;

        /* Unit: 32 bytes, zero-padded */
        (void) MemoryOperationsHelper::Set(p, 0, UDPS_MAX_UNIT_LEN);
        uint32 unitLen = static_cast<uint32>(signalInfos[i].unit.Size());
        if (unitLen >= UDPS_MAX_UNIT_LEN) {
            unitLen = UDPS_MAX_UNIT_LEN - 1u;
        }
        (void) MemoryOperationsHelper::Copy(p, signalInfos[i].unit.Buffer(), unitLen);
        p += UDPS_MAX_UNIT_LEN;

        payloadSize += UDPS_SIGNAL_DESC_SIZE;
    }

    /* 1 byte: publishing mode (so clients can parse DATA payloads correctly) */
    if ((payloadSize + 1u) > bufSize) {
        return false;
    }
    buf[payloadSize] = static_cast<uint8>(publishMode);
    payloadSize += 1u;

    return true;
}

void UDPStreamer::QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp) {
    uint8 *dst = wireBuffer;

    /* 8-byte packet timestamp */
    (void) MemoryOperationsHelper::Copy(dst, &timestamp, UDPS_TIMESTAMP_BYTES);
    dst += UDPS_TIMESTAMP_BYTES;

    for (uint32 i = 0u; i < numSigs; i++) {
        const uint8 *src = srcBuf + signalInfos[i].bufferOffset;

        if (signalInfos[i].quantType == UDPStreamerQuantNone) {
            /* Raw copy */
            (void) MemoryOperationsHelper::Copy(dst, src, signalInfos[i].srcByteSize);
            dst += signalInfos[i].srcByteSize;
        }
        else {
            float64 rMin   = signalInfos[i].rangeMin;
            float64 rRange = signalInfos[i].rangeMax - rMin;
            if (rRange == 0.0) {
                rRange = 1.0; /* guard against divide-by-zero */
            }
            bool isSrcFloat32 = (signalInfos[i].type == Float32Bit);
            uint32 nelems = signalInfos[i].numElements;
            const uint8 *s = src;

            for (uint32 e = 0u; e < nelems; e++) {
                float64 rawVal = 0.0;
                if (isSrcFloat32) {
                    float32 f32 = 0.0f;
                    (void) MemoryOperationsHelper::Copy(&f32, s, 4u);
                    rawVal = static_cast<float64>(f32);
                    s += 4u;
                }
                else {
                    (void) MemoryOperationsHelper::Copy(&rawVal, s, 8u);
                    s += 8u;
                }

                /* Normalize and clamp to [0.0, 1.0] */
                float64 norm = (rawVal - rMin) / rRange;
                if (norm < 0.0) { norm = 0.0; }
                if (norm > 1.0) { norm = 1.0; }

                switch (signalInfos[i].quantType) {
                    case UDPStreamerQuantUint8: {
                        uint8 q = static_cast<uint8>(norm * 255.0);
                        *dst = q;
                        dst += 1u;
                        break;
                    }
                    case UDPStreamerQuantInt8: {
                        int8 q = static_cast<int8>((norm * 254.0) - 127.0);
                        (void) MemoryOperationsHelper::Copy(dst, &q, 1u);
                        dst += 1u;
                        break;
                    }
                    case UDPStreamerQuantUint16: {
                        uint16 q = static_cast<uint16>(norm * 65535.0);
                        (void) MemoryOperationsHelper::Copy(dst, &q, 2u);
                        dst += 2u;
                        break;
                    }
                    case UDPStreamerQuantInt16: {
                        int16 q = static_cast<int16>((norm * 65534.0) - 32767.0);
                        (void) MemoryOperationsHelper::Copy(dst, &q, 2u);
                        dst += 2u;
                        break;
                    }
                    default:
                        break;
                }
            }
        }
    }
}

uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
    uint8 code = UDPS_TYPECODE_UNKNOWN;
    if (td == UnsignedInteger8Bit)       { code = UDPS_TYPECODE_UINT8;   }
    else if (td == SignedInteger8Bit)    { code = UDPS_TYPECODE_INT8;    }
    else if (td == UnsignedInteger16Bit) { code = UDPS_TYPECODE_UINT16;  }
    else if (td == SignedInteger16Bit)   { code = UDPS_TYPECODE_INT16;   }
    else if (td == UnsignedInteger32Bit) { code = UDPS_TYPECODE_UINT32;  }
    else if (td == SignedInteger32Bit)   { code = UDPS_TYPECODE_INT32;   }
    else if (td == UnsignedInteger64Bit) { code = UDPS_TYPECODE_UINT64;  }
    else if (td == SignedInteger64Bit)   { code = UDPS_TYPECODE_INT64;   }
    else if (td == Float32Bit)           { code = UDPS_TYPECODE_FLOAT32; }
    else if (td == Float64Bit)           { code = UDPS_TYPECODE_FLOAT64; }
    return code;
}

uint16 UDPStreamer::GetPort() const {
    return port;
}

uint32 UDPStreamer::GetMaxPayloadSize() const {
    return maxPayloadSize;
}

bool UDPStreamer::IsClientConnected() const {
    return server.HasClients();
}

bool UDPStreamer::IsMulticast() const {
    return server.IsMulticast();
}

CLASS_REGISTER(UDPStreamer, "1.0")

} /* namespace MARTe */