/**
 * @file UDPSServer.cpp
 * @brief Implementation of UDPSServer — multi-client UDPS session management.
 */

#include "UDPSServer.h"

#include "AdvancedErrorManagement.h"
#include "HighResolutionTimer.h"
#include "MemoryOperationsHelper.h"
#include "StreamString.h"

#include <sys/select.h>
#include <poll.h>

namespace MARTe {

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

UDPSServer::UDPSServer()
    : port(0u),
      maxPayloadSize(UDPS_SERVER_DEFAULT_MAX_PAYLOAD),
      dataPort(0u),
      useMulticast(false),
      clientTimeoutTicks(0u),
      numUnicastClients(0u),
      numTCPClients(0u),
      cachedConfig(NULL_PTR(uint8 *)),
      cachedConfigSize(0u),
      sendBuf(NULL_PTR(uint8 *)),
      sendBufCapacity(0u),
      configCounter(0u),
      started(false) {

    for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
        unicastClients[i].ipAddr[0]     = '\0';
        unicastClients[i].clientPort    = 0u;
        unicastClients[i].lastSeenTicks = 0u;
        unicastClients[i].active        = false;
        unicastClients[i].isStatic      = false;
    }

    for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
        tcpClients[i] = NULL_PTR(BasicTCPSocket *);
    }
}

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

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

bool UDPSServer::Initialise(StructuredDataI &data) {
    uint32 portU32 = 0u;
    if (data.Read("Port", portU32)) {
        port = static_cast<uint16>(portU32);
    }
    else {
        REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
                            "UDPSServer: Port not specified.");
        return false;
    }
    /* port == 0 is valid: unicast push-only mode (no serverSocket bind,
     * no CONNECT/DISCONNECT/ACK reception). Clients added via AddStaticClient(). */

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

    if (useMulticast) {
        uint32 dpU32 = static_cast<uint32>(port) + 1u;
        (void) data.Read("DataPort", dpU32);
        dataPort = static_cast<uint16>(dpU32);
        if (dataPort == port) {
            REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
                                "UDPSServer: DataPort (%u) must differ from Port (%u).",
                                static_cast<uint32>(dataPort), static_cast<uint32>(port));
            return false;
        }
    }

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

    uint32 timeoutSecs = UDPS_SERVER_DEFAULT_CLIENT_TIMEOUT_S;
    (void) data.Read("ClientTimeout", timeoutSecs);
    clientTimeoutTicks = (timeoutSecs > 0u)
        ? (static_cast<uint64>(timeoutSecs) * HighResolutionTimer::Frequency())
        : 0u;

    return true;
}

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

bool UDPSServer::Start() {
    if (started) {
        return true;
    }

    sendBufCapacity = UDPS_HEADER_SIZE + maxPayloadSize;
    sendBuf = new uint8[sendBufCapacity];
    if (sendBuf == NULL_PTR(uint8 *)) {
        REPORT_ERROR_STATIC(ErrorManagement::FatalError,
                            "UDPSServer: Could not allocate send buffer.");
        return false;
    }

    bool ok = true;

    if (useMulticast) {
        // TCP listener for CONNECT + CONFIG
        ok = tcpListener.Open();
        if (ok) {
            ok = tcpListener.Listen(port, 5);
        }
        if (ok) {
            tcpListener.SetBlocking(false);
        }
        // UDP data socket connected to multicast group
        if (ok) {
            ok = dataSocket.Open();
        }
        if (ok) {
            ok = dataSocket.Connect(multicastGroup.Buffer(), dataPort);
        }
        if (!ok) {
            REPORT_ERROR_STATIC(ErrorManagement::FatalError,
                                "UDPSServer: Failed to open multicast sockets on port %u.",
                                static_cast<uint32>(port));
        }
    }
    else {
        // Unicast send socket (unconnected; SetDestination per Write)
        ok = uniSendSocket.Open();
        if (!ok) {
            REPORT_ERROR_STATIC(ErrorManagement::FatalError,
                                "UDPSServer: Failed to open unicast send socket.");
        }

        // Receive socket: only bind if port != 0 (skip for push-only mode)
        if (ok && (port != 0u)) {
            ok = serverSocket.Open();
            if (ok) {
                ok = serverSocket.Listen(port); // UDP "listen" = bind
            }
            if (!ok) {
                REPORT_ERROR_STATIC(ErrorManagement::FatalError,
                                    "UDPSServer: Failed to bind receive socket on port %u.",
                                    static_cast<uint32>(port));
            }
        }
    }

    started = ok;
    return ok;
}

