/**
 * @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 "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 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 = static_cast<uint32>(sizeof(UDPSPacketHeader)) + 1u;

/** Server socket receive timeout in milliseconds. */
static const uint32 UDPS_RECV_TIMEOUT_MS = 5u;

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

/** Sentinel: no time-signal reference; use the packet-level timestamp. */
static const uint32 UDPS_NO_TIME_SIGNAL = 0xFFFFFFFFu;

/** Max signal name length in CONFIG packet (including null terminator). */
static const uint32 UDPS_MAX_SIGNAL_NAME = 64u;

/** Max unit string length in CONFIG packet (including null terminator). */
static const uint32 UDPS_MAX_UNIT_LEN = 32u;

/** Size in bytes of one signal descriptor in the CONFIG payload. */
static const uint32 UDPS_SIGNAL_DESC_SIZE =
    UDPS_MAX_SIGNAL_NAME  /* name         */
    + 1u                  /* typeCode     */
    + 1u                  /* quantType    */
    + 1u                  /* numDimensions*/
    + 4u                  /* numRows      */
    + 4u                  /* numCols      */
    + 8u                  /* rangeMin     */
    + 8u                  /* rangeMax     */
    + 1u                  /* timeMode     */
    + 8u                  /* samplingRate */
    + 4u                  /* timeSignalIdx*/
    + UDPS_MAX_UNIT_LEN;  /* unit         */

/** 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;
    numSigs         = 0u;
    signalInfos     = NULL_PTR(UDPStreamerSignalInfo *);
    readyBuffer     = NULL_PTR(uint8 *);
    scratchBuffer   = NULL_PTR(uint8 *);
    wireBuffer      = NULL_PTR(uint8 *);
    totalSrcBytes   = 0u;
    totalWireBytes  = 0u;
    syncTimestamp   = 0u;
    clientConnected = false;
    packetCounter   = 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.");
            }
        }
    }

    if (serverSocket.IsValid()) {
        (void) serverSocket.Close();
    }
    if (clientSocket.IsValid()) {
        (void) clientSocket.Close();
    }

    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;
        }
    }

    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++) {
        /* 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;
                }
            }
        }
    }

    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();

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

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

    /* 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);
        }
    }

    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;

    /* Open server socket (idempotent: skip if already valid) */
    if (!serverSocket.IsValid()) {
        ok = serverSocket.Open();
        if (!ok) {
            REPORT_ERROR(ErrorManagement::FatalError,
                         "Could not open server UDP socket.");
        }
        if (ok) {
            ok = serverSocket.Listen(port);
            if (!ok) {
                REPORT_ERROR(ErrorManagement::FatalError,
                             "Could not bind server socket to port %u.",
                             static_cast<uint32>(port));
            }
        }
        /* Socket stays blocking; Read() uses the timeout via select() internally. */
    }

    /* Start the background thread (idempotent) */
    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 timestamp as early as possible */
    uint64 ts = HighResolutionTimer::Counter();

    /* RT-safe copy of signal memory → readyBuffer */
    bufMutex.FastLock(TTInfiniteWait);
    (void) MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
    syncTimestamp = ts;
    bufMutex.FastUnLock();

    /* Wake the background sender thread */
    (void) dataSem.Post();

    return true;
}

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

    if (info.GetStage() == ExecutionInfo::StartupStage) {
        REPORT_ERROR(ErrorManagement::Information,
                     "UDPStreamer background thread started (port %u).",
                     static_cast<uint32>(port));
    }

    if (info.GetStage() == ExecutionInfo::MainStage) {
        /* --- Poll server socket for incoming client commands ---
         * Use select() directly so we get a silent timeout with no log spam.
         * MARTe2's Read(buf, size, timeout) calls recvfrom() and logs an error
         * on every timeout (EAGAIN from SO_RCVTIMEO). */
        uint8 cmdBuf[256u];
        Handle sockFd = serverSocket.GetReadHandle();
        fd_set rfds;
        FD_ZERO(&rfds);
        FD_SET(static_cast<int>(sockFd), &rfds);
        struct timeval tv;
        tv.tv_sec  = 0;
        tv.tv_usec = static_cast<long>(UDPS_RECV_TIMEOUT_MS) * 1000L;
        int nReady = select(static_cast<int>(sockFd) + 1, &rfds, NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv);
        if (nReady > 0) {
            uint32 recvSize = static_cast<uint32>(sizeof(cmdBuf));
            bool received = serverSocket.Read(reinterpret_cast<char8 *>(cmdBuf), recvSize);
            if (received && (recvSize >= static_cast<uint32>(sizeof(UDPSPacketHeader)))) {
                HandleClientCommand(cmdBuf, recvSize);
            }
        }

        /* --- Wait for RT thread to post new data --- */
        /* ResetWait atomically lowers the barrier then waits, preventing missed posts */
        ErrorManagement::ErrorType waitErr =
            dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
        bool dataReady = (waitErr == ErrorManagement::NoError);

        if (dataReady && clientConnected) {
            /* Copy readyBuffer → scratchBuffer under brief spinlock */
            uint64 ts = 0u;
            bufMutex.FastLock(TTInfiniteWait);
            (void) MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer, totalSrcBytes);
            ts = syncTimestamp;
            bufMutex.FastUnLock();

            /* Serialize signal data into wireBuffer */
            QuantizeAndSerialize(scratchBuffer, ts);

            /* Send (fragmented if needed) */
            packetCounter++;
            if (!SendFragmented(UDPS_TYPE_DATA, packetCounter, wireBuffer, totalWireBytes)) {
                REPORT_ERROR(ErrorManagement::Warning,
                             "Failed to send DATA packet (counter=%u).", packetCounter);
            }
        }
    }

    if (info.GetStage() == ExecutionInfo::TerminationStage) {
        if (clientConnected) {
            (void) clientSocket.Close();
            clientConnected = false;
        }
        REPORT_ERROR(ErrorManagement::Information,
                     "UDPStreamer background thread terminated.");
    }

    return ret;
}

