#include "BasicTCPSocket.h"
#include "ConfigurationDatabase.h"
#include "DebugService.h"
#include "GlobalObjectsDatabase.h"
#include "HighResolutionTimer.h"
#include "LoggerService.h"
#include "Message.h"
#include "ObjectRegistryDatabase.h"
#include "ReferenceT.h"
#include "Sleep.h"
#include "StreamString.h"
#include "StringHelper.h"
#include "Threads.h"
#include "TimeoutType.h"
#include "UDPSProtocol.h"
#include <string.h>

namespace MARTe {

CLASS_REGISTER(DebugService, "1.0")

// C++98 ODR definitions for static constants
const uint32 DebugService::CMD_RATE_LIMIT;
const uint32 DebugService::CLIENT_IDLE_TIMEOUT_MS;
const uint32 DebugService::INPUT_BUFFER_MAX;

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

DebugService::DebugService()
    : DebugServiceBase(),
      EmbeddedServiceMethodBinderI(),
      binderServer(this, ServiceBinder::ServerType),
      binderStreamer(this, ServiceBinder::StreamerType),
      threadService(binderServer),
      streamerService(binderStreamer) {
    controlPort          = 0u;
    streamPort           = 8081u;
    logPort              = 8082u;
    streamIP             = "127.0.0.1";
    isServer             = false;
    suppressTimeoutLogs  = true;
    activeClient         = NULL_PTR(BasicTCPSocket *);
    cmdCountInWindow     = 0u;
    cmdWindowStartMs     = 0u;
    lastDataTimeMs       = 0u;
    inputBuffer          = "";

    // UDPS members
    udpsNumSlots         = 0u;
    udpsDataPayload      = NULL_PTR(uint8 *);
    udpsDataPayloadSize  = 0u;
    udpsPacketCounter    = 0u;
    udpsConfigPending    = false;
}

DebugService::~DebugService() {
    if (DebugServiceI::GetInstance() == this) {
        DebugServiceI::SetInstance(NULL_PTR(DebugServiceI *));
    }
    threadService.Stop();
    streamerService.Stop();
    tcpServer.Close();
    (void) udpsServer.Stop();
    if (activeClient != NULL_PTR(BasicTCPSocket *)) {
        activeClient->Close();
        delete activeClient;
        activeClient = NULL_PTR(BasicTCPSocket *);
    }
    if (udpsDataPayload != NULL_PTR(uint8 *)) {
        delete[] udpsDataPayload;
        udpsDataPayload = NULL_PTR(uint8 *);
    }
    // signals owned by DebugServiceBase destructor
}

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

bool DebugService::Initialise(StructuredDataI &data) {
    if (!ReferenceContainer::Initialise(data)) return false;

    uint32 port = 0u;
    if (data.Read("ControlPort", port)) {
        controlPort = (uint16)port;
    } else {
        (void)data.Read("TcpPort", port);
        controlPort = (uint16)port;
    }

    if (controlPort > 0u) {
        isServer = true;
        DebugServiceI::SetInstance(this);
    }

    port = 8081u;
    if (data.Read("StreamPort", port)) {
        streamPort = (uint16)port;
    } else {
        (void)data.Read("UdpPort", port);
        streamPort = (uint16)port;
    }

    port = 8082u;
    if (data.Read("LogPort", port)) {
        logPort = (uint16)port;
    } else {
        (void)data.Read("TcpLogPort", port);
        logPort = (uint16)port;
    }

    StreamString tempIP;
    if (data.Read("StreamIP", tempIP)) {
        streamIP = tempIP;
    } else {
        streamIP = "127.0.0.1";
    }

    uint32 suppress = 1u;
    if (data.Read("SuppressTimeoutLogs", suppress)) {
        suppressTimeoutLogs = (suppress == 1u);
    }

    StreamString tempToken;
    if (data.Read("AuthToken", tempToken)) {
        authToken = tempToken;
    }
    clientAuthenticated = (authToken.Size() == 0u);

    // Capture only the local subtree — do NOT call MoveToRoot() on the shared CDB.
    (void)data.Copy(fullConfig);

    if (isServer) {
        if (!traceBuffer.Init(8 * 1024 * 1024)) return false;
        PatchRegistry();

        ConfigurationDatabase threadData;
        threadData.Write("Timeout", (uint32)1000);
        threadService.Initialise(threadData);
        streamerService.Initialise(threadData);

        if (!tcpServer.Open())              return false;
        if (!tcpServer.Listen(controlPort)) return false;

        // Initialise UDP streamer in push-only mode (port=0 skips serverSocket bind)
        ConfigurationDatabase udpsConfig;
        (void)udpsConfig.Write("Port", 0u);
        (void)udpsConfig.Write("MaxPayloadSize", static_cast<uint32>(UDPSServer::UDPS_SERVER_DEFAULT_MAX_PAYLOAD));
        if (!udpsServer.Initialise(udpsConfig)) return false;
        if (!udpsServer.Start())               return false;
        if (streamPort > 0u) {
            (void)udpsServer.AddStaticClient(streamIP.Buffer(), streamPort);
        }

        if (threadService.Start()   != ErrorManagement::NoError) return false;
        if (streamerService.Start() != ErrorManagement::NoError) return false;

        // Send initial (empty) CONFIG so clients know the schema version
        SendUDPSConfig();
    }
    return true;
}

// ---------------------------------------------------------------------------
// Transport config hook (called by RebuildConfigFromRegistry in base)
// ---------------------------------------------------------------------------

void DebugService::RebuildTransportConfig() {
    const char8 *myName = GetName();
    if (myName != NULL_PTR(const char8 *)) {
        if (fullConfig.MoveRelative(myName)) {
            (void)fullConfig.Write("ControlPort", static_cast<uint32>(controlPort));
            (void)fullConfig.Write("UdpPort",     static_cast<uint32>(streamPort));
            (void)fullConfig.Write("LogPort",     static_cast<uint32>(logPort));
            if (streamIP.Size() > 0u)
                (void)fullConfig.Write("StreamIP", streamIP.Buffer());
            (void)fullConfig.MoveToAncestor(1u);
        }
    }
}

// ---------------------------------------------------------------------------
// SERVICE_INFO hook
// ---------------------------------------------------------------------------

void DebugService::GetServiceInfo(StreamString &out) {
    out.Printf("OK SERVICE_INFO TCP_CTRL:%u UDP_STREAM:%u TCP_LOG:%u STATE:%s\n",
               controlPort, streamPort, logPort,
               isPaused ? "PAUSED" : "RUNNING");
}

// ---------------------------------------------------------------------------
// Execute / HandleMessage (framework boilerplate)
// ---------------------------------------------------------------------------

ErrorManagement::ErrorType DebugService::Execute(ExecutionInfo &info) {
    (void)info;
    return ErrorManagement::FatalError;
}

ErrorManagement::ErrorType DebugService::HandleMessage(ReferenceT<Message> &data) {
    (void)data;
    return ErrorManagement::NoError;
}

// ---------------------------------------------------------------------------
// TraceSignal override — sets config-pending flag after base handling
// ---------------------------------------------------------------------------

uint32 DebugService::TraceSignal(const char8 *name, bool enable, uint32 decimation) {
    uint32 ret = DebugServiceBase::TraceSignal(name, enable, decimation);
    udpsConfigPending = true;
    return ret;
}

// ---------------------------------------------------------------------------
// InjectTcpLoggerIfNeeded
// ---------------------------------------------------------------------------

void DebugService::InjectTcpLoggerIfNeeded() {
    if (logPort == 0u) return;

    Reference existing = ObjectRegistryDatabase::Instance()->Find("LoggerService");
    if (existing.IsValid()) {
        ReferenceContainer *rc =
            dynamic_cast<ReferenceContainer *>(existing.operator->());
        if (rc != NULL_PTR(ReferenceContainer *)) {
            for (uint32 i = 0u; i < rc->Size(); i++) {
                Reference child = rc->Get(i);
                // Check by class name to avoid a direct symbol dependency on TcpLogger.so
                if (child.IsValid()) {
                    const ClassProperties *cp = child->GetClassProperties();
                    if (cp != NULL_PTR(const ClassProperties *)) {
                        if (StringHelper::Compare(cp->GetName(), "TcpLogger") == 0) {
                            return;  // already has a TcpLogger
                        }
                    }
                }
            }
        }
    }

    ConfigurationDatabase lsCdb;
    (void)lsCdb.Write("Class", "LoggerService");
    uint32 cpus = 1u;
    (void)lsCdb.Write("CPUs", cpus);
    if (lsCdb.CreateRelative("+DebugConsumer")) {
        (void)lsCdb.Write("Class", "TcpLogger");
        uint32 p = static_cast<uint32>(logPort);
        (void)lsCdb.Write("Port", p);
        (void)lsCdb.MoveToAncestor(1u);
    }
    (void)lsCdb.MoveToRoot();

    ReferenceT<LoggerService> ls(
        "LoggerService", GlobalObjectsDatabase::Instance()->GetStandardHeap());
    if (!ls.IsValid()) return;
    ls->SetName("LoggerService");
    if (!ls->Initialise(lsCdb)) return;
    (void)ObjectRegistryDatabase::Instance()->Insert(ls);
}

// ---------------------------------------------------------------------------
// Server thread
// ---------------------------------------------------------------------------

ErrorManagement::ErrorType DebugService::Server(ExecutionInfo &info) {
    if (info.GetStage() == ExecutionInfo::TerminationStage)
        return ErrorManagement::NoError;
    if (info.GetStage() == ExecutionInfo::StartupStage) {
        serverThreadId = Threads::Id();
        Sleep::MSec(500u);
        InjectTcpLoggerIfNeeded();
        return ErrorManagement::NoError;
    }

    uint64 nowMs = (uint64)((float64)HighResolutionTimer::Counter() *
                             HighResolutionTimer::Period() * 1000.0);

    if (activeClient == NULL_PTR(BasicTCPSocket *)) {
        BasicTCPSocket *newClient = tcpServer.WaitConnection(TimeoutType(100));
        if (newClient != NULL_PTR(BasicTCPSocket *)) {
            clientMutex.FastLock();
            activeClient = newClient;
            clientMutex.FastUnLock();
            cmdCountInWindow = 0u;
            cmdWindowStartMs = nowMs;
            lastDataTimeMs   = nowMs;
            /* CR-5: require auth if an AuthToken is configured. */
            clientAuthenticated = (authToken.Size() == 0u);
        }
    } else {
        if (nowMs - lastDataTimeMs > CLIENT_IDLE_TIMEOUT_MS) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                "Server: TCP client idle for >%u ms — closing connection.",
                CLIENT_IDLE_TIMEOUT_MS);
            inputBuffer = "";
            clientMutex.FastLock();
            activeClient->Close();
            delete activeClient;
            activeClient = NULL_PTR(BasicTCPSocket *);
            clientMutex.FastUnLock();
            cmdCountInWindow = 0u;
        } else if (!activeClient->IsConnected()) {
            inputBuffer = "";
            clientMutex.FastLock();
            activeClient->Close();
            delete activeClient;
            activeClient = NULL_PTR(BasicTCPSocket *);
            clientMutex.FastUnLock();
        } else {
            char buffer[1024];
            uint32 size = 1024u;
            if (activeClient->Read(buffer, size)) {
                if (size > 0u) {
                    lastDataTimeMs = nowMs;

                    if (nowMs - cmdWindowStartMs >= 1000u) {
                        cmdWindowStartMs = nowMs;
                        cmdCountInWindow = 0u;
                    }

                    if (inputBuffer.Size() + (uint32)size > INPUT_BUFFER_MAX) {
                        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "Server: input buffer overflow (>%u bytes without newline) "
                            "— disconnecting.", INPUT_BUFFER_MAX);
                        inputBuffer = "";
                        clientMutex.FastLock();
                        activeClient->Close();
                        delete activeClient;
                        activeClient = NULL_PTR(BasicTCPSocket *);
                        clientMutex.FastUnLock();
                        cmdCountInWindow = 0u;
                    } else {
                        (void)inputBuffer.Seek(inputBuffer.Size());
                        uint32 writeSize = (uint32)size;
                        inputBuffer.Write(buffer, writeSize);

                        const char8 *raw   = inputBuffer.Buffer();
                        uint32       total = (uint32)inputBuffer.Size();
                        uint32       lineStart = 0u;
                        bool         rateLimitExceeded = false;

                        for (uint32 pos = 0u; pos < total && !rateLimitExceeded; pos++) {
                            if (raw[pos] != '\n') continue;
                            uint32 len = pos - lineStart;
                            if (len > 0u && raw[lineStart + len - 1u] == '\r') len--;
                            if (len > 0u) {
                                cmdCountInWindow++;
                                if (cmdCountInWindow > CMD_RATE_LIMIT) {
                                    REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                        "Server: client exceeded rate limit (%u cmd/s) "
                                        "— disconnecting.", CMD_RATE_LIMIT);
                                    rateLimitExceeded = true;
                                    break;
                                }
                                StreamString command;
                                uint32 cmdLen = len;
                                command.Write(raw + lineStart, cmdLen);

                                /* CR-5: Auth token gate.  If an AuthToken is
                                 * configured, the client must send
                                 * "AUTH <token>" before any other command. */
                                if (authToken.Size() > 0u) {
                                    const char8 *cmdPtr = command.Buffer();
                                    if (cmdLen >= 5u &&
                                        strncmp(cmdPtr, "AUTH ", 5u) == 0) {
                                        const char8 *recvToken = cmdPtr + 5u;
                                        uint32 recvLen = cmdLen - 5u;
                                        /* Strip trailing \r if present */
                                        if (recvLen > 0u &&
                                            recvToken[recvLen - 1u] == '\r') {
                                            recvLen--;
                                        }
                                        if (recvLen == authToken.Size() &&
                                            strncmp(recvToken,
                                                    authToken.Buffer(),
                                                    recvLen) == 0) {
                                            clientAuthenticated = true;
                                            const char8 *okResp =
                                                "OK AUTHENTICATED\n";
                                            uint32 respSz =
                                                static_cast<uint32>(
                                                    strlen(okResp));
                                            (void) activeClient->Write(
                                                okResp, respSz);
                                        } else {
                                            const char8 *badResp =
                                                "ERR AUTH_FAILED\n";
                                            uint32 respSz =
                                                static_cast<uint32>(
                                                    strlen(badResp));
                                            (void) activeClient->Write(
                                                badResp, respSz);
                                        }
                                    } else if (!clientAuthenticated) {
                                        const char8 *needAuth =
                                            "ERR AUTH_REQUIRED\n";
                                        uint32 respSz =
                                            static_cast<uint32>(
                                                strlen(needAuth));
                                        (void) activeClient->Write(
                                            needAuth, respSz);
                                    } else {
                                        // Dispatch via base HandleCommand,
                                        // write response to socket.
                                        StreamString out;
                                        HandleCommand(command, out);
                                        if (out.Size() > 0u) {
                                            const char8 *wPtr = out.Buffer();
                                            uint32 remaining =
                                                (uint32)out.Size();
                                            lastDataTimeMs =
                                                (uint64)((float64)
                                                    HighResolutionTimer::Counter() *
                                                    HighResolutionTimer::Period() *
                                                    1000.0);
                                            while (remaining > 0u) {
                                                uint32 wrote = remaining;
                                                if (!activeClient->Write(
                                                        wPtr, wrote) ||
                                                    wrote == 0u) {
                                                    break;
                                                }
                                                wPtr      += wrote;
                                                remaining -= wrote;
                                                lastDataTimeMs =
                                                    (uint64)((float64)
                                                        HighResolutionTimer::Counter() *
                                                        HighResolutionTimer::Period() *
                                                        1000.0);
                                            }
                                        }
                                    }
                                } else {
                                    // No auth token configured — back-compat.
                                    // Dispatch via base HandleCommand, write
                                    // response to socket.
                                    StreamString out;
                                    HandleCommand(command, out);
                                    if (out.Size() > 0u) {
                                        const char8 *wPtr = out.Buffer();
                                        uint32 remaining  = (uint32)out.Size();
                                        lastDataTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
                                                                  HighResolutionTimer::Period() * 1000.0);
                                        while (remaining > 0u) {
                                            uint32 wrote = remaining;
                                            if (!activeClient->Write(wPtr, wrote) || wrote == 0u) {
                                                break;
                                            }
                                            wPtr      += wrote;
                                            remaining -= wrote;
                                            lastDataTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
                                                                      HighResolutionTimer::Period() * 1000.0);
                                        }
                                    }
                                }
                            }
                            lineStart = pos + 1u;
                        }