bool UDPSServer::Stop() {
    if (!started) {
        return true;
    }

    // Evict all unicast clients
    for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
        if (unicastClients[i].active) {
            EvictUnicastClient(i);
        }
    }
    numUnicastClients = 0u;

    // Close all TCP clients
    for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
        if (tcpClients[i] != NULL_PTR(BasicTCPSocket *)) {
            EvictTCPClient(i);
        }
    }
    numTCPClients = 0u;

    // Close sockets
    if (serverSocket.IsValid()) {
        (void) serverSocket.Close();
    }
    if (uniSendSocket.IsValid()) {
        (void) uniSendSocket.Close();
    }
    if (tcpListener.IsValid()) {
        (void) tcpListener.Close();
    }
    if (dataSocket.IsValid()) {
        (void) dataSocket.Close();
    }

    // Free heap buffers
    if (cachedConfig != NULL_PTR(uint8 *)) {
        delete[] cachedConfig;
        cachedConfig     = NULL_PTR(uint8 *);
        cachedConfigSize = 0u;
    }
    if (sendBuf != NULL_PTR(uint8 *)) {
        delete[] sendBuf;
        sendBuf         = NULL_PTR(uint8 *);
        sendBufCapacity = 0u;
    }

    started = false;
    return true;
}

// ---------------------------------------------------------------------------
// ServiceClients
// ---------------------------------------------------------------------------

