/**
 * @file UDPSClient.cpp
 * @brief Implementation of UDPSClient — auto-reconnecting UDPS receiver.
 */

#include "UDPSClient.h"

#include "AdvancedErrorManagement.h"
#include "MemoryOperationsHelper.h"

#include <sys/select.h>
#include <sys/socket.h>
#include <errno.h>

namespace MARTe {

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

UDPSClient::UDPSClient()
    : serverPort(0u),
      dataPort(0u),
      useMulticast(false),
      silenceTimeoutTicks(0u),
      reconnectDelayTicks(0u),
      maxPayloadSize(UDPS_CLIENT_DEFAULT_MAX_PAYLOAD),
      cpuMask(0xFFFFFFFFu),
      stackSize(65536u),
      recvBufferSize(UDPS_CLIENT_DEFAULT_RECV_BUFFER),
      listener(NULL_PTR(UDPSClientListener *)),
      threadService(*this),
      connected(false),
      lastDataTicks(0u),
      disconnectTick(0u),
      localPort(0u),
      lastGcTicks(0u) {

    for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
        reassemblySlots[i].counter           = 0u;
        reassemblySlots[i].type              = 0u;
        reassemblySlots[i].totalFragments    = 0u;
        reassemblySlots[i].receivedFragments = 0u;
        reassemblySlots[i].active            = false;
        reassemblySlots[i].firstSeenTicks    = 0u;
        reassemblySlots[i].chunkSize         = 0u;
        (void) MemoryOperationsHelper::Set(reassemblySlots[i].recvMask, 0, 32u);
    }
}

UDPSClient::~UDPSClient() {
    (void) Stop();
}

// ---------------------------------------------------------------------------
// Initialise
// ---------------------------------------------------------------------------

bool UDPSClient::Initialise(StructuredDataI &data) {
    StreamString saddr;
    if (!data.Read("ServerAddr", saddr) || (saddr.Size() == 0u)) {
        REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
                            "UDPSClient: ServerAddr not specified.");
        return false;
    }
    serverAddr = saddr;

    uint32 portU32 = 0u;
    if (!data.Read("Port", portU32)) {
        REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
                            "UDPSClient: Port not specified.");
        return false;
    }
    serverPort = static_cast<uint16>(portU32);

    StreamString mcGroup;
    if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) {
        multicastGroup = mcGroup;
        useMulticast   = true;
    }

    if (useMulticast) {
        uint32 dpU32 = static_cast<uint32>(serverPort) + 1u;
        (void) data.Read("DataPort", dpU32);
        dataPort = static_cast<uint16>(dpU32);
    }

    uint32 silenceS = UDPS_CLIENT_DEFAULT_SILENCE_TIMEOUT_S;
    (void) data.Read("SilenceTimeout", silenceS);
    silenceTimeoutTicks = static_cast<uint64>(silenceS) * HighResolutionTimer::Frequency();

    uint32 reconnectS = UDPS_CLIENT_DEFAULT_RECONNECT_DELAY_S;
    (void) data.Read("ReconnectDelay", reconnectS);
    reconnectDelayTicks = static_cast<uint64>(reconnectS) * HighResolutionTimer::Frequency();

    uint32 mps = UDPS_CLIENT_DEFAULT_MAX_PAYLOAD;
    (void) data.Read("MaxPayloadSize", mps);
    maxPayloadSize = mps;

    (void) data.Read("CPUMask", cpuMask);
    (void) data.Read("StackSize", stackSize);

    recvBufferSize = UDPS_CLIENT_DEFAULT_RECV_BUFFER;
    (void) data.Read("RecvBufferSize", recvBufferSize);

    return true;
}

// ---------------------------------------------------------------------------
// SetListener / Start / Stop
// ---------------------------------------------------------------------------

void UDPSClient::SetListener(UDPSClientListener *l) {
    listener = l;
}

bool UDPSClient::Start() {
    threadService.SetCPUMask(cpuMask);
    threadService.SetStackSize(stackSize);
    ErrorManagement::ErrorType err = threadService.Start();
    return (err == ErrorManagement::NoError);
}