                        if (rateLimitExceeded) {
                            inputBuffer = "";
                            clientMutex.FastLock();
                            activeClient->Close();
                            delete activeClient;
                            activeClient = NULL_PTR(BasicTCPSocket *);
                            clientMutex.FastUnLock();
                            cmdCountInWindow = 0u;
                        } else {
                            StreamString newInputBuffer;
                            if (lineStart < total) {
                                uint32 remLen = total - lineStart;
                                newInputBuffer.Write(raw + lineStart, remLen);
                            }
                            inputBuffer = newInputBuffer;
                        }
                    }
                }
            } else {
                inputBuffer = "";
                clientMutex.FastLock();
                activeClient->Close();
                delete activeClient;
                activeClient = NULL_PTR(BasicTCPSocket *);
                clientMutex.FastUnLock();
            }
        }
    }
    return ErrorManagement::NoError;
}

// ---------------------------------------------------------------------------
// Streamer thread — UDPS binary telemetry
// ---------------------------------------------------------------------------

ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo &info) {
    if (info.GetStage() == ExecutionInfo::TerminationStage) {
        if (udpsDataPayload != NULL_PTR(uint8 *)) {
            delete[] udpsDataPayload;
            udpsDataPayload = NULL_PTR(uint8 *);
        }
        return ErrorManagement::NoError;
    }
    if (info.GetStage() == ExecutionInfo::StartupStage) {
        streamerThreadId = Threads::Id();
        return ErrorManagement::NoError;
    }

    // a) Poll for new CONNECT clients (no-op in push-only mode with port=0)
    udpsServer.ServiceClients();

    // b) If config has changed, rebuild and send CONFIG packet
    if (udpsConfigPending) {
        SendUDPSConfig();
        udpsConfigPending = false;
    }

    // b) Drain traceBuffer — pack each sample into udpsDataPayload
    bool anyData = false;
    uint32 id, size;
    uint64 ts;
    uint8  udpsSampleBuf[UDPS_MAX_SAMPLE_BYTES];

    while (traceBuffer.Pop(id, ts, udpsSampleBuf, size, UDPS_MAX_SAMPLE_BYTES)) {
        // Find matching slot by internalID
        for (uint32 i = 0u; i < udpsNumSlots; i++) {
            if (udpsSlots[i].internalID == id) {
                if ((udpsDataPayload != NULL_PTR(uint8 *)) &&
                    (8u + udpsSlots[i].wireOffset + udpsSlots[i].wireSize <= udpsDataPayloadSize)) {
                    uint32 copySize = size;
                    if (copySize > udpsSlots[i].wireSize) copySize = udpsSlots[i].wireSize;
                    memcpy(udpsDataPayload + 8u + udpsSlots[i].wireOffset, udpsSampleBuf, copySize);
                    udpsSlots[i].everFilled = true;
                }
                anyData = true;
                break;
            }
        }
    }

    // c) If we have data, stamp with HRT and send via udpsServer
    if (anyData && udpsNumSlots > 0u && udpsDataPayload != NULL_PTR(uint8 *)) {
        uint64 hrt = HighResolutionTimer::Counter();
        memcpy(udpsDataPayload, &hrt, 8u);
        udpsPacketCounter++;
        (void)udpsServer.SendData(udpsPacketCounter, udpsDataPayload, udpsDataPayloadSize);
    }

    if (!anyData) {
        Sleep::MSec(1u);
    }

    return ErrorManagement::NoError;
}