void UDPSServer::ServiceClients() {
    if (!started) {
        return;
    }

    if (useMulticast) {
        // Poll TCP listener for new connections (non-blocking).
        // Guard with poll() first so WaitConnection is never called on an empty
        // queue — avoids the MARTe2 "Failed accept in unblocking mode" log spam.
        if (tcpListener.IsValid()) {
            struct pollfd pfd;
            pfd.fd      = static_cast<int>(tcpListener.GetReadHandle());
            pfd.events  = POLLIN;
            pfd.revents = 0;
            bool pending = (::poll(&pfd, 1u, 0) > 0) &&
                           ((pfd.revents & POLLIN) != 0);
            BasicTCPSocket *newConn = pending
                ? tcpListener.WaitConnection(0u)
                : NULL_PTR(BasicTCPSocket *);
            if (newConn != NULL_PTR(BasicTCPSocket *)) {
                // Find free TCP client slot
                uint32 freeSlot = UDPS_SERVER_MAX_TCP_CLIENTS;
                for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
                    if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) {
                        freeSlot = i;
                        break;
                    }
                }
                if (freeSlot < UDPS_SERVER_MAX_TCP_CLIENTS) {
                    HandleMulticastTCPConnect(newConn, freeSlot);
                }
                else {
                    REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                        "UDPSServer: TCP client table full, rejecting new connection.");
                    (void) newConn->Close();
                    delete newConn;
                }
            }
        }

        // Poll existing TCP clients for DISCONNECT
        for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
            if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) {
                continue;
            }
            // Non-blocking peek
            int fd = tcpClients[i]->GetReadHandle();
            if (fd < 0 || fd >= FD_SETSIZE) { continue; /* HI-6: skip FDs outside select range */ }
            fd_set rset;
            FD_ZERO(&rset);
            FD_SET(fd, &rset);
            struct timeval tv;
            tv.tv_sec = 0; tv.tv_usec = 0;
            int nready = select(fd + 1, &rset, NULL, NULL, &tv);
            if (nready > 0) {
                uint8  pktBuf[UDPS_HEADER_SIZE];
                uint32 recvSize = UDPS_HEADER_SIZE;
                bool   recvOk   = tcpClients[i]->Read(reinterpret_cast<char8 *>(pktBuf), recvSize);
                if (!recvOk || (recvSize == 0u)) {
                    REPORT_ERROR_STATIC(ErrorManagement::Information,
                                        "UDPSServer: TCP client disconnected (slot %u).", i);
                    EvictTCPClient(i);
                    continue;
                }
                if (recvSize >= UDPS_HEADER_SIZE) {
                    const UDPSPacketHeader *hdr =
                        reinterpret_cast<const UDPSPacketHeader *>(pktBuf);
                    if ((hdr->magic == UDPS_MAGIC) &&
                        (hdr->type  == UDPS_TYPE_DISCONNECT)) {
                        REPORT_ERROR_STATIC(ErrorManagement::Information,
                                            "UDPSServer: TCP client sent DISCONNECT (slot %u).", i);
                        EvictTCPClient(i);
                    }
                }
            }
        }
    }
    else {
        // Unicast: poll serverSocket for CONNECT / DISCONNECT / ACK
        if (!serverSocket.IsValid()) {
            return;
        }
        int fd = serverSocket.GetReadHandle();
        if (fd < 0 || fd >= FD_SETSIZE) { return; /* HI-6 */ }
        fd_set rset;
        FD_ZERO(&rset);
        FD_SET(fd, &rset);
        struct timeval tv = {0, 0};
        int nready = select(fd + 1, &rset, NULL, NULL, &tv);
        while (nready > 0) {
            uint8  pktBuf[UDPS_HEADER_SIZE + 4u];
            uint32 recvSize = static_cast<uint32>(sizeof(pktBuf));
            bool   recvOk   = serverSocket.Read(reinterpret_cast<char8 *>(pktBuf), recvSize);
            if (!recvOk || (recvSize < UDPS_HEADER_SIZE)) {
                break;
            }
            const UDPSPacketHeader *hdr =
                reinterpret_cast<const UDPSPacketHeader *>(pktBuf);
            if (hdr->magic != UDPS_MAGIC) {
                break;
            }
            InternetHost src = serverSocket.GetSource();
            if (hdr->type == UDPS_TYPE_CONNECT) {
                HandleUnicastConnect(src);
            }
            else if (hdr->type == UDPS_TYPE_DISCONNECT) {
                HandleUnicastDisconnect(src);
            }
            else if (hdr->type == UDPS_TYPE_ACK) {
                HandleUnicastAck(src);
            }

            // Check if more data is ready
            FD_ZERO(&rset);
            FD_SET(fd, &rset);
            tv.tv_sec  = 0;
            tv.tv_usec = 0;
            nready = select(fd + 1, &rset, NULL, NULL, &tv);
        }

        // Evict stale clients (if timeout is configured)
        if (clientTimeoutTicks > 0u) {
            EvictStaleUnicastClients();
        }
    }
}

// ---------------------------------------------------------------------------
// SendConfig
// ---------------------------------------------------------------------------

bool UDPSServer::SendConfig(const uint8 *payload, uint32 payloadSize) {
    if (!started) {
        return false;
    }

    // Cache for future CONNECT clients
    (void) CacheConfig(payload, payloadSize);

    configCounter++;

    bool ok = true;

    if (useMulticast) {
        // Send CONFIG over TCP to each connected client
        for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
            if (tcpClients[i] == NULL_PTR(BasicTCPSocket *)) {
                continue;
            }
            bool sent = SendFragmentedTCP(*tcpClients[i], UDPS_TYPE_CONFIG,
                                          configCounter, payload, payloadSize);
            if (!sent) {
                REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                    "UDPSServer: CONFIG send failed to TCP client %u, evicting.", i);
                EvictTCPClient(i);
                ok = false;
            }
        }
    }
    else {
        // Send CONFIG to each unicast client
        for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
            if (!unicastClients[i].active) {
                continue;
            }
            InternetHost dest(unicastClients[i].clientPort, unicastClients[i].ipAddr);
            bool sent = SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG,
                                          configCounter, payload, payloadSize);
            if (!sent) {
                REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                    "UDPSServer: CONFIG send failed to %s:%u.",
                                    unicastClients[i].ipAddr,
                                    static_cast<uint32>(unicastClients[i].clientPort));
                ok = false;
            }
        }
    }

    return ok;
}