bool UDPSClient::Stop() {
    ErrorManagement::ErrorType err = threadService.Stop();
    Disconnect();
    return (err == ErrorManagement::NoError);
}

// ---------------------------------------------------------------------------
// Execute (thread entry)
// ---------------------------------------------------------------------------

ErrorManagement::ErrorType UDPSClient::Execute(ExecutionInfo &info) {
    if (info.GetStage() == ExecutionInfo::StartupStage) {
        REPORT_ERROR_STATIC(ErrorManagement::Information,
                            "UDPSClient: Receive thread started.");
        return ErrorManagement::NoError;
    }

    if (info.GetStage() == ExecutionInfo::TerminationStage) {
        REPORT_ERROR_STATIC(ErrorManagement::Information,
                            "UDPSClient: Receive thread stopping.");
        Disconnect();
        return ErrorManagement::NoError;
    }

    // MainStage
    uint64 now = HighResolutionTimer::Counter();

    if (!connected) {
        // Wait reconnectDelay before retrying
        if ((disconnectTick == 0u) ||
            ((now - disconnectTick) >= reconnectDelayTicks)) {
            if (!Connect()) {
                disconnectTick = HighResolutionTimer::Counter();
            }
        }
        return ErrorManagement::NoError;
    }

    // Connected: receive + process
    bool ok = ReceiveAndProcess();
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Receive error; disconnecting.");
        Disconnect();
        return ErrorManagement::NoError;
    }

    /* Re-sample time AFTER ReceiveAndProcess() so that lastDataTicks (which may
     * have been updated inside ReceiveAndProcess) is never newer than `now`.
     * Without this, if a packet arrives during ReceiveAndProcess(), `now` (captured
     * before the call) < lastDataTicks, causing unsigned wraparound in the
     * subtraction and a spurious silence timeout. */
    now = HighResolutionTimer::Counter();

    // Check silence timeout
    if (silenceTimeoutTicks > 0u) {
        uint64 lastSeen = lastDataTicks;
        if ((lastSeen > 0u) && ((now - lastSeen) >= silenceTimeoutTicks)) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSClient: Server silent; disconnecting.");
            Disconnect();
        }
    }

    // Periodic GC of stale reassembly slots (~every 1 s)
    uint64 gcFreq = HighResolutionTimer::Frequency();
    if ((now - lastGcTicks) >= gcFreq) {
        GcReassemblySlots();
        lastGcTicks = now;
    }

    return ErrorManagement::NoError;
}

// ---------------------------------------------------------------------------
// Private: Connect
// ---------------------------------------------------------------------------

bool UDPSClient::Connect() {
    bool ok = useMulticast ? ConnectMulticast() : ConnectUnicast();
    if (ok) {
        connected     = true;
        lastDataTicks = HighResolutionTimer::Counter();
        if (listener != NULL_PTR(UDPSClientListener *)) {
            listener->OnUDPSConnected();
        }
        REPORT_ERROR_STATIC(ErrorManagement::Information,
                            "UDPSClient: Connected to %s:%u.",
                            serverAddr.Buffer(), static_cast<uint32>(serverPort));
    }
    return ok;
}

void UDPSClient::SetRecvBufferSize(BasicUDPSocket &sock) {
    /* BasicUDPSocket exposes no SO_RCVBUF API; the OS default (Linux
     * rmem_default, typically ~208 KiB) is easily overrun by high-throughput
     * sources, causing silent kernel-level datagram drops. Work around this
     * by calling setsockopt() directly on the raw handle. Best-effort: a
     * failure here just leaves the OS default in place. */
    Handle fd = sock.GetReadHandle();
    if (fd >= 0) {
        int32 sz = static_cast<int32>(recvBufferSize);
        if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &sz, sizeof(sz)) != 0) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSClient: Could not set SO_RCVBUF to %u bytes.",
                                recvBufferSize);
        }
    }
}

