#include "DebugService.h"
#include "AdvancedErrorManagement.h"
#include "StreamString.h"
#include "DebugBrokerWrapper.h"
#include "ObjectRegistryDatabase.h"
#include "ClassRegistryItem.h"
#include "ObjectBuilder.h"
#include "TypeConversion.h"
#include "HighResolutionTimer.h"
#include "ConfigurationDatabase.h"
#include "GAM.h"

// Explicitly include target brokers for templating

namespace MARTe {

DebugService* DebugService::instance = NULL_PTR(DebugService*);

static void EscapeJson(const char8* src, StreamString &dst) {
    if (src == NULL_PTR(const char8*)) return;
    while (*src != '\0') {
        if (*src == '"') dst += "\\\"";
        else if (*src == '\\') dst += "\\\\";
        else if (*src == '\n') dst += "\\n";
        else if (*src == '\r') dst += "\\r";
        else if (*src == '\t') dst += "\\t";
        else dst += *src;
        src++;
    }
}

CLASS_REGISTER(DebugService, "1.0")

DebugService::DebugService() : 
    ReferenceContainer(), EmbeddedServiceMethodBinderI(),
    binderServer(this, ServiceBinder::ServerType), 
    binderStreamer(this, ServiceBinder::StreamerType),
    threadService(binderServer),
    streamerService(binderStreamer)
{
    controlPort = 0;
    streamPort = 8081; 
    streamIP = "127.0.0.1";
    numberOfSignals = 0;
    numberOfAliases = 0;
    isServer = false;
    suppressTimeoutLogs = true; 
    isPaused = false;
    for (uint32 i=0; i<MAX_CLIENTS; i++) {
        activeClients[i] = NULL_PTR(BasicTCPSocket*);
    }
    serverThreadId = InvalidThreadIdentifier;
    streamerThreadId = InvalidThreadIdentifier;
}

DebugService::~DebugService() {
    if (instance == this) {
        instance = NULL_PTR(DebugService*);
    }

    threadService.Stop();
    streamerService.Stop();
    
    tcpServer.Close();
    udpSocket.Close();

    for (uint32 i=0; i<MAX_CLIENTS; i++) {
        if (activeClients[i] != NULL_PTR(BasicTCPSocket*)) {
            activeClients[i]->Close();
            delete activeClients[i];
        }
    }
}

bool DebugService::Initialise(StructuredDataI & data) {
    if (!ReferenceContainer::Initialise(data)) return false;
    
    if (!data.Read("ControlPort", controlPort)) {
        (void)data.Read("TcpPort", controlPort);
    }
    
    if (controlPort > 0) {
        isServer = true;
        instance = this; 
    }
    
    if (!data.Read("StreamPort", streamPort)) {
        (void)data.Read("UdpPort", streamPort);
    }

    StreamString tempIP;
    if (data.Read("StreamIP", tempIP)) {
        streamIP = tempIP;
    } else {
        streamIP = "127.0.0.1";
    }
    
    uint32 suppress = 1;
    if (data.Read("SuppressTimeoutLogs", suppress)) {
        suppressTimeoutLogs = (suppress == 1);
    }
    
    if (isServer) {
        if (!traceBuffer.Init(1024 * 1024)) return false;
        
        PatchRegistry();

        ConfigurationDatabase threadData;
        threadData.Write("Timeout", (uint32)1000);
        threadService.Initialise(threadData);
        streamerService.Initialise(threadData);
        
        if (!tcpServer.Open()) {
            REPORT_ERROR(ErrorManagement::FatalError, "DebugService: Failed to open TCP Server Socket");
            return false;
        }
        if (!tcpServer.Listen(controlPort)) {
            REPORT_ERROR(ErrorManagement::FatalError, "DebugService: Failed to Listen on port %u", controlPort);
            return false;
        }
        printf("[DebugService] TCP Server listening on port %u\n", controlPort);

        if (!udpSocket.Open()) {
            REPORT_ERROR(ErrorManagement::FatalError, "DebugService: Failed to open UDP Socket");
            return false;
        }
        printf("[DebugService] UDP Streamer socket opened\n");

        if (threadService.Start() != ErrorManagement::NoError) {
            REPORT_ERROR(ErrorManagement::FatalError, "DebugService: Failed to start Server thread");
            return false;
        }
        if (streamerService.Start() != ErrorManagement::NoError) {
            REPORT_ERROR(ErrorManagement::FatalError, "DebugService: Failed to start Streamer thread");
            return false;
        }
        printf("[DebugService] Worker threads started.\n");
    }
    
    return true;
}

void PatchItemInternal(const char8* className, ObjectBuilder* builder) {
    ClassRegistryDatabase *db = ClassRegistryDatabase::Instance();
    ClassRegistryItem *item = (ClassRegistryItem*)db->Find(className);
    if (item != NULL_PTR(ClassRegistryItem*)) {
        item->SetObjectBuilder(builder);
    }
}

void DebugService::PatchRegistry() {
    static DebugMemoryMapInputBrokerBuilder b1;
    PatchItemInternal("MemoryMapInputBroker", &b1);
    static DebugMemoryMapOutputBrokerBuilder b2;
    PatchItemInternal("MemoryMapOutputBroker", &b2);
    static DebugMemoryMapSynchronisedInputBrokerBuilder b3;
    PatchItemInternal("MemoryMapSynchronisedInputBroker", &b3);
    static DebugMemoryMapSynchronisedOutputBrokerBuilder b4;
    PatchItemInternal("MemoryMapSynchronisedOutputBroker", &b4);
    static DebugMemoryMapInterpolatedInputBrokerBuilder b5;
    PatchItemInternal("MemoryMapInterpolatedInputBroker", &b5);
    static DebugMemoryMapMultiBufferInputBrokerBuilder b6;
    PatchItemInternal("MemoryMapMultiBufferInputBroker", &b6);
    static DebugMemoryMapMultiBufferOutputBrokerBuilder b7;
    PatchItemInternal("MemoryMapMultiBufferOutputBroker", &b7);
    static DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder b8;
    PatchItemInternal("MemoryMapSynchronisedMultiBufferInputBroker", &b8);
    static DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder b9;
    PatchItemInternal("MemoryMapSynchronisedMultiBufferOutputBroker", &b9);

}

void DebugService::ProcessSignal(DebugSignalInfo* s, uint32 size) {
    if (s != NULL_PTR(DebugSignalInfo*)) {
        if (s->isForcing) {
            MemoryOperationsHelper::Copy(s->memoryAddress, s->forcedValue, size);
        }
        if (s->isTracing) {
            if (s->decimationFactor <= 1) {
                (void)traceBuffer.Push(s->internalID, s->memoryAddress, size);
            }
            else {
                if (s->decimationCounter == 0) {
                    (void)traceBuffer.Push(s->internalID, s->memoryAddress, size);
                    s->decimationCounter = s->decimationFactor - 1;
                }
                else {
                    s->decimationCounter--;
                }
            }
        }
    }
}

DebugSignalInfo* DebugService::RegisterSignal(void* memoryAddress, TypeDescriptor type, const char8* name) {
    mutex.FastLock();
    DebugSignalInfo* res = NULL_PTR(DebugSignalInfo*);
    
    uint32 sigIdx = 0xFFFFFFFF;
    for(uint32 i=0; i<numberOfSignals; i++) {
        if(signals[i].memoryAddress == memoryAddress) {
            res = &signals[i];
            sigIdx = i;
            break;
        }
    }
    
    if (res == NULL_PTR(DebugSignalInfo*) && numberOfSignals < MAX_SIGNALS) {
        sigIdx = numberOfSignals;
        res = &signals[numberOfSignals];
        res->memoryAddress = memoryAddress;
        res->type = type;
        res->name = name; 
        res->isTracing = false;
        res->isForcing = false;
        res->internalID = numberOfSignals;
        res->decimationFactor = 1;
        res->decimationCounter = 0;
        numberOfSignals++;
    }

    if (sigIdx != 0xFFFFFFFF && numberOfAliases < MAX_ALIASES) {
        bool foundAlias = false;
        for (uint32 i=0; i<numberOfAliases; i++) {
            if (aliases[i].name == name) { foundAlias = true; break; }
        }
        if (!foundAlias) {
            aliases[numberOfAliases].name = name;
            aliases[numberOfAliases].signalIndex = sigIdx;
            numberOfAliases++;
        }
    }

    mutex.FastUnLock();
    return res;
}

static bool RecursiveGetFullObjectName(ReferenceContainer *container, const Object &obj, StreamString &path) {
    uint32 size = container->Size();
    for (uint32 i=0; i<size; i++) {
        Reference child = container->Get(i);
        if (child.IsValid()) {
            if (child.operator->() == &obj) {
                path = child->GetName();
                return true;
            }
            ReferenceContainer *inner = dynamic_cast<ReferenceContainer*>(child.operator->());
            if (inner) {
                if (RecursiveGetFullObjectName(inner, obj, path)) {
                    StreamString prefix = child->GetName();
                    prefix += ".";
                    prefix += path;
                    path = prefix;
                    return true;
                }
            }
        }
    }
    return false;
}

bool DebugService::GetFullObjectName(const Object &obj, StreamString &fullPath) {
    fullPath = "";
    if (RecursiveGetFullObjectName(ObjectRegistryDatabase::Instance(), obj, fullPath)) {
        return true;
    }
    return false;
}

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

ErrorManagement::ErrorType DebugService::Server(ExecutionInfo & info) {
    if (info.GetStage() == ExecutionInfo::TerminationStage) return ErrorManagement::NoError;
    if (info.GetStage() == ExecutionInfo::StartupStage) {
        serverThreadId = Threads::Id();
        return ErrorManagement::NoError;
    }
    
    while (info.GetStage() == ExecutionInfo::MainStage) {
        BasicTCPSocket *newClient = tcpServer.WaitConnection(1); 
        if (newClient != NULL_PTR(BasicTCPSocket *)) {
            clientsMutex.FastLock();
            bool added = false;
            for (uint32 i=0; i<MAX_CLIENTS; i++) {
                if (activeClients[i] == NULL_PTR(BasicTCPSocket*)) {
                    activeClients[i] = newClient;
                    added = true;
                    break;
                }
            }
            clientsMutex.FastUnLock();
            if (!added) {
                newClient->Close();
                delete newClient;
            }
        }

        for (uint32 i=0; i<MAX_CLIENTS; i++) {
            BasicTCPSocket *client = NULL_PTR(BasicTCPSocket*);
            clientsMutex.FastLock();
            client = activeClients[i];
            clientsMutex.FastUnLock();

            if (client != NULL_PTR(BasicTCPSocket*)) {
                char buffer[1024];
                uint32 size = 1024;
                TimeoutType timeout(0); 
                bool ok = client->Read(buffer, size, timeout);
                
                if (ok && size > 0) {
                    StreamString command;
                    command.Write(buffer, size);
                    HandleCommand(command, client);
                } else if (!ok) {
                    if (!client->IsValid()) {
                        clientsMutex.FastLock();
                        client->Close();
                        delete client;
                        activeClients[i] = NULL_PTR(BasicTCPSocket*);
                        clientsMutex.FastUnLock();
                    }
                }
            }
        }
        Sleep::MSec(10); 
    }
    return ErrorManagement::NoError;
}

ErrorManagement::ErrorType DebugService::Streamer(ExecutionInfo & info) {
    if (info.GetStage() == ExecutionInfo::TerminationStage) return ErrorManagement::NoError;
    if (info.GetStage() == ExecutionInfo::StartupStage) {
        streamerThreadId = Threads::Id();
        return ErrorManagement::NoError;
    }
    
    InternetHost dest(streamPort, streamIP.Buffer());
    udpSocket.SetDestination(dest);

    uint8 packetBuffer[4096];
    uint32 packetOffset = 0;
    uint32 sequenceNumber = 0;

    while (info.GetStage() == ExecutionInfo::MainStage) {
        uint32 id;
        uint32 size;
        uint8 sampleData[1024]; 
        bool hasData = false;

        while ((info.GetStage() == ExecutionInfo::MainStage) && traceBuffer.Pop(id, sampleData, size, 1024)) {
            hasData = true;
            if (packetOffset == 0) {
                TraceHeader header;
                header.magic = 0xDA7A57AD;
                header.seq = sequenceNumber++;
                header.timestamp = HighResolutionTimer::Counter();
                header.count = 0;
                MemoryOperationsHelper::Copy(packetBuffer, &header, sizeof(TraceHeader));
                packetOffset = sizeof(TraceHeader);
            }

            if (packetOffset + 8 + size > 1400) {
                uint32 toWrite = packetOffset;
                udpSocket.Write((char8*)packetBuffer, toWrite);
                packetOffset = 0;
                TraceHeader header;
                header.magic = 0xDA7A57AD;
                header.seq = sequenceNumber++;
                header.timestamp = HighResolutionTimer::Counter();
                header.count = 0;
                MemoryOperationsHelper::Copy(packetBuffer, &header, sizeof(TraceHeader));
                packetOffset = sizeof(TraceHeader);
            }

            MemoryOperationsHelper::Copy(&packetBuffer[packetOffset], &id, 4);
            MemoryOperationsHelper::Copy(&packetBuffer[packetOffset + 4], &size, 4);
            MemoryOperationsHelper::Copy(&packetBuffer[packetOffset + 8], sampleData, size);
            packetOffset += (8 + size);
            TraceHeader *h = (TraceHeader*)packetBuffer;
            h->count++;
        }

        if (packetOffset > 0) {
            uint32 toWrite = packetOffset;
            udpSocket.Write((char8*)packetBuffer, toWrite);
            packetOffset = 0;
        }
        if (!hasData) Sleep::MSec(1); 
    }
    return ErrorManagement::NoError;
}

static bool SuffixMatch(const char8* target, const char8* pattern) {
    uint32 tLen = StringHelper::Length(target);
    uint32 pLen = StringHelper::Length(pattern);
    if (pLen > tLen) return false;
    const char8* suffix = target + (tLen - pLen);
    if (StringHelper::Compare(suffix, pattern) == 0) {
        if (tLen == pLen || *(suffix - 1) == '.') return true;
    }
    return false;
}

void DebugService::HandleCommand(StreamString cmd, BasicTCPSocket *client) {
    StreamString token;
    cmd.Seek(0);
    char8 term;
    const char8* delims = " \r\n";
    if (cmd.GetToken(token, delims, term)) {
        if (token == "FORCE") {
            StreamString name, val;
            if (cmd.GetToken(name, delims, term) && cmd.GetToken(val, delims, term)) {
                uint32 count = ForceSignal(name.Buffer(), val.Buffer());
                if (client) { 
                    StreamString resp; resp.Printf("OK FORCE %u\n", count);
                    uint32 s = resp.Size(); client->Write(resp.Buffer(), s); 
                }
            }
        }
        else if (token == "UNFORCE") {
            StreamString name;
            if (cmd.GetToken(name, delims, term)) {
                uint32 count = UnforceSignal(name.Buffer());
                if (client) { 
                    StreamString resp; resp.Printf("OK UNFORCE %u\n", count);
                    uint32 s = resp.Size(); client->Write(resp.Buffer(), s); 
                }
            }
        }
        else if (token == "TRACE") {
            StreamString name, state, decim;
            if (cmd.GetToken(name, delims, term) && cmd.GetToken(state, delims, term)) {
                bool enable = (state == "1");
                uint32 d = 1;
                if (cmd.GetToken(decim, delims, term)) {
                    AnyType decimVal(UnsignedInteger32Bit, 0u, &d);
                    AnyType decimStr(CharString, 0u, decim.Buffer());
                    TypeConvert(decimVal, decimStr);
                }
                uint32 count = TraceSignal(name.Buffer(), enable, d);
                if (client) { 
                    StreamString resp; resp.Printf("OK TRACE %u\n", count);
                    uint32 s = resp.Size(); client->Write(resp.Buffer(), s); 
                }
            }
        }
        else if (token == "DISCOVER") Discover(client);
        else if (token == "PAUSE") {
            SetPaused(true);
            if (client) { uint32 s = 3; client->Write("OK\n", s); }
        }
        else if (token == "RESUME") {
            SetPaused(false);
            if (client) { uint32 s = 3; client->Write("OK\n", s); }
        }
        else if (token == "TREE") {
            StreamString json;
            json = "{\"Name\": \"Root\", \"Class\": \"ObjectRegistryDatabase\", \"Children\": [\n";
            ExportTree(ObjectRegistryDatabase::Instance(), json);
            json += "\n]}\nOK TREE\n";
            uint32 s = json.Size();
            client->Write(json.Buffer(), s);
        }
        else if (token == "INFO") {
            StreamString path;
            if (cmd.GetToken(path, delims, term)) InfoNode(path.Buffer(), client);
        }
        else if (token == "LS") {
            StreamString path;
            if (cmd.GetToken(path, delims, term)) ListNodes(path.Buffer(), client);
            else ListNodes(NULL_PTR(const char8*), client);
        }
        else if (client) {
            const char* msg = "ERROR: Unknown command\n";
            uint32 s = StringHelper::Length(msg);
            client->Write(msg, s);
        }
    }
}

void DebugService::InfoNode(const char8* path, BasicTCPSocket *client) {
    if (!client) return;
    Reference ref = ObjectRegistryDatabase::Instance()->Find(path);
    StreamString json = "{";
    
    if (ref.IsValid()) {
        json += "\"Name\": \""; EscapeJson(ref->GetName(), json);
        json += "\", \"Class\": \""; EscapeJson(ref->GetClassProperties()->GetName(), json); json += "\"";
        
        ConfigurationDatabase db;
        if (ref->ExportData(db)) {
            json += ", \"Config\": {";
            db.MoveToRoot();
            uint32 nChildren = db.GetNumberOfChildren();
            for (uint32 i=0; i<nChildren; i++) {
                const char8* cname = db.GetChildName(i);
                AnyType at = db.GetType(cname);
                char8 valBuf[1024];
                AnyType strType(CharString, 0u, valBuf);
                strType.SetNumberOfElements(0, 1024);
                if (TypeConvert(strType, at)) {
                    json += "\""; EscapeJson(cname, json); json += "\": \"";
                    EscapeJson(valBuf, json); json += "\"";
                    if (i < nChildren - 1) json += ", ";
                }
            }
            json += "}";
        }
    } else {
        mutex.FastLock();
        bool found = false;
        for (uint32 i=0; i<numberOfAliases; i++) {
            if (aliases[i].name == path || SuffixMatch(aliases[i].name.Buffer(), path)) {
                DebugSignalInfo &s = signals[aliases[i].signalIndex];
                const char8* tname = TypeDescriptor::GetTypeNameFromTypeDescriptor(s.type);
                json.Printf("\"Name\": \"%s\", \"Class\": \"Signal\", \"Type\": \"%s\", \"ID\": %d", 
                            s.name.Buffer(), tname ? tname : "Unknown", s.internalID);
                found = true;
                break;
            }
        }
        mutex.FastUnLock();
        if (!found) json += "\"Error\": \"Object not found\"";
    }
    
    json += "}\nOK INFO\n";
    uint32 s = json.Size();
    client->Write(json.Buffer(), s);
}

uint32 DebugService::ExportTree(ReferenceContainer *container, StreamString &json) {
    if (container == NULL_PTR(ReferenceContainer*)) return 0;
    uint32 size = container->Size();
    uint32 validCount = 0;
    for (uint32 i = 0u; i < size; i++) {
        Reference child = container->Get(i);
        if (child.IsValid()) {
            if (validCount > 0u) json += ",\n";
            
            StreamString nodeJson;
            const char8* cname = child->GetName();
            if (cname == NULL_PTR(const char8*)) cname = "unnamed";
            const char8* clsname = child->GetClassProperties()->GetName();

            nodeJson += "{\"Name\": \""; EscapeJson(cname, nodeJson);
            nodeJson += "\", \"Class\": \""; EscapeJson(clsname, nodeJson); nodeJson += "\"";

            ReferenceContainer *inner = dynamic_cast<ReferenceContainer*>(child.operator->());
            DataSourceI *ds = dynamic_cast<DataSourceI*>(child.operator->());
            GAM *gam = dynamic_cast<GAM*>(child.operator->());

            if ((inner != NULL_PTR(ReferenceContainer*)) || (ds != NULL_PTR(DataSourceI*)) || (gam != NULL_PTR(GAM*))) {
                nodeJson += ", \"Children\": [\n";
                uint32 subCount = 0u;
                if (inner != NULL_PTR(ReferenceContainer*)) subCount += ExportTree(inner, nodeJson);
                
                if (ds != NULL_PTR(DataSourceI*)) {
                    uint32 nSignals = ds->GetNumberOfSignals();
                    for (uint32 j = 0u; j < nSignals; j++) {
                        if (subCount > 0u) nodeJson += ",\n";
                        subCount++;
                        StreamString sname; (void)ds->GetSignalName(j, sname);
                        const char8* stype = TypeDescriptor::GetTypeNameFromTypeDescriptor(ds->GetSignalType(j));
                        uint8 dims = 0u; (void)ds->GetSignalNumberOfDimensions(j, dims);
                        uint32 elems = 0u; (void)ds->GetSignalNumberOfElements(j, elems);

                        nodeJson += "{\"Name\": \""; EscapeJson(sname.Buffer(), nodeJson);
                        nodeJson += "\", \"Class\": \"Signal\", \"Type\": \""; EscapeJson(stype ? stype : "Unknown", nodeJson);
                        nodeJson.Printf("\", \"Dimensions\": %d, \"Elements\": %u}", dims, elems);
                    }
                }

                if (gam != NULL_PTR(GAM*)) {
                    uint32 nIn = gam->GetNumberOfInputSignals();
                    for (uint32 j = 0u; j < nIn; j++) {
                        if (subCount > 0u) nodeJson += ",\n";
                        subCount++;
                        StreamString sname; (void)gam->GetSignalName(InputSignals, j, sname);
                        const char8* stype = TypeDescriptor::GetTypeNameFromTypeDescriptor(gam->GetSignalType(InputSignals, j));
                        uint32 dims = 0u; (void)gam->GetSignalNumberOfDimensions(InputSignals, j, dims);
                        uint32 elems = 0u; (void)gam->GetSignalNumberOfElements(InputSignals, j, elems);

                        nodeJson += "{\"Name\": \"In."; EscapeJson(sname.Buffer(), nodeJson);
                        nodeJson += "\", \"Class\": \"InputSignal\", \"Type\": \""; EscapeJson(stype ? stype : "Unknown", nodeJson);
                        nodeJson.Printf("\", \"Dimensions\": %u, \"Elements\": %u}", dims, elems);
                    }
                    uint32 nOut = gam->GetNumberOfOutputSignals();
                    for (uint32 j = 0u; j < nOut; j++) {
                        if (subCount > 0u) nodeJson += ",\n";
                        subCount++;
                        StreamString sname; (void)gam->GetSignalName(OutputSignals, j, sname);
                        const char8* stype = TypeDescriptor::GetTypeNameFromTypeDescriptor(gam->GetSignalType(OutputSignals, j));
                        uint32 dims = 0u; (void)gam->GetSignalNumberOfDimensions(OutputSignals, j, dims);
                        uint32 elems = 0u; (void)gam->GetSignalNumberOfElements(OutputSignals, j, elems);

                        nodeJson += "{\"Name\": \"Out."; EscapeJson(sname.Buffer(), nodeJson);
                        nodeJson += "\", \"Class\": \"OutputSignal\", \"Type\": \""; EscapeJson(stype ? stype : "Unknown", nodeJson);
                        nodeJson.Printf("\", \"Dimensions\": %u, \"Elements\": %u}", dims, elems);
                    }
                }
                nodeJson += "\n]";
            }
            nodeJson += "}";
            json += nodeJson;
            validCount++;
        }
    }
    return validCount;
}

uint32 DebugService::ForceSignal(const char8* name, const char8* valueStr) {
    mutex.FastLock();
    uint32 count = 0;
    for (uint32 i = 0; i < numberOfAliases; i++) {
        if (aliases[i].name == name || SuffixMatch(aliases[i].name.Buffer(), name)) {
            DebugSignalInfo &s = signals[aliases[i].signalIndex];
            s.isForcing = true;
            AnyType dest(s.type, 0u, s.forcedValue);
            AnyType source(CharString, 0u, valueStr);
            (void)TypeConvert(dest, source);
            count++;
        }
    }
    mutex.FastUnLock();
    return count;
}

uint32 DebugService::UnforceSignal(const char8* name) {
    mutex.FastLock();
    uint32 count = 0;
    for (uint32 i = 0; i < numberOfAliases; i++) {
        if (aliases[i].name == name || SuffixMatch(aliases[i].name.Buffer(), name)) {
            signals[aliases[i].signalIndex].isForcing = false;
            count++;
        }
    }
    mutex.FastUnLock();
    return count;
}

uint32 DebugService::TraceSignal(const char8* name, bool enable, uint32 decimation) {
    mutex.FastLock();
    uint32 count = 0;
    for (uint32 i = 0; i < numberOfAliases; i++) {
        if (aliases[i].name == name || SuffixMatch(aliases[i].name.Buffer(), name)) {
            DebugSignalInfo &s = signals[aliases[i].signalIndex];
            s.isTracing = enable;
            s.decimationFactor = decimation;
            s.decimationCounter = 0;
            count++;
            printf("[Debug] Tracing state for %s (ID: %u) set to %d\n", aliases[i].name.Buffer(), s.internalID, enable);
        }
    }
    mutex.FastUnLock();
    return count;
}

void DebugService::Discover(BasicTCPSocket *client) {
    if (client) {
        StreamString header = "{\n \"Signals\": [\n";
        uint32 s = header.Size();
        client->Write(header.Buffer(), s);
        mutex.FastLock();
        for (uint32 i = 0; i < numberOfAliases; i++) {
            StreamString line;
            DebugSignalInfo &sig = signals[aliases[i].signalIndex];
            const char8* typeName = TypeDescriptor::GetTypeNameFromTypeDescriptor(sig.type);
            if (typeName == NULL_PTR(const char8*)) typeName = "Unknown";
            line.Printf("  {\"name\": \"%s\", \"id\": %d, \"type\": \"%s\"}", aliases[i].name.Buffer(), sig.internalID, typeName);
            if (i < numberOfAliases - 1) line += ",";
            line += "\n";
            s = line.Size();
            client->Write(line.Buffer(), s);
        }
        mutex.FastUnLock();
        StreamString footer = " ]\n}\nOK DISCOVER\n";
        s = footer.Size();
        client->Write(footer.Buffer(), s);
    }
}

void DebugService::ListNodes(const char8* path, BasicTCPSocket *client) {
    if (!client) return;
    Reference ref;
    if (path == NULL_PTR(const char8*) || StringHelper::Length(path) == 0 || StringHelper::Compare(path, "/") == 0) {
        ref = ObjectRegistryDatabase::Instance();
    } else {
        ref = ObjectRegistryDatabase::Instance()->Find(path);
    }

    if (ref.IsValid()) {
        StreamString header;
        header.Printf("Nodes under %s:\n", path ? path : "/");
        uint32 s = header.Size();
        client->Write(header.Buffer(), s);

        ReferenceContainer *container = dynamic_cast<ReferenceContainer*>(ref.operator->());
        if (container) {
            uint32 size = container->Size();
            for (uint32 i=0; i<size; i++) {
                Reference child = container->Get(i);
                if (child.IsValid()) {
                    StreamString line;
                    line.Printf("  %s [%s]\n", child->GetName(), child->GetClassProperties()->GetName());
                    s = line.Size();
                    client->Write(line.Buffer(), s);
                }
            }
        }

        DataSourceI *ds = dynamic_cast<DataSourceI*>(ref.operator->());
        if (ds) {
            StreamString dsHeader = "  Signals:\n";
            s = dsHeader.Size(); client->Write(dsHeader.Buffer(), s);
            uint32 nSignals = ds->GetNumberOfSignals();
            for (uint32 i=0; i<nSignals; i++) {
                StreamString sname, line;
                ds->GetSignalName(i, sname);
                TypeDescriptor stype = ds->GetSignalType(i);
                const char8* stypeName = TypeDescriptor::GetTypeNameFromTypeDescriptor(stype);
                line.Printf("    %s [%s]\n", sname.Buffer(), stypeName ? stypeName : "Unknown");
                s = line.Size(); client->Write(line.Buffer(), s);
            }
        }

        GAM *gam = dynamic_cast<GAM*>(ref.operator->());
        if (gam) {
            uint32 nIn = gam->GetNumberOfInputSignals();
            uint32 nOut = gam->GetNumberOfOutputSignals();
            StreamString gamHeader;
            gamHeader.Printf("  Input Signals (%d):\n", nIn);
            s = gamHeader.Size(); client->Write(gamHeader.Buffer(), s);
            for (uint32 i=0; i<nIn; i++) {
                StreamString sname, line;
                gam->GetSignalName(InputSignals, i, sname);
                line.Printf("    %s\n", sname.Buffer());
                s = line.Size(); client->Write(line.Buffer(), s);
            }
            gamHeader.SetSize(0);
            gamHeader.Printf("  Output Signals (%d):\n", nOut);
            s = gamHeader.Size(); client->Write(gamHeader.Buffer(), s);
            for (uint32 i=0; i<nOut; i++) {
                StreamString sname, line;
                gam->GetSignalName(OutputSignals, i, sname);
                line.Printf("    %s\n", sname.Buffer());
                s = line.Size(); client->Write(line.Buffer(), s);
            }
        }

        const char* okMsg = "OK LS\n";
        s = StringHelper::Length(okMsg);
        client->Write(okMsg, s);
    } else {
        const char* msg = "ERROR: Path not found\n";
        uint32 s = StringHelper::Length(msg);
        client->Write(msg, s);
    }
}

}