// ---------------------------------------------------------------------------
// SendData
// ---------------------------------------------------------------------------

bool UDPSServer::SendData(uint32 counter, const uint8 *payload, uint32 payloadSize) {
    if (!started) {
        return false;
    }

    bool ok = true;

    if (useMulticast) {
        // Single multicast write (no dest needed — socket already connected)
        bool sent = SendFragmentedUDP(dataSocket, NULL_PTR(InternetHost *),
                                      UDPS_TYPE_DATA, counter, payload, payloadSize);
        if (!sent) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSServer: DATA send to multicast group failed.");
            ok = false;
        }
    }
    else {
        for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
            if (!unicastClients[i].active) {
                continue;
            }
            InternetHost dest(unicastClients[i].clientPort, unicastClients[i].ipAddr);
            bool sent = SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_DATA,
                                          counter, payload, payloadSize);
            if (!sent) {
                REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                    "UDPSServer: DATA send failed to %s:%u.",
                                    unicastClients[i].ipAddr,
                                    static_cast<uint32>(unicastClients[i].clientPort));
                ok = false;
            }
        }
    }

    return ok;
}

// ---------------------------------------------------------------------------
// AddStaticClient
// ---------------------------------------------------------------------------

bool UDPSServer::AddStaticClient(const char8 *ip, uint16 port_) {
    if ((ip == NULL_PTR(const char8 *)) || (ip[0] == '\0')) {
        return false;
    }

    // Check for duplicate
    uint32 existing = FindUnicastClient(ip, port_);
    if (existing < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
        unicastClients[existing].isStatic = true; // ensure it's marked static
        return true;
    }

    // Find free slot
    uint32 freeSlot = UDPS_SERVER_MAX_UNICAST_CLIENTS;
    for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
        if (!unicastClients[i].active) {
            freeSlot = i;
            break;
        }
    }
    if (freeSlot >= UDPS_SERVER_MAX_UNICAST_CLIENTS) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSServer: Unicast client table full, cannot add static client %s:%u.",
                            ip, static_cast<uint32>(port_));
        return false;
    }

    // Populate slot
    uint32 ipLen = 0u;
    while ((ipLen < 63u) && (ip[ipLen] != '\0')) {
        unicastClients[freeSlot].ipAddr[ipLen] = ip[ipLen];
        ipLen++;
    }
    unicastClients[freeSlot].ipAddr[ipLen] = '\0';
    unicastClients[freeSlot].clientPort    = port_;
    unicastClients[freeSlot].lastSeenTicks = HighResolutionTimer::Counter();
    unicastClients[freeSlot].active        = true;
    unicastClients[freeSlot].isStatic      = true;
    numUnicastClients++;

    REPORT_ERROR_STATIC(ErrorManagement::Information,
                        "UDPSServer: Static client added: %s:%u.",
                        ip, static_cast<uint32>(port_));
    return true;
}

// ---------------------------------------------------------------------------
// Accessors
// ---------------------------------------------------------------------------

uint32 UDPSServer::GetClientCount() const {
    uint32 count = 0u;
    for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
        if (unicastClients[i].active) {
            count++;
        }
    }
    for (uint32 i = 0u; i < UDPS_SERVER_MAX_TCP_CLIENTS; i++) {
        if (tcpClients[i] != NULL_PTR(BasicTCPSocket *)) {
            count++;
        }
    }
    return count;
}