bool UDPSClient::ConnectUnicast() {
    // Open a local UDP socket bound to an ephemeral port
    if (!recvSocket.Open()) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not open receive socket.");
        return false;
    }
    SetRecvBufferSize(recvSocket);

    if (!recvSocket.Listen(0u)) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not bind receive socket.");
        (void) recvSocket.Close();
        return false;
    }

    // Build and send CONNECT packet to server
    uint8  connectPkt[UDPS_HEADER_SIZE];
    UDPSBuildHeader(connectPkt, UDPS_TYPE_CONNECT, 0u, 0u, 1u, 0u);

    InternetHost serverDest(serverPort, serverAddr.Buffer());
    (void) recvSocket.SetDestination(serverDest);
    uint32 sendSize = UDPS_HEADER_SIZE;
    bool   ok       = recvSocket.Write(reinterpret_cast<const char8 *>(connectPkt), sendSize);
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not send CONNECT to %s:%u.",
                            serverAddr.Buffer(), static_cast<uint32>(serverPort));
        (void) recvSocket.Close();
        return false;
    }

    return true;
}

bool UDPSClient::ConnectMulticast() {
    /* Join the multicast group BEFORE sending CONNECT over TCP.
     * The UDPStreamer broadcasts CONFIG via multicast immediately when it
     * receives a CONNECT packet.  If the multicast socket is not yet joined,
     * that CONFIG packet is dropped by the kernel and the session never becomes
     * configured.  Correct order: join → CONNECT → receive CONFIG. */

    // Open and bind the multicast receive socket first
    if (!mcastSocket.Open()) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not open multicast socket.");
        return false;
    }
    SetRecvBufferSize(mcastSocket);

    bool ok = mcastSocket.Listen(dataPort);
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not bind multicast socket on port %u.",
                            static_cast<uint32>(dataPort));
        (void) mcastSocket.Close();
        return false;
    }

    ok = mcastSocket.Join(multicastGroup.Buffer());
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not join multicast group %s.",
                            multicastGroup.Buffer());
        (void) mcastSocket.Close();
        return false;
    }
    REPORT_ERROR_STATIC(ErrorManagement::Information,
                        "UDPSClient: Joined multicast group %s on port %u.",
                        multicastGroup.Buffer(), static_cast<uint32>(dataPort));

    // Now open the TCP control connection and announce ourselves
    if (!tcpSocket.Open()) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not open TCP socket.");
        (void) mcastSocket.Close();
        return false;
    }

    ok = tcpSocket.Connect(serverAddr.Buffer(), serverPort);
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: TCP connect to %s:%u failed.",
                            serverAddr.Buffer(), static_cast<uint32>(serverPort));
        (void) tcpSocket.Close();
        (void) mcastSocket.Close();
        return false;
    }

    // Send CONNECT — UDPStreamer will now multicast CONFIG, which we are ready to receive
    uint8  connectPkt[UDPS_HEADER_SIZE];
    UDPSBuildHeader(connectPkt, UDPS_TYPE_CONNECT, 0u, 0u, 1u, 0u);
    uint32 sendSize = UDPS_HEADER_SIZE;
    ok = tcpSocket.Write(reinterpret_cast<const char8 *>(connectPkt), sendSize);
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not send CONNECT over TCP.");
        (void) tcpSocket.Close();
        (void) mcastSocket.Close();
        return false;
    }

    return true;
}

// ---------------------------------------------------------------------------
// Private: Disconnect
// ---------------------------------------------------------------------------