void UDPStreamer::HandleClientCommand(const uint8 *buf, uint32 size) {
    if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
        return;
    }
    const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
    if (hdr->magic != UDPS_MAGIC) {
        return;
    }

    if (hdr->type == UDPS_TYPE_CONNECT) {
        InternetHost src = serverSocket.GetSource();

        /* Disconnect any previous client */
        if (clientConnected) {
            if (clientSocket.IsValid()) {
                (void) clientSocket.Close();
            }
            clientConnected = false;
        }

        /* Open a new client socket and connect to the requesting address */
        bool sockOk = clientSocket.Open();
        if (sockOk) {
            sockOk = clientSocket.Connect(src.GetAddress().Buffer(), src.GetPort());
        }
        if (sockOk) {
            clientConnected = true;
            REPORT_ERROR(ErrorManagement::Information,
                         "Client connected from %s:%u.",
                         src.GetAddress().Buffer(),
                         static_cast<uint32>(src.GetPort()));

            /* Send CONFIG packet */
            uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u;
            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) SendFragmented(UDPS_TYPE_CONFIG, 0u, cfgBuf, cfgPayloadSize);
                }
                else {
                    REPORT_ERROR(ErrorManagement::Warning,
                                 "Could not build CONFIG payload.");
                }
                heap->Free(reinterpret_cast<void *&>(cfgBuf));
            }
        }
        else {
            REPORT_ERROR(ErrorManagement::Warning,
                         "Could not connect to client %s:%u.",
                         src.GetAddress().Buffer(),
                         static_cast<uint32>(src.GetPort()));
        }
    }
    else if (hdr->type == UDPS_TYPE_DISCONNECT) {
        REPORT_ERROR(ErrorManagement::Information, "Client sent DISCONNECT.");
        if (clientConnected) {
            if (clientSocket.IsValid()) {
                (void) clientSocket.Close();
            }
            clientConnected = false;
        }
    }
    else if (hdr->type == UDPS_TYPE_ACK) {
        /* Optional: track acknowledged counters for loss detection */
        if (size >= static_cast<uint32>(sizeof(UDPSPacketHeader)) + 4u) {
            uint32 ackedCounter = 0u;
            const uint8 *pl = buf + sizeof(UDPSPacketHeader);
            (void) MemoryOperationsHelper::Copy(&ackedCounter, pl, 4u);
            REPORT_ERROR(ErrorManagement::Debug,
                         "ACK received for packet counter %u.", ackedCounter);
        }
    }
}

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;
    }

    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;
                }
            }
        }
    }
}

bool UDPStreamer::SendFragmented(uint8 type,
                                  uint32 counter,
                                  const uint8 *payload,
                                  uint32 payloadSize) {
    uint32 headerSize  = static_cast<uint32>(sizeof(UDPSPacketHeader));
    uint32 maxChunk    = maxPayloadSize - headerSize;
    uint32 totalFrags  = (payloadSize == 0u) ? 1u :
                         ((payloadSize + maxChunk - 1u) / maxChunk);

    uint32 sendBufSize = headerSize + maxChunk;
    HeapI *heap        = GlobalObjectsDatabase::Instance()->GetStandardHeap();
    uint8 *sendBuf     = reinterpret_cast<uint8 *>(heap->Malloc(sendBufSize));
    if (sendBuf == NULL_PTR(uint8 *)) {
        REPORT_ERROR(ErrorManagement::FatalError,
                     "Could not allocate send buffer (%u bytes).", sendBufSize);
        return false;
    }

    bool ok     = true;
    uint32 offs = 0u;

    for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
        uint32 chunkSize = payloadSize - offs;
        if (chunkSize > maxChunk) {
            chunkSize = maxChunk;
        }

        UDPSPacketHeader *hdr   = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
        hdr->magic              = UDPS_MAGIC;
        hdr->type               = type;
        hdr->counter            = counter;
        hdr->fragmentIdx        = static_cast<uint16>(f);
        hdr->totalFragments     = static_cast<uint16>(totalFrags);
        hdr->payloadBytes       = chunkSize;

        if (chunkSize > 0u) {
            (void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
                                                payload + offs,
                                                chunkSize);
        }

        uint32 sendSize = headerSize + chunkSize;
        ok = clientSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::Warning,
                         "Fragment %u/%u send failed.", f + 1u, totalFrags);
        }
        offs += chunkSize;
    }

    heap->Free(reinterpret_cast<void *&>(sendBuf));
    return ok;
}

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 clientConnected;
}

CLASS_REGISTER(UDPStreamer, "1.0")

} /* namespace MARTe */