bool UDPSServer::HasClients() const {
    return (GetClientCount() > 0u);
}

bool UDPSServer::IsMulticast() const {
    return useMulticast;
}

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

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

// ---------------------------------------------------------------------------
// Private: SendFragmentedUDP
// ---------------------------------------------------------------------------

bool UDPSServer::SendFragmentedUDP(BasicUDPSocket &sock,
                                    InternetHost   *dest,
                                    uint8           type,
                                    uint32          counter,
                                    const uint8    *payload,
                                    uint32          payloadSize) {
    uint32 maxChunk   = maxPayloadSize; // payload bytes per fragment (excl. header)
    uint32 totalFrags = (payloadSize == 0u) ? 1u :
                        ((payloadSize + maxChunk - 1u) / maxChunk);

    bool   ok   = true;
    uint32 offs = 0u;

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

        UDPSBuildHeader(sendBuf, type, counter,
                        static_cast<uint16>(f),
                        static_cast<uint16>(totalFrags),
                        chunkSize);

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

        uint32 sendSize = UDPS_HEADER_SIZE + chunkSize;

        if (dest != NULL_PTR(InternetHost *)) {
            (void) sock.SetDestination(*dest);
        }

        ok = sock.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
        if (!ok) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSServer: UDP fragment %u/%u write failed.",
                                f + 1u, totalFrags);
        }

        offs += chunkSize;
    }

    return ok;
}

// ---------------------------------------------------------------------------
// Private: SendFragmentedTCP
// ---------------------------------------------------------------------------

bool UDPSServer::SendFragmentedTCP(BasicTCPSocket &sock,
                                    uint8           type,
                                    uint32          counter,
                                    const uint8    *payload,
                                    uint32          payloadSize) {
    uint32 maxChunk   = maxPayloadSize;
    uint32 totalFrags = (payloadSize == 0u) ? 1u :
                        ((payloadSize + maxChunk - 1u) / maxChunk);

    bool   ok   = true;
    uint32 offs = 0u;

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

        UDPSBuildHeader(sendBuf, type, counter,
                        static_cast<uint16>(f),
                        static_cast<uint16>(totalFrags),
                        chunkSize);

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

        uint32 sendSize = UDPS_HEADER_SIZE + chunkSize;
        ok = sock.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
        if (!ok) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSServer: TCP fragment %u/%u write failed.",
                                f + 1u, totalFrags);
        }

        offs += chunkSize;
    }

    return ok;
}

// ---------------------------------------------------------------------------
// Private: EvictStaleUnicastClients
// ---------------------------------------------------------------------------

void UDPSServer::EvictStaleUnicastClients() {
    uint64 now = HighResolutionTimer::Counter();
    for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
        if (!unicastClients[i].active || unicastClients[i].isStatic) {
            continue;
        }
        uint64 elapsed = now - unicastClients[i].lastSeenTicks;
        if (elapsed >= clientTimeoutTicks) {
            REPORT_ERROR_STATIC(ErrorManagement::Information,
                                "UDPSServer: Evicting stale client %s:%u.",
                                unicastClients[i].ipAddr,
                                static_cast<uint32>(unicastClients[i].clientPort));
            EvictUnicastClient(i);
        }
    }
}

// ---------------------------------------------------------------------------
// Private: HandleUnicastConnect
// ---------------------------------------------------------------------------