void UDPSClient::Disconnect() {
    if (!connected) {
        return;
    }

    // Send DISCONNECT
    if (useMulticast) {
        if (tcpSocket.IsValid()) {
            uint8  disconnPkt[UDPS_HEADER_SIZE];
            UDPSBuildHeader(disconnPkt, UDPS_TYPE_DISCONNECT, 0u, 0u, 1u, 0u);
            uint32 sendSize = UDPS_HEADER_SIZE;
            (void) tcpSocket.Write(reinterpret_cast<const char8 *>(disconnPkt), sendSize);
            (void) tcpSocket.Close();
        }
        if (mcastSocket.IsValid()) {
            (void) mcastSocket.Close();
        }
    }
    else {
        if (recvSocket.IsValid()) {
            uint8  disconnPkt[UDPS_HEADER_SIZE];
            UDPSBuildHeader(disconnPkt, UDPS_TYPE_DISCONNECT, 0u, 0u, 1u, 0u);
            InternetHost serverDest(serverPort, serverAddr.Buffer());
            (void) recvSocket.SetDestination(serverDest);
            uint32 sendSize = UDPS_HEADER_SIZE;
            (void) recvSocket.Write(reinterpret_cast<const char8 *>(disconnPkt), sendSize);
            (void) recvSocket.Close();
        }
    }

    connected      = false;
    disconnectTick = HighResolutionTimer::Counter();

    if (listener != NULL_PTR(UDPSClientListener *)) {
        listener->OnUDPSDisconnected();
    }
}

// ---------------------------------------------------------------------------
// Private: ReceiveAndProcess
// ---------------------------------------------------------------------------

bool UDPSClient::ReceiveAndProcess() {
    // Select the receive socket(s)
    int fd = -1;
    int tcpFd = -1;
    if (useMulticast) {
        fd = mcastSocket.GetReadHandle();
        /* In multicast mode the server delivers CONFIG over the TCP control
         * connection — it must be polled too, otherwise the session never
         * becomes configured. */
        tcpFd = tcpSocket.GetReadHandle();
    }
    else {
        fd = recvSocket.GetReadHandle();
    }
    if (fd < 0) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: ReceiveAndProcess: socket fd < 0 (socket invalid).");
        return false;
    }

    /* HI-6: guard against FD_SETSIZE overflow */
    if (fd < 0 || fd >= FD_SETSIZE ||
        (tcpFd >= 0 && tcpFd >= FD_SETSIZE)) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: fd >= FD_SETSIZE (%d/%d) — skipping select.",
                            fd, tcpFd);
        return false;
    }

    fd_set rset;
    FD_ZERO(&rset);
    FD_SET(fd, &rset);
    int maxFd = fd;
    if (tcpFd >= 0) {
        FD_SET(tcpFd, &rset);
        if (tcpFd > maxFd) {
            maxFd = tcpFd;
        }
    }
    // 10 ms timeout so Execute() doesn't busy-spin
    struct timeval tv;
    tv.tv_sec = 0; tv.tv_usec = 10000;
    int nready = select(maxFd + 1, &rset, NULL, NULL, &tv);
    if (nready < 0) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: ReceiveAndProcess: select() returned %d (errno=%d).",
                            nready, errno);
        return false; // socket error
    }
    if (nready == 0) {
        return true;  // timeout, no data
    }

    // TCP control frame (multicast CONFIG path)
    if ((tcpFd >= 0) && FD_ISSET(tcpFd, &rset)) {
        if (!ReceiveTCPFrame()) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSClient: TCP control connection lost.");
            return false;
        }
        lastDataTicks = HighResolutionTimer::Counter();
    }

    if (!FD_ISSET(fd, &rset)) {
        return true;  // only the TCP socket was readable
    }

    uint32 recvSize = static_cast<uint32>(sizeof(recvBuf));
    bool   ok;
    if (useMulticast) {
        ok = mcastSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
    }
    else {
        ok = recvSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
    }

    if (!ok || (recvSize < UDPS_HEADER_SIZE)) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: ReceiveAndProcess: Read() failed or short packet "
                            "(ok=%s, recvSize=%u, HEADER_SIZE=%u).",
                            ok ? "true" : "false", recvSize, UDPS_HEADER_SIZE);
        return false;
    }

    lastDataTicks = HighResolutionTimer::Counter();
    ProcessDatagram(recvBuf, recvSize);
    return true;
}

// ---------------------------------------------------------------------------
// Private: ReceiveTCPFrame / ReadExactTCP
// ---------------------------------------------------------------------------