// ---------------------------------------------------------------------------
// SendUDPSConfig — build and send a UDPS CONFIG packet
// ---------------------------------------------------------------------------

bool DebugService::SendUDPSConfig() {
    // Snapshot traced signals under mutex
    mutex.FastLock();

    // Count traced signals
    uint32 tracedCount = 0u;
    for (uint32 i = 0u; i < signals.GetNumberOfElements(); i++) {
        if (signals[i]->isTracing) {
            tracedCount++;
        }
    }

    // Build CONFIG payload into a local stack buffer
    static const uint32 CFG_BUF_SIZE = 65535u;
    uint8  cfgBuf[CFG_BUF_SIZE];
    uint8 *payload = cfgBuf;

    // Write numTraced (uint32 LE)
    memcpy(payload, &tracedCount, 4u);
    uint32 payloadOffset = 4u;

    // Build slot table in parallel
    uint32 currentOffset = 0u;
    uint32 newNumSlots = 0u;
    uint32 totalWireBytes = 0u;

    for (uint32 i = 0u; i < signals.GetNumberOfElements(); i++) {
        if (!signals[i]->isTracing) continue;
        if (newNumSlots >= UDPS_MAX_SLOTS) break;

        uint8 typeCode = UDPSTypeDescriptorToCode(signals[i]->type);
        if (typeCode == UDPS_TYPECODE_UNKNOWN) {
            // Skip unsupported types
            continue;
        }

        uint32 numElements = signals[i]->numberOfElements;
        if (numElements == 0u) numElements = 1u;

        uint32 numRows, numCols;
        if (signals[i]->numberOfDimensions >= 2u) {
            // For matrix: approximate square root
            numCols = numElements;
            numRows = 1u;
        } else if (signals[i]->numberOfDimensions == 1u) {
            numRows = numElements;
            numCols = 1u;
        } else {
            numRows = 1u;
            numCols = 1u;
        }

        uint32 wireSize = UDPSTypeCodeByteSize(typeCode) * numElements;

        // Write signal descriptor (136 bytes) to payload
        if (payloadOffset + UDPS_SIGNAL_DESC_SIZE <= CFG_BUF_SIZE - 1u) {
            UDPSSignalDescriptor *desc = reinterpret_cast<UDPSSignalDescriptor *>(payload + payloadOffset);
            memset(desc, 0, UDPS_SIGNAL_DESC_SIZE);
            if (signals[i]->name.Size() > 0u) {
                strncpy(desc->name, signals[i]->name.Buffer(), UDPS_MAX_SIGNAL_NAME - 1u);
            }
            desc->typeCode       = typeCode;
            desc->quantType      = UDPS_QUANT_NONE;
            desc->numDimensions  = signals[i]->numberOfDimensions;
            desc->numRows        = numRows;
            desc->numCols        = numCols;
            desc->rangeMin       = 0.0;
            desc->rangeMax       = 0.0;
            desc->timeMode       = UDPS_TIMEMODE_PACKET;
            desc->samplingRate   = 0.0;
            desc->timeSignalIdx  = UDPS_NO_TIME_SIGNAL;
            // unit stays zero

            payloadOffset += UDPS_SIGNAL_DESC_SIZE;
        }

        // Fill slot table entry
        udpsSlots[newNumSlots].internalID  = signals[i]->internalID;
        udpsSlots[newNumSlots].wireOffset  = currentOffset;
        udpsSlots[newNumSlots].wireSize    = wireSize;
        udpsSlots[newNumSlots].everFilled  = false;
        newNumSlots++;

        currentOffset    += wireSize;
        totalWireBytes   += wireSize;
    }

    // Write publish mode byte
    if (payloadOffset < CFG_BUF_SIZE) {
        payload[payloadOffset] = UDPS_PUBLISH_STRICT;
        payloadOffset++;
    }

    udpsNumSlots = newNumSlots;

    mutex.FastUnLock();

    // Reallocate data payload buffer
    if (udpsDataPayload != NULL_PTR(uint8 *)) {
        delete[] udpsDataPayload;
        udpsDataPayload = NULL_PTR(uint8 *);
    }
    udpsDataPayloadSize = 8u + totalWireBytes; // 8 bytes HRT + signal data
    if (udpsDataPayloadSize > 0u) {
        udpsDataPayload = new uint8[udpsDataPayloadSize];
        memset(udpsDataPayload, 0, udpsDataPayloadSize);
    }

    // Send CONFIG packet (also cached in udpsServer for new CONNECT clients)
    udpsPacketCounter++;
    (void)udpsServer.SendConfig(payload, payloadOffset);

    return true;
}

} // namespace MARTe