void UDPSServer::HandleUnicastConnect(const InternetHost &src) {
    StreamString srcAddrStr = src.GetAddress();
    const char8 *srcAddr    = srcAddrStr.Buffer();
    uint16       srcPort    = src.GetPort();

    REPORT_ERROR_STATIC(ErrorManagement::Information,
                        "UDPSServer: CONNECT from %s:%u.",
                        srcAddr, static_cast<uint32>(srcPort));

    // Check if this client is already known
    uint32 existing = FindUnicastClient(srcAddr, srcPort);
    if (existing < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
        // Refresh last-seen and resend CONFIG
        unicastClients[existing].lastSeenTicks = HighResolutionTimer::Counter();
        if (cachedConfig != NULL_PTR(uint8 *)) {
            InternetHost dest(srcPort, srcAddr);
            configCounter++;
            (void) SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG,
                                     configCounter, cachedConfig, cachedConfigSize);
        }
        return;
    }

    // New client — find a free slot (or evict oldest non-static)
    uint32 slot = UDPS_SERVER_MAX_UNICAST_CLIENTS;
    for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
        if (!unicastClients[i].active) {
            slot = i;
            break;
        }
    }
    if (slot >= UDPS_SERVER_MAX_UNICAST_CLIENTS) {
        slot = FindOldestUnicastClient();
        if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
            REPORT_ERROR_STATIC(ErrorManagement::Information,
                                "UDPSServer: Client table full — evicting %s:%u.",
                                unicastClients[slot].ipAddr,
                                static_cast<uint32>(unicastClients[slot].clientPort));
            EvictUnicastClient(slot);
        }
        else {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSServer: All slots occupied by static clients; rejecting %s:%u.",
                                srcAddr, static_cast<uint32>(srcPort));
            return;
        }
    }

    // Fill slot
    uint32 addrLen = 0u;
    while ((addrLen < 63u) && (srcAddr[addrLen] != '\0')) {
        unicastClients[slot].ipAddr[addrLen] = srcAddr[addrLen];
        addrLen++;
    }
    unicastClients[slot].ipAddr[addrLen] = '\0';
    unicastClients[slot].clientPort      = srcPort;
    unicastClients[slot].lastSeenTicks   = HighResolutionTimer::Counter();
    unicastClients[slot].active          = true;
    unicastClients[slot].isStatic        = false;
    numUnicastClients++;

    // Send cached CONFIG if available
    if (cachedConfig != NULL_PTR(uint8 *)) {
        InternetHost dest(srcPort, srcAddr);
        configCounter++;
        (void) SendFragmentedUDP(uniSendSocket, &dest, UDPS_TYPE_CONFIG,
                                 configCounter, cachedConfig, cachedConfigSize);
    }
}

// ---------------------------------------------------------------------------
// Private: HandleUnicastDisconnect
// ---------------------------------------------------------------------------

void UDPSServer::HandleUnicastDisconnect(const InternetHost &src) {
    StreamString srcAddrStr = src.GetAddress();
    const char8 *srcAddr    = srcAddrStr.Buffer();
    uint16       srcPort    = src.GetPort();

    uint32 slot = FindUnicastClient(srcAddr, srcPort);
    if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
        REPORT_ERROR_STATIC(ErrorManagement::Information,
                            "UDPSServer: DISCONNECT from %s:%u.",
                            srcAddr, static_cast<uint32>(srcPort));
        EvictUnicastClient(slot);
    }
}

// ---------------------------------------------------------------------------
// Private: HandleUnicastAck
// ---------------------------------------------------------------------------

void UDPSServer::HandleUnicastAck(const InternetHost &src) {
    StreamString srcAddrStr = src.GetAddress();
    const char8 *srcAddr    = srcAddrStr.Buffer();
    uint16       srcPort    = src.GetPort();

    uint32 slot = FindUnicastClient(srcAddr, srcPort);
    if (slot < UDPS_SERVER_MAX_UNICAST_CLIENTS) {
        unicastClients[slot].lastSeenTicks = HighResolutionTimer::Counter();
    }
}

// ---------------------------------------------------------------------------
// Private: FindUnicastClient
// ---------------------------------------------------------------------------

uint32 UDPSServer::FindUnicastClient(const char8 *ip, uint16 port_) const {
    for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
        if (!unicastClients[i].active) {
            continue;
        }
        if (unicastClients[i].clientPort != port_) {
            continue;
        }
        // String compare
        bool match = true;
        uint32 k   = 0u;
        while ((k < 63u) && match) {
            if (unicastClients[i].ipAddr[k] != ip[k]) {
                match = false;
            }
            if (ip[k] == '\0') {
                break;
            }
            k++;
        }
        if (match) {
            return i;
        }
    }
    return UDPS_SERVER_MAX_UNICAST_CLIENTS;
}