bool UDPSClient::ReceiveTCPFrame() {
    /* TCP is a byte stream: read exactly one UDPS frame (17-byte header
     * followed by payloadBytes of payload) and hand it to ProcessDatagram.
     * Fragmented CONFIGs arrive as consecutive frames and go through the
     * normal reassembly path. */
    if (!ReadExactTCP(recvBuf, UDPS_HEADER_SIZE)) {
        return false;
    }
    const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(recvBuf);
    if (hdr->magic != UDPS_MAGIC) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: TCP stream desynchronised (bad magic).");
        return false;
    }
    uint32 payloadBytes = hdr->payloadBytes;
    if (payloadBytes > (static_cast<uint32>(sizeof(recvBuf)) - UDPS_HEADER_SIZE)) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: TCP frame payload too large (%u).", payloadBytes);
        return false;
    }
    if (payloadBytes > 0u) {
        if (!ReadExactTCP(&recvBuf[UDPS_HEADER_SIZE], payloadBytes)) {
            return false;
        }
    }
    ProcessDatagram(recvBuf, UDPS_HEADER_SIZE + payloadBytes);
    return true;
}

bool UDPSClient::ReadExactTCP(uint8 *dst, uint32 n) {
    uint32 got = 0u;
    while (got < n) {
        uint32 chunk = n - got;
        if (!tcpSocket.Read(reinterpret_cast<char8 *>(&dst[got]), chunk)) {
            return false;
        }
        if (chunk == 0u) {
            return false;  // orderly close
        }
        got += chunk;
    }
    return true;
}

// ---------------------------------------------------------------------------
// Private: ProcessDatagram
// ---------------------------------------------------------------------------

void UDPSClient::ProcessDatagram(const uint8 *buf, uint32 size) {
    if (size < UDPS_HEADER_SIZE) {
        return;
    }
    const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
    if (hdr->magic != UDPS_MAGIC) {
        return;
    }
    if ((hdr->type != UDPS_TYPE_DATA) && (hdr->type != UDPS_TYPE_CONFIG)) {
        return;
    }

    uint32 payloadBytes = hdr->payloadBytes;
    if ((payloadBytes + UDPS_HEADER_SIZE) > size) {
        return; // truncated
    }

    if (hdr->totalFragments == 1u) {
        if ((hdr->type == UDPS_TYPE_DATA) && (listener != NULL_PTR(UDPSClientListener *))) {
            listener->OnUDPSFragment(hdr->counter, size, true);
        }
        // Single-fragment shortcut: deliver immediately
        if (listener != NULL_PTR(UDPSClientListener *)) {
            const uint8 *pl = buf + UDPS_HEADER_SIZE;
            if (hdr->type == UDPS_TYPE_CONFIG) {
                listener->OnUDPSConfig(pl, payloadBytes);
            }
            else {
                listener->OnUDPSData(pl, payloadBytes);
            }
        }
        return;
    }

    bool completed = PlaceFragment(hdr, buf + UDPS_HEADER_SIZE, payloadBytes);
    if ((hdr->type == UDPS_TYPE_DATA) && (listener != NULL_PTR(UDPSClientListener *))) {
        listener->OnUDPSFragment(hdr->counter, size, completed);
    }
}

// ---------------------------------------------------------------------------
// Private: PlaceFragment
// ---------------------------------------------------------------------------

bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
                                const uint8 *payload,
                                uint32 payloadBytes) {
    uint32 counter      = hdr->counter;
    uint16 fragIdx      = hdr->fragmentIdx;
    uint16 totalFrags   = hdr->totalFragments;

    if ((fragIdx >= totalFrags) || (totalFrags > 512u)) {
        return false; // sanity check
    }

    // Find existing slot for this counter
    uint32 slot = UDPS_CLIENT_MAX_REASSEMBLY_SLOTS;
    for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
        if (reassemblySlots[i].active && (reassemblySlots[i].counter == counter)) {
            slot = i;
            break;
        }
    }

    // Allocate new slot if not found
    if (slot >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
        for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
            if (!reassemblySlots[i].active) {
                slot = i;
                break;
            }
        }
        if (slot >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
            // All slots occupied — evict the oldest
            uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL;
            for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
                if (reassemblySlots[i].firstSeenTicks < oldestTick) {
                    oldestTick = reassemblySlots[i].firstSeenTicks;
                    slot = i;
                }
            }
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSClient: Reassembly slots full; evicting oldest.");
        }

        reassemblySlots[slot].counter           = counter;
        reassemblySlots[slot].type              = hdr->type;
        reassemblySlots[slot].totalFragments    = totalFrags;
        reassemblySlots[slot].receivedFragments = 0u;
        reassemblySlots[slot].active            = true;
        reassemblySlots[slot].firstSeenTicks    = HighResolutionTimer::Counter();
        reassemblySlots[slot].chunkSize         = 0u;
        reassemblySlots[slot].assembledBytes    = 0u;
        (void) MemoryOperationsHelper::Set(reassemblySlots[slot].recvMask, 0, 32u);
    }

    UDPSReassemblySlot &s = reassemblySlots[slot];

    // Skip duplicate
    uint32 byteIdx = fragIdx / 8u;
    uint8  bitMask = static_cast<uint8>(1u << (fragIdx % 8u));
    if (byteIdx < 32u) {
        if ((s.recvMask[byteIdx] & bitMask) != 0u) {
            return false; // already have this fragment
        }
    }

    // Compute placement offset
    uint32 chunkSize = s.chunkSize;
    if (chunkSize == 0u) {
        // Learn chunk size from first non-last fragment
        if (fragIdx == 0u) {
            chunkSize = payloadBytes;
            s.chunkSize = chunkSize;
        }
        else {
            // Can't place yet without knowing chunk size — drop (rare edge case)
            return false;
        }
    }

    uint32 offset = static_cast<uint32>(fragIdx) * chunkSize;
    if ((offset + payloadBytes) > static_cast<uint32>(sizeof(s.payload))) {
        return false; // overflow guard
    }

    if (payloadBytes > 0u) {
        (void) MemoryOperationsHelper::Copy(s.payload + offset, payload, payloadBytes);
    }

    // Track the exact assembled size: the highest byte written across all
    // fragments. The last fragment is usually smaller than chunkSize, so the
    // total is not chunkSize*totalFragments.
    if ((offset + payloadBytes) > s.assembledBytes) {
        s.assembledBytes = offset + payloadBytes;
    }

    if (byteIdx < 32u) {
        s.recvMask[byteIdx] |= bitMask;
    }
    s.receivedFragments++;

    // Check if complete
    if (s.receivedFragments >= s.totalFragments) {
        DeliverAssembled(s);
        s.active = false;
        return true;
    }
    return false;
}

// ---------------------------------------------------------------------------
// Private: DeliverAssembled
// ---------------------------------------------------------------------------

void UDPSClient::DeliverAssembled(UDPSReassemblySlot &s) {
    if (listener == NULL_PTR(UDPSClientListener *)) {
        return;
    }
    // The exact assembled size is the highest byte offset written across all
    // fragments (the last fragment is typically smaller than chunkSize), tracked
    // incrementally in PlaceFragment.
    uint32 totalSize = s.assembledBytes;

    if (s.type == UDPS_TYPE_CONFIG) {
        listener->OnUDPSConfig(s.payload, totalSize);
    }
    else {
        listener->OnUDPSData(s.payload, totalSize);
    }
}

// ---------------------------------------------------------------------------
// Private: GcReassemblySlots
// ---------------------------------------------------------------------------

void UDPSClient::GcReassemblySlots() {
    uint64 staleThreshold = 2u * HighResolutionTimer::Frequency(); // 2 seconds
    uint64 now            = HighResolutionTimer::Counter();

    for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
        if (!reassemblySlots[i].active) {
            continue;
        }
        if ((now - reassemblySlots[i].firstSeenTicks) > staleThreshold) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSClient: Discarding stale reassembly slot (counter %u).",
                                reassemblySlots[i].counter);
            reassemblySlots[i].active = false;
        }
    }
}

} // namespace MARTe