// ---------------------------------------------------------------------------
// Private: FindOldestUnicastClient
// ---------------------------------------------------------------------------

uint32 UDPSServer::FindOldestUnicastClient() const {
    uint32 oldest     = UDPS_SERVER_MAX_UNICAST_CLIENTS;
    uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL;

    for (uint32 i = 0u; i < UDPS_SERVER_MAX_UNICAST_CLIENTS; i++) {
        if (!unicastClients[i].active || unicastClients[i].isStatic) {
            continue;
        }
        if (unicastClients[i].lastSeenTicks < oldestTick) {
            oldestTick = unicastClients[i].lastSeenTicks;
            oldest     = i;
        }
    }
    return oldest;
}

// ---------------------------------------------------------------------------
// Private: EvictUnicastClient
// ---------------------------------------------------------------------------

void UDPSServer::EvictUnicastClient(uint32 idx) {
    if (idx >= UDPS_SERVER_MAX_UNICAST_CLIENTS) {
        return;
    }
    unicastClients[idx].active = false;
    if (numUnicastClients > 0u) {
        numUnicastClients--;
    }
}

// ---------------------------------------------------------------------------
// Private: HandleMulticastTCPConnect
// ---------------------------------------------------------------------------

void UDPSServer::HandleMulticastTCPConnect(BasicTCPSocket *newClient, uint32 idx) {
    if (idx >= UDPS_SERVER_MAX_TCP_CLIENTS) {
        return;
    }

    tcpClients[idx] = newClient;
    numTCPClients++;

    REPORT_ERROR_STATIC(ErrorManagement::Information,
                        "UDPSServer: Multicast TCP client connected (slot %u).", idx);

    // Send cached CONFIG over TCP
    if (cachedConfig != NULL_PTR(uint8 *)) {
        configCounter++;
        bool sent = SendFragmentedTCP(*newClient, UDPS_TYPE_CONFIG,
                                      configCounter, cachedConfig, cachedConfigSize);
        if (!sent) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSServer: Failed to send CONFIG to new TCP client (slot %u).", idx);
            EvictTCPClient(idx);
        }
    }
}

// ---------------------------------------------------------------------------
// Private: EvictTCPClient
// ---------------------------------------------------------------------------

void UDPSServer::EvictTCPClient(uint32 idx) {
    if (idx >= UDPS_SERVER_MAX_TCP_CLIENTS) {
        return;
    }
    if (tcpClients[idx] != NULL_PTR(BasicTCPSocket *)) {
        (void) tcpClients[idx]->Close();
        delete tcpClients[idx];
        tcpClients[idx] = NULL_PTR(BasicTCPSocket *);
        if (numTCPClients > 0u) {
            numTCPClients--;
        }
    }
}

// ---------------------------------------------------------------------------
// Private: CacheConfig
// ---------------------------------------------------------------------------

bool UDPSServer::CacheConfig(const uint8 *payload, uint32 payloadSize) {
    if (cachedConfig != NULL_PTR(uint8 *)) {
        delete[] cachedConfig;
        cachedConfig     = NULL_PTR(uint8 *);
        cachedConfigSize = 0u;
    }
    if ((payload == NULL_PTR(const uint8 *)) || (payloadSize == 0u)) {
        return true;
    }
    cachedConfig = new uint8[payloadSize];
    if (cachedConfig == NULL_PTR(uint8 *)) {
        REPORT_ERROR_STATIC(ErrorManagement::FatalError,
                            "UDPSServer: Failed to allocate cached CONFIG buffer.");
        return false;
    }
    (void) MemoryOperationsHelper::Copy(cachedConfig, payload, payloadSize);
    cachedConfigSize = payloadSize;
    return true;
}

} // namespace MARTe