#include "BasicTCPSocket.h"
#include "ClassRegistryItem.h"
#include "ConfigurationDatabase.h"
#include "DataSourceI.h"
#include "DebugBrokerWrapper.h"
#include "DebugService.h"
#include "GAM.h"
#include "GlobalObjectsDatabase.h"
#include "HighResolutionTimer.h"
#include "LoggerService.h"
#include "Message.h"
#include "ObjectBuilder.h"
#include "ObjectRegistryDatabase.h"
#include "Threads.h"
#include "Sleep.h"
#include "StreamString.h"
#include "TimeoutType.h"
#include "TcpLogger.h"
#include "TypeConversion.h"
#include "ReferenceT.h"

namespace MARTe {

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

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

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

static bool FindPathInContainer(ReferenceContainer *container,
                                const Object *target, StreamString &path) {
  if (container == NULL_PTR(ReferenceContainer *))
    return false;
  uint32 n = container->Size();
  for (uint32 i = 0u; i < n; i++) {
    Reference ref = container->Get(i);
    if (ref.IsValid()) {
      if (ref.operator->() == target) {
        path = ref->GetName();
        return true;
      }
      ReferenceContainer *sub =
          dynamic_cast<ReferenceContainer *>(ref.operator->());
      if (sub != NULL_PTR(ReferenceContainer *)) {
        if (FindPathInContainer(sub, target, path)) {
          StreamString full;
          full.Printf("%s.%s", ref->GetName(), path.Buffer());
          path = full;
          return true;
        }
      }
    }
  }
  return false;
}

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;
  logPort = 8082;
  streamIP = "127.0.0.1";
  isServer = false;
  suppressTimeoutLogs = true;
  isPaused = false;
  stepRemaining = 0u;
  manualConfigSet = false;
  activeClient = NULL_PTR(BasicTCPSocket *);
  streamerPacketOffset = 0u;
  streamerSequenceNumber = 0u;
}

DebugService::~DebugService() {
  if (instance == this) {
    instance = NULL_PTR(DebugService *);
  }
  threadService.Stop();
  streamerService.Stop();
  tcpServer.Close();
  udpSocket.Close();
  if (activeClient != NULL_PTR(BasicTCPSocket *)) {
    activeClient->Close();
    delete activeClient;
  }
  for (uint32 i = 0; i < signals.Size(); i++) {
    delete signals[i];
  }
}

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

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

  if (controlPort > 0) {
    isServer = true;
    instance = this;
  }

  port = 8081;
  if (data.Read("StreamPort", port)) {
    streamPort = (uint16)port;
  } else {
    (void)data.Read("UdpPort", port);
    streamPort = (uint16)port;
  }
  
  port = 8082;
  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 = 1;
  if (data.Read("SuppressTimeoutLogs", suppress)) {
    suppressTimeoutLogs = (suppress == 1);
  }

  // Do NOT call MoveToRoot() on the shared CDB here — that corrupts the
  // ReferenceContainer::Initialise() traversal cursor for sibling objects.
  // Just capture the local subtree; full config is rebuilt lazily from the
  // live ObjectRegistryDatabase when ServeConfig/EnrichWithConfig is called.
  (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;
    if (!udpSocket.Open())
      return false;
    if (threadService.Start() != ErrorManagement::NoError)
      return false;
    if (streamerService.Start() != ErrorManagement::NoError)
      return false;
  }
  return true;
}

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

  // Check if the ORD already contains a LoggerService with at least one TcpLogger.
  // If so, leave it untouched.
  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++) {
        ReferenceT<TcpLogger> child = rc->Get(i);
        if (child.IsValid()) {
          printf("[DebugService] Found existing TcpLogger in LoggerService — skipping injection.\n");
          return;
        }
      }
    }
  }

  // Build a CDB that mirrors the config-file declaration:
  //   LoggerService node (current position)
  //     Class    = LoggerService
  //     CPUs     = 1
  //     DebugConsumer (child)
  //       Class  = TcpLogger
  //       Port   = <logPort>
  ConfigurationDatabase lsCdb;
  (void)lsCdb.Write("Class", "LoggerService");
  uint32 cpus = 1u;
  (void)lsCdb.Write("CPUs", cpus);
  // ReferenceContainer::Initialise only instantiates children whose names
  // start with '+' (matching the StandardParser convention).
  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()) {
    printf("[DebugService] Failed to create LoggerService object.\n");
    return;
  }
  ls->SetName("LoggerService");
  if (!ls->Initialise(lsCdb)) {
    printf("[DebugService] LoggerService::Initialise() failed.\n");
    return;
  }

  // Insert into the ORD so it is findable by name and cleaned up on shutdown.
  if (!ObjectRegistryDatabase::Instance()->Insert(ls)) {
    printf("[DebugService] Failed to insert LoggerService into ORD.\n");
    // Keep a local reference anyway so the logger thread stays alive.
  }

  // The ORD now holds a reference to ls; the LoggerService stays alive until
  // ObjectRegistryDatabase::Purge().  No need to store a local reference.
  printf("[DebugService] Auto-injected LoggerService + TcpLogger on port %u.\n", logPort);
}

void DebugService::SetFullConfig(ConfigurationDatabase &config) {
  config.MoveToRoot();
  config.Copy(fullConfig);
  manualConfigSet = true;
}

static void BuildCDBFromContainer(ReferenceContainer *container,
                                  ConfigurationDatabase &cdb) {
  if (container == NULL_PTR(ReferenceContainer *))
    return;
  uint32 n = container->Size();
  for (uint32 i = 0u; i < n; i++) {
    Reference child = container->Get(i);
    if (!child.IsValid())
      continue;
    const char8 *name = child->GetName();
    if (name == NULL_PTR(const char8 *))
      continue;

    bool created = cdb.CreateRelative(name);
    if (!created) {
      if (!cdb.MoveRelative(name))
        continue;
    }

    const char8 *className = child->GetClassProperties()->GetName();
    if (className != NULL_PTR(const char8 *))
      (void)cdb.Write("Class", className);

    // Export scalar parameters via a SEPARATE CDB so the live cursor is
    // never touched by ExportData.  Then copy only top-level LEAF values
    // (i.e. scalars where MoveRelative fails) and skip sub-nodes.
    //
    // This avoids two ExportData pitfalls:
    //   1. ReferenceContainer::ExportData writes numeric-indexed child nodes
    //      (+0, +1, ...) — those are sub-nodes and get filtered out.
    //   2. Some DataSource ExportData implementations follow internal
    //      references and write sibling objects as children — also sub-nodes,
    //      also filtered out.
    //
    // Scalar parameters (ControlPort, UdpPort, CPUs, Port, ...) pass through
    // because they are leaf values, not sub-nodes.
    {
      ConfigurationDatabase exportCdb;
      if (child->ExportData(exportCdb)) {
        exportCdb.MoveToRoot();
        uint32 nExport = exportCdb.GetNumberOfChildren();
        for (uint32 j = 0u; j < nExport; j++) {
          const char8 *ek = exportCdb.GetChildName(j);
          if (StringHelper::Compare(ek, "Class") == 0 ||
              StringHelper::Compare(ek, "Name") == 0 ||
              StringHelper::Compare(ek, "IsContainer") == 0)
            continue;
          // Sub-node check: MoveRelative succeeds only for nodes, not scalars
          if (exportCdb.MoveRelative(ek)) {
            exportCdb.MoveToAncestor(1u);
            continue; // skip sub-nodes entirely
          }
          // Leaf scalar — convert to string and write into the main CDB
          AnyType at = exportCdb.GetType(ek);
          if (at.GetDataPointer() != NULL_PTR(void *)) {
            char8 buf[1024];
            AnyType st(CharString, 0u, buf);
            st.SetNumberOfElements(0, 1024);
            if (TypeConvert(st, at)) {
              (void)cdb.Write(ek, buf);
            }
          }
        }
      }
    }

    ReferenceContainer *sub =
        dynamic_cast<ReferenceContainer *>(child.operator->());
    if (sub != NULL_PTR(ReferenceContainer *))
      BuildCDBFromContainer(sub, cdb);

    (void)cdb.MoveToAncestor(1u);
  }
}

void DebugService::RebuildConfigFromRegistry() {
  fullConfig = ConfigurationDatabase();
  BuildCDBFromContainer(ObjectRegistryDatabase::Instance(), fullConfig);

  // ExportData on ReferenceContainer subclasses (including DebugService itself)
  // only writes Name/IsContainer/indexed children — it never re-emits the
  // config-file parameters that were read in Initialise().  Write them back
  // explicitly from the member variables that Initialise() stored.
  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);
    }
  }
}

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

void DebugService::PatchRegistry() {
  DebugMemoryMapInputBrokerBuilder *b1 = new DebugMemoryMapInputBrokerBuilder();
  PatchItemInternal("MemoryMapInputBroker", b1);
  DebugMemoryMapOutputBrokerBuilder *b2 =
      new DebugMemoryMapOutputBrokerBuilder();
  PatchItemInternal("MemoryMapOutputBroker", b2);
  DebugMemoryMapSynchronisedInputBrokerBuilder *b3 =
      new DebugMemoryMapSynchronisedInputBrokerBuilder();
  PatchItemInternal("MemoryMapSynchronisedInputBroker", b3);
  DebugMemoryMapSynchronisedOutputBrokerBuilder *b4 =
      new DebugMemoryMapSynchronisedOutputBrokerBuilder();
  PatchItemInternal("MemoryMapSynchronisedOutputBroker", b4);
  DebugMemoryMapInterpolatedInputBrokerBuilder *b5 =
      new DebugMemoryMapInterpolatedInputBrokerBuilder();
  PatchItemInternal("MemoryMapInterpolatedInputBroker", b5);
  DebugMemoryMapMultiBufferInputBrokerBuilder *b6 =
      new DebugMemoryMapMultiBufferInputBrokerBuilder();
  PatchItemInternal("MemoryMapMultiBufferInputBroker", b6);
  DebugMemoryMapMultiBufferOutputBrokerBuilder *b7 =
      new DebugMemoryMapMultiBufferOutputBrokerBuilder();
  PatchItemInternal("MemoryMapMultiBufferOutputBroker", b7);
  DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder *b8 =
      new DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder();
  PatchItemInternal("MemoryMapSynchronisedMultiBufferInputBroker", b8);
  DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder *b9 =
      new DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder();
  PatchItemInternal("MemoryMapSynchronisedMultiBufferOutputBroker", b9);
  DebugMemoryMapAsyncOutputBrokerBuilder *b10 =
      new DebugMemoryMapAsyncOutputBrokerBuilder();
  PatchItemInternal("MemoryMapAsyncOutputBroker", b10);
  DebugMemoryMapAsyncTriggerOutputBrokerBuilder *b11 =
      new DebugMemoryMapAsyncTriggerOutputBrokerBuilder();
  PatchItemInternal("MemoryMapAsyncTriggerOutputBroker", b11);
}

DebugSignalInfo *DebugService::RegisterSignal(void *memoryAddress,
                                              TypeDescriptor type,
                                              const char8 *name,
                                              uint8 numberOfDimensions,
                                              uint32 numberOfElements) {
  fprintf(stderr, "<debug> RegisterSignal[%p]: %s\n", (void*)this, name);
  mutex.FastLock();
  DebugSignalInfo *res = NULL_PTR(DebugSignalInfo *);
  uint32 sigIdx = 0xFFFFFFFF;
  for (uint32 i = 0; i < signals.Size(); i++) {
    if (signals[i]->memoryAddress == memoryAddress) {
      res = signals[i];
      sigIdx = i;
      break;
    }
  }
  if (res == NULL_PTR(DebugSignalInfo *)) {
    sigIdx = signals.Size();
    res = new DebugSignalInfo();
    res->memoryAddress = memoryAddress;
    res->type = type;
    res->name = name;
    res->numberOfDimensions = numberOfDimensions;
    res->numberOfElements = numberOfElements;
    res->isTracing = false;
    res->isForcing = false;
    res->internalID = sigIdx;
    res->decimationFactor = 1;
    res->decimationCounter = 0;
    res->breakOp = BREAK_OFF;
    res->breakThreshold = 0.0;
    signals.Push(res);
  }
  if (sigIdx != 0xFFFFFFFF) {
    bool foundAlias = false;
    for (uint32 i = 0; i < aliases.Size(); i++) {
      if (aliases[i].name == name) {
        foundAlias = true;
        break;
      }
    }
    if (!foundAlias) {
      SignalAlias a;
      a.name = name;
      a.signalIndex = sigIdx;
      aliases.Push(a);
    }
  }
  mutex.FastUnLock();
  return res;
}

void DebugService::ProcessSignal(DebugSignalInfo *signalInfo, uint32 size,
                                 uint64 timestamp) {
  if (signalInfo == NULL_PTR(DebugSignalInfo *))
    return;
  if (signalInfo->isForcing) {
    memcpy(signalInfo->memoryAddress, signalInfo->forcedValue, size);
  }
  if (signalInfo->isTracing) {
    if (signalInfo->decimationCounter == 0) {
      traceBuffer.Push(signalInfo->internalID, timestamp,
                       (uint8 *)signalInfo->memoryAddress, size);
    }
    signalInfo->decimationCounter =
        (signalInfo->decimationCounter + 1) % signalInfo->decimationFactor;
  }
}

void DebugService::RegisterBroker(DebugSignalInfo **signalPointers,
                                  uint32 numSignals, MemoryMapBroker *broker,
                                  volatile bool *anyActiveFlag,
                                  Vec<uint32> *activeIndices,
                                  Vec<uint32> *activeSizes,
                                  FastPollingMutexSem *activeMutex,
                                  volatile bool *anyBreakFlag,
                                  Vec<uint32> *breakIndices,
                                  const char8 *gamName, bool isOutput) {
  mutex.FastLock();
  BrokerInfo b;
  b.signalPointers = signalPointers;
  b.numSignals = numSignals;
  b.broker = broker;
  b.anyActiveFlag = anyActiveFlag;
  b.activeIndices = activeIndices;
  b.activeSizes = activeSizes;
  b.activeMutex = activeMutex;
  b.anyBreakFlag = anyBreakFlag;
  b.breakIndices = breakIndices;
  b.isOutput = isOutput;
  if (gamName != NULL_PTR(const char8 *))
    b.gamName = gamName;
  brokers.Push(b);
  mutex.FastUnLock();
}

void DebugService::UpdateBrokersActiveStatus() {
  for (uint32 i = 0; i < brokers.Size(); i++) {
    uint32 count = 0;
    for (uint32 j = 0; j < brokers[i].numSignals; j++) {
      DebugSignalInfo *s = brokers[i].signalPointers[j];
      if (s != NULL_PTR(DebugSignalInfo *) && (s->isTracing || s->isForcing)) {
        count++;
      }
    }

    Vec<uint32> tempInd;
    Vec<uint32> tempSizes;
    for (uint32 j = 0; j < brokers[i].numSignals; j++) {
      DebugSignalInfo *s = brokers[i].signalPointers[j];
      if (s != NULL_PTR(DebugSignalInfo *) && (s->isTracing || s->isForcing)) {
        tempInd.Push(j);
        tempSizes.Push((brokers[i].broker != NULL_PTR(MemoryMapBroker *))
                           ? brokers[i].broker->GetCopyByteSize(j)
                           : 4);
      }
    }

    if (brokers[i].activeMutex)
      brokers[i].activeMutex->FastLock();

    if (brokers[i].activeIndices)
      *(brokers[i].activeIndices) = tempInd;
    if (brokers[i].activeSizes)
      *(brokers[i].activeSizes) = tempSizes;
    if (brokers[i].anyActiveFlag)
      *(brokers[i].anyActiveFlag) = (count > 0);

    if (brokers[i].activeMutex)
      brokers[i].activeMutex->FastUnLock();
  }
}

void DebugService::UpdateBrokersBreakStatus() {
  for (uint32 i = 0; i < brokers.Size(); i++) {
    Vec<uint32> tempBreak;
    uint32 count = 0;
    for (uint32 j = 0; j < brokers[i].numSignals; j++) {
      DebugSignalInfo *s = brokers[i].signalPointers[j];
      if (s != NULL_PTR(DebugSignalInfo *) && s->breakOp != BREAK_OFF) {
        tempBreak.Push(j);
        count++;
      }
    }
    if (brokers[i].activeMutex)
      brokers[i].activeMutex->FastLock();
    if (brokers[i].breakIndices)
      *(brokers[i].breakIndices) = tempBreak;
    if (brokers[i].anyBreakFlag)
      *(brokers[i].anyBreakFlag) = (count > 0);
    if (brokers[i].activeMutex)
      brokers[i].activeMutex->FastUnLock();
  }
}

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

ErrorManagement::ErrorType DebugService::HandleMessage(ReferenceT<Message> &data) {
  printf("<debug> DebugService received custom message: Function=%s\n", (const char8*)data->GetFunction());
  return ErrorManagement::NoError;
}

ErrorManagement::ErrorType DebugService::Server(ExecutionInfo &info) {
  if (info.GetStage() == ExecutionInfo::TerminationStage)
    return ErrorManagement::NoError;
  if (info.GetStage() == ExecutionInfo::StartupStage) {
    serverThreadId = Threads::Id();
    // Wait for ObjectRegistryDatabase::Initialise() to finish processing all
    // sibling objects (LoggerService etc. come after DebugService in the config).
    // 500 ms is well above any realistic initialisation time.
    Sleep::MSec(500u);
    InjectTcpLoggerIfNeeded();
    return ErrorManagement::NoError;
  }
  // The MARTe2 framework calls Execute() in a loop; each call should do
  // one unit of work and return so the framework can check for Stop().
  // This replaces the old internal infinite-while pattern.
  if (activeClient == NULL_PTR(BasicTCPSocket *)) {
    // Wait briefly for a new connection; return so the framework loop can
    // check if Stop() was requested between calls.
    BasicTCPSocket *newClient = tcpServer.WaitConnection(TimeoutType(100));
    if (newClient != NULL_PTR(BasicTCPSocket *)) {
      activeClient = newClient;
    }
  } else {
    // Check if client is still connected
    if (!activeClient->IsConnected()) {
      activeClient->Close();
      delete activeClient;
      activeClient = NULL_PTR(BasicTCPSocket *);
    } else {
      char buffer[1024];
      uint32 size = 1024;
      if (activeClient->Read(buffer, size)) {
        if (size > 0) {
          // Process each line separately
          char *ptr = buffer;
          char *end = buffer + size;
          while (ptr < end) {
            char *newline = (char *)memchr(ptr, '\n', end - ptr);
            if (!newline) {
              break;
            }
            *newline = '\0';
            // Skip carriage return if present
            if (newline > ptr && *(newline - 1) == '\r')
              *(newline - 1) = '\0';
            StreamString command;
            uint32 len = (uint32)(newline - ptr);
            command.Write(ptr, len);
            if (command.Size() > 0) {
              HandleCommand(command, activeClient);
            }
            ptr = newline + 1;
          }
        }
      } else {
        // Read failed (client disconnected or error), clean up
        activeClient->Close();
        delete activeClient;
        activeClient = NULL_PTR(BasicTCPSocket *);
      }
    }
  }
  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;
  }
  // Set UDP destination (idempotent, called each Execute() invocation)
  InternetHost dest(streamPort, streamIP.Buffer());
  (void)udpSocket.SetDestination(dest);

  // Poll monitored signals
  uint64 currentTimeMs = (uint64)((float64)HighResolutionTimer::Counter() *
                                  HighResolutionTimer::Period() * 1000.0);
  mutex.FastLock();
  for (uint32 i = 0; i < monitoredSignals.Size(); i++) {
    if (currentTimeMs >= (monitoredSignals[i].lastPollTime + monitoredSignals[i].periodMs)) {
      monitoredSignals[i].lastPollTime = currentTimeMs;
      uint64 ts = (uint64)((float64)HighResolutionTimer::Counter() *
                           HighResolutionTimer::Period() * 1000000.0);
      void *address = NULL_PTR(void *);
      if (monitoredSignals[i].dataSource->GetSignalMemoryBuffer(monitoredSignals[i].signalIdx, 0, address)) {
        traceBuffer.Push(monitoredSignals[i].internalID, ts, (uint8 *)address, monitoredSignals[i].size);
      }
    }
  }
  mutex.FastUnLock();

  // Drain ring buffer into UDP packet(s)
  uint32 id, size;
  uint64 ts;
  uint8 sampleData[1024];
  bool hasData = false;
  while (traceBuffer.Pop(id, ts, sampleData, size, 1024)) {
    hasData = true;
    if (streamerPacketOffset == 0u) {
      TraceHeader header;
      header.magic = 0xDA7A57AD;
      header.seq = streamerSequenceNumber++;
      header.timestamp = HighResolutionTimer::Counter();
      header.count = 0;
      memcpy(streamerPacketBuffer, &header, sizeof(TraceHeader));
      streamerPacketOffset = sizeof(TraceHeader);
    }
    if (streamerPacketOffset + 16u + size > 1400u) {
      uint32 toWrite = streamerPacketOffset;
      (void)udpSocket.Write((char8 *)streamerPacketBuffer, toWrite);
      TraceHeader header;
      header.magic = 0xDA7A57AD;
      header.seq = streamerSequenceNumber++;
      header.timestamp = HighResolutionTimer::Counter();
      header.count = 0;
      memcpy(streamerPacketBuffer, &header, sizeof(TraceHeader));
      streamerPacketOffset = sizeof(TraceHeader);
    }
    memcpy(&streamerPacketBuffer[streamerPacketOffset], &id, 4);
    memcpy(&streamerPacketBuffer[streamerPacketOffset + 4], &ts, 8);
    memcpy(&streamerPacketBuffer[streamerPacketOffset + 12], &size, 4);
    memcpy(&streamerPacketBuffer[streamerPacketOffset + 16], sampleData, size);
    streamerPacketOffset += (16u + size);
    ((TraceHeader *)streamerPacketBuffer)->count++;
  }
  if (streamerPacketOffset > 0u) {
    uint32 toWrite = streamerPacketOffset;
    (void)udpSocket.Write((char8 *)streamerPacketBuffer, toWrite);
    streamerPacketOffset = 0u;
  }
  if (!hasData) {
    Sleep::MSec(1);
  }
  return ErrorManagement::NoError;
}

bool DebugService::GetFullObjectName(const Object &obj,
                                     StreamString &fullPath) {
  fullPath = "";
  if (FindPathInContainer(ObjectRegistryDatabase::Instance(), &obj, fullPath)) {
    return true;
  }
  const char8 *name = obj.GetName();
  if (name != NULL_PTR(const char8 *))
    fullPath = name;
  return true;
}

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();
          (void)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();
          (void)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());
          (void)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();
          (void)client->Write(resp.Buffer(), s);
        }
      }
    } else if (token == "STEP") {
      StreamString nStr;
      uint32 n = 1u;
      if (cmd.GetToken(nStr, delims, term)) {
        AnyType nVal(UnsignedInteger32Bit, 0u, &n);
        AnyType nS(CharString, 0u, nStr.Buffer());
        (void)TypeConvert(nVal, nS);
      }
      // Optional thread name: STEP <n> [<threadName>]
      StreamString threadStr;
      const char8 *threadArg = NULL_PTR(const char8 *);
      if (cmd.GetToken(threadStr, delims, term) && threadStr.Size() > 0u) {
        threadArg = threadStr.Buffer();
      }
      Step(n, threadArg);
      if (client) {
        StreamString resp;
        resp.Printf("OK STEP %u\n", n);
        uint32 s = resp.Size();
        (void)client->Write(resp.Buffer(), s);
      }
    } else if (token == "STEP_STATUS") {
      GetStepStatus(client);
    } else if (token == "VALUE") {
      StreamString sigName;
      if (cmd.GetToken(sigName, delims, term)) {
        GetSignalValue(sigName.Buffer(), client);
      } else if (client) {
        const char8 *errResp = "{\"Error\": \"Missing signal name\"}\nOK VALUE\n";
        uint32 s = StringHelper::Length(errResp);
        (void)client->Write(errResp, s);
      }
    } else if (token == "BREAK") {
      // BREAK <signal> <op> <threshold>   — set break condition
      // BREAK <signal> OFF                — clear break condition
      StreamString name, opStr;
      if (cmd.GetToken(name, delims, term) && cmd.GetToken(opStr, delims, term)) {
        uint32 count = 0;
        if (opStr == "OFF") {
          count = ClearBreak(name.Buffer());
        } else {
          StreamString threshStr;
          if (cmd.GetToken(threshStr, delims, term)) {
            uint8 op = BREAK_OFF;
            if      (opStr == ">")  op = BREAK_GT;
            else if (opStr == "<")  op = BREAK_LT;
            else if (opStr == "==") op = BREAK_EQ;
            else if (opStr == ">=") op = BREAK_GEQ;
            else if (opStr == "<=") op = BREAK_LEQ;
            else if (opStr == "!=") op = BREAK_NEQ;
            if (op != BREAK_OFF) {
              float64 threshold = 0.0;
              AnyType thrVal(Float64Bit, 0u, &threshold);
              AnyType thrStr(CharString, 0u, threshStr.Buffer());
              (void)TypeConvert(thrVal, thrStr);
              count = SetBreak(name.Buffer(), op, threshold);
            }
          }
        }
        if (client) {
          StreamString resp;
          resp.Printf("OK BREAK %u\n", count);
          uint32 s = resp.Size();
          (void)client->Write(resp.Buffer(), s);
        }
      }
    } else if (token == "DISCOVER")
      Discover(client);
        else if (token == "MSG") {
          StreamString dest, func, waitStr;
          if (cmd.GetToken(dest, delims, term) &&
              cmd.GetToken(func, delims, term) &&
              cmd.GetToken(waitStr, delims, term)) {
            bool wait = (waitStr == "1");
            
            const char8 *pStart = cmd.Buffer() + cmd.Position();
            StreamString rawPayload = pStart;
    
            // Decode escaped newlines (\n)
            StreamString payload;
            rawPayload.Seek(0u);
            char8 c;
            while (rawPayload.Size() > rawPayload.Position()) {
              uint32 readS = 1;
              if (rawPayload.Read(&c, readS)) {
                if (c == '\\') {
                  char8 next;
                  if (rawPayload.Read(&next, readS)) {
                    if (next == 'n') {
                      payload += '\n';
                    } else {
                      payload += c;
                      payload += next;
                    }
                  } else {
                    payload += c;
                  }
                } else {
                  payload += c;
                }
              }
            }
    
                    ReferenceT<Message> msg(
                        "Message", GlobalObjectsDatabase::Instance()->GetStandardHeap());
                    
                    ConfigurationDatabase msgConfig;
                    msgConfig.Write("Destination", dest.Buffer());
                    msgConfig.Write("Function", func.Buffer());
                    if (wait) {
                        msgConfig.Write("Mode", "ExpectsReply");
                    }
            
                    // Parse payload key=value lines into a ConfigurationDatabase.
                    // ConstantGAM::SetOutput (and similar handlers) expect a
                    // ReferenceT<StructuredDataI> inserted into the Message's
                    // reference container — NOT a sub-node of the message config.
                    ReferenceT<ConfigurationDatabase> paramCdb(
                        "ConfigurationDatabase",
                        GlobalObjectsDatabase::Instance()->GetStandardHeap());

                    if (payload.Size() > 0u && paramCdb.IsValid()) {
                      payload.Seek(0u);
                      StreamString line;
                      while (payload.GetToken(line, "\n", term)) {
                        if (line.Size() > 0u) {
                          const char8 *eq = StringHelper::SearchChar(line.Buffer(), '=');
                          if (eq != NULL_PTR(const char8 *)) {
                            uint32 eqPos = (uint32)(eq - line.Buffer());
                            (void)line.Seek(0u);

                            char8 keyBuf[256] = {'\0'};
                            uint32 keyReadSize = eqPos;
                            (void)line.Read(keyBuf, keyReadSize);

                            (void)line.Seek(eqPos + 1u);
                            uint32 valLen = (uint32)(line.Size() - eqPos - 1u);
                            char8 valBuf[1024] = {'\0'};
                            (void)line.Read(valBuf, valLen);

                            // Trim trailing whitespace from value
                            for (int32 ti = (int32)valLen - 1; ti >= 0; ti--) {
                              if (valBuf[ti] == ' ' || valBuf[ti] == '\r' || valBuf[ti] == '\t')
                                valBuf[ti] = '\0';
                              else
                                break;
                            }

                            StreamString key = keyBuf;
                            key = key.Buffer();  // trim happens via assignment
                            // Trim leading whitespace from key
                            const char8 *kp = keyBuf;
                            while (*kp == ' ' || *kp == '\t') kp++;

                            if (*kp != '\0') {
                              (void)paramCdb->Write(kp, valBuf);
                            }
                          }
                        }
                        line = "";
                      }
                    }

                    ErrorManagement::ErrorType err = ErrorManagement::ParametersError;
                    if (msg->Initialise(msgConfig)) {
                      if (paramCdb.IsValid() && payload.Size() > 0u) {
                        // Insert the CDB as a ReferenceT<StructuredDataI> parameter
                        (void)msg->Insert(paramCdb);
                      }
                                                        // Find destination object in the global database
                                                        Reference destObj = ObjectRegistryDatabase::Instance()->Find(dest.Buffer());
                                                        if (destObj.IsValid()) {
                                                            Object* sender = this;
                                                            StreamString myPath;
                                                            if (!GetFullObjectName(*this, myPath)) {
                                                                sender = NULL_PTR(Object*);
                                                            }

                                                            if (wait) {
                                                              err = MessageI::WaitForReply(msg, TTInfiniteWait);
                                                            } else {
                                                              (void)MessageI::SendMessage(msg, sender);
                                                              // Fire-and-forget: destination found, message sent.
                                                              // Whether the recipient had a matching filter is not
                                                              // reported back to the caller — return OK.
                                                              err = ErrorManagement::NoError;
                                                            }
                                                        } else {
                                                    printf("<debug> MSG: Destination object %s not found in ORD\n", dest.Buffer());
                                                }
                                                
                                                                        if (err != ErrorManagement::NoError) {
                                                                            printf("<debug> MSG: MessageI dispatch failed.\n");
                                                                        }
                                                                                                        } else {
                                                printf("<debug> MSG: Message initialization failed\n");
                                            }
                                    
                                                        if (client) {
                    
              if (err == ErrorManagement::NoError) {
                uint32 okSize = 7;
                (void)client->Write("OK MSG\n", okSize);
              } else {
                uint32 errSize = 10;
                (void)client->Write("ERROR MSG\n", errSize);
              }
            }
          }
        }
     else if (token == "SERVICE_INFO") {
      if (client) {
        StreamString resp;
        resp.Printf("OK SERVICE_INFO TCP_CTRL:%u UDP_STREAM:%u TCP_LOG:%u STATE:%s\n",
                    controlPort, streamPort, logPort, isPaused ? "PAUSED" : "RUNNING");
        uint32 s = resp.Size();
        (void)client->Write(resp.Buffer(), s);
      }
    } else if (token == "MONITOR") {
      StreamString subToken;
      if (cmd.GetToken(subToken, delims, term) && subToken == "SIGNAL") {
        StreamString name, period;
        if (cmd.GetToken(name, delims, term) && cmd.GetToken(period, delims, term)) {
          uint32 p = 100;
          AnyType pVal(UnsignedInteger32Bit, 0u, &p);
          AnyType pStr(CharString, 0u, period.Buffer());
          (void)TypeConvert(pVal, pStr);
          uint32 count = RegisterMonitorSignal(name.Buffer(), p);
          if (client) {
            StreamString resp;
            resp.Printf("OK MONITOR %u\n", count);
            uint32 s = resp.Size();
            (void)client->Write(resp.Buffer(), s);
          }
        }
      }
    } else if (token == "UNMONITOR") {
      StreamString subToken;
      if (cmd.GetToken(subToken, delims, term) && subToken == "SIGNAL") {
        StreamString name;
        if (cmd.GetToken(name, delims, term)) {
          uint32 count = UnmonitorSignal(name.Buffer());
          if (client) {
            StreamString resp;
            resp.Printf("OK UNMONITOR %u\n", count);
            uint32 s = resp.Size();
            (void)client->Write(resp.Buffer(), s);
          }
        }
      }
    } else if (token == "CONFIG")
      ServeConfig(client);
    else if (token == "PAUSE") {
      SetPaused(true);
      if (client) {
        uint32 s = 3;
        (void)client->Write("OK\n", s);
      }
    } else if (token == "RESUME") {
      SetPaused(false);
      if (client) {
        uint32 s = 3;
        (void)client->Write("OK\n", s);
      }
    } else if (token == "TREE") {
      StreamString json;
      json = "{\"Name\": \"Root\", \"Class\": \"ObjectRegistryDatabase\", "
             "\"Children\": [\n";
      (void)ExportTree(ObjectRegistryDatabase::Instance(), json, NULL_PTR(const char8 *));
      json += "\n]}\nOK TREE\n";
      uint32 s = json.Size();
      if (client)
        (void)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);
    }
  }
}

void DebugService::EnrichWithConfig(const char8 *path, StreamString &json) {
  if (path == NULL_PTR(const char8 *))
    return;
  if (!manualConfigSet) {
    RebuildConfigFromRegistry();
  }
  fullConfig.MoveToRoot();

  fprintf(stderr, "[EnrichWithConfig] path=%s\n", path);

  const char8 *current = path;
  bool ok = true;
  while (ok) {
    const char8 *nextDot = StringHelper::SearchString(current, ".");
    StreamString part;
    if (nextDot != NULL_PTR(const char8 *)) {
      uint32 len = (uint32)(nextDot - current);
      (void)part.Write(current, len);
      current = nextDot + 1;
    } else {
      part = current;
      ok = false;
    }

    // Normalise short direction names to both forms so we search consistently.
    // Paths use "In"/"Out" (alias convention); CDBs may store either form.
    bool found = false;

    // 1. Try exact match (bare name - rebuilt-from-registry CDBs use this)
    if (fullConfig.MoveRelative(part.Buffer())) {
      fprintf(stderr, "[EnrichWithConfig] nav exact '%s' OK\n", part.Buffer());
      found = true;
    }

    // 2. Try +name (raw config-file CDBs prefix nodes with '+')
    if (!found) {
      StreamString prefixed;
      prefixed.Printf("+%s", part.Buffer());
      if (fullConfig.MoveRelative(prefixed.Buffer())) {
        fprintf(stderr, "[EnrichWithConfig] nav prefixed '%s' OK\n", prefixed.Buffer());
        found = true;
      }
    }

    // 3. Expand short direction aliases: In -> InputSignals / Out -> OutputSignals
    if (!found) {
      if (part == "In") {
        if (fullConfig.MoveRelative("InputSignals")) {
          fprintf(stderr, "[EnrichWithConfig] nav 'In'->InputSignals OK\n");
          found = true;
        } else if (fullConfig.MoveRelative("+InputSignals")) {
          fprintf(stderr, "[EnrichWithConfig] nav 'In'->+InputSignals OK\n");
          found = true;
        }
      } else if (part == "Out") {
        if (fullConfig.MoveRelative("OutputSignals")) {
          found = true;
        } else if (fullConfig.MoveRelative("+OutputSignals")) {
          found = true;
        }
      }
    }

    if (!found) {
      fprintf(stderr, "[EnrichWithConfig] FAILED at part '%s'\n", part.Buffer());
      fullConfig.MoveToRoot();
      return;
    }
  }

  ConfigurationDatabase db;
  fullConfig.Copy(db);
  fullConfig.MoveToRoot();
  db.MoveToRoot();
  uint32 n = db.GetNumberOfChildren();
  for (uint32 i = 0u; i < n; i++) {
    const char8 *name = db.GetChildName(i);
    AnyType at = db.GetType(name);
    if (!at.GetTypeDescriptor().isStructuredData) {
      json += ", \"";
      EscapeJson(name, json);
      json += "\": \"";
      char8 buf[1024];
      AnyType st(CharString, 0u, buf);
      st.SetNumberOfElements(0, 1024);
      if (TypeConvert(st, at)) {
        EscapeJson(buf, json);
      }
      json += "\"";
    }
  }
}

void DebugService::JsonifyDatabase(ConfigurationDatabase &db,
                                   StreamString &json) {
  json += "{";
  uint32 n = db.GetNumberOfChildren();
  for (uint32 i = 0u; i < n; i++) {
    const char8 *name = db.GetChildName(i);
    json += "\"";
    EscapeJson(name, json);
    json += "\": ";
    if (db.MoveRelative(name)) {
      ConfigurationDatabase child;
      db.Copy(child);
      JsonifyDatabase(child, json);
      db.MoveToAncestor(1u);
    } else {
      AnyType at = db.GetType(name);
      char8 buf[1024];
      AnyType st(CharString, 0u, buf);
      st.SetNumberOfElements(0, 1024);
      if (TypeConvert(st, at)) {
        json += "\"";
        EscapeJson(buf, json);
        json += "\"";
      } else {
        json += "null";
      }
    }
    if (i < n - 1)
      json += ", ";
  }
  json += "}";
}

void DebugService::ServeConfig(BasicTCPSocket *client) {
  if (client == NULL_PTR(BasicTCPSocket *))
    return;

  // If no manual config was injected (test case), rebuild from the live registry.
  // In production, Initialise() only captures the DebugService subtree (to avoid
  // corrupting the shared CDB cursor), so we always need to rebuild here.
  if (!manualConfigSet) {
    RebuildConfigFromRegistry();
  }

  StreamString json;
  fullConfig.MoveToRoot();
  JsonifyDatabase(fullConfig, json);
  json += "\nOK CONFIG\n";
  uint32 s = json.Size();
  (void)client->Write(json.Buffer(), 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 += "}";
    }
    EnrichWithConfig(path, json);
  } else {
    StreamString enrichAlias;
    mutex.FastLock();
    bool found = false;
    for (uint32 i = 0; i < aliases.Size(); 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);
        enrichAlias = aliases[i].name;
        found = true;
        break;
      }
    }
    if (!found) {
      fprintf(stderr, "[InfoNode][%p] signal '%s' NOT found in %u aliases:\n", (void*)this, path, (uint32)aliases.Size());
      for (uint32 i = 0; i < aliases.Size(); i++) {
        fprintf(stderr, "  alias[%u] = '%s'\n", i, aliases[i].name.Buffer());
      }
    }
    mutex.FastUnLock();
    if (found)
      EnrichWithConfig(enrichAlias.Buffer(), json);
    else
      json += "\"Error\": \"Object not found\"";
  }
  json += "}\nOK INFO\n";
  uint32 s = json.Size();
  (void)client->Write(json.Buffer(), s);
}

uint32 DebugService::ExportTree(ReferenceContainer *container,
                                StreamString &json, const char8 *pathPrefix) {
  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";

      StreamString currentPath;
      if (pathPrefix != NULL_PTR(const char8 *)) {
        currentPath.Printf("%s.%s", pathPrefix, cname);
      } else {
        currentPath = cname;
      }

      nodeJson += "{\"Name\": \"";
      EscapeJson(cname, nodeJson);
      nodeJson += "\", \"Class\": \"";
      EscapeJson(child->GetClassProperties()->GetName(), 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, currentPath.Buffer());
        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);

            StreamString signalFullPath;
            signalFullPath.Printf("%s.%s", currentPath.Buffer(), sname.Buffer());
            bool traceable = false;
            bool forcable = false;
            (void)IsInstrumented(signalFullPath.Buffer(), traceable, forcable);

            nodeJson += "{\"Name\": \"";
            EscapeJson(sname.Buffer(), nodeJson);
            nodeJson += "\", \"Class\": \"Signal\", \"Type\": \"";
            EscapeJson(stype ? stype : "Unknown", nodeJson);
            nodeJson.Printf("\", \"Dimensions\": %d, \"Elements\": %u", dims,
                            elems);
            nodeJson.Printf(", \"IsTraceable\": %s, \"IsForcable\": %s}",
                            traceable ? "true" : "false",
                            forcable ? "true" : "false");
          }
        }
        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);

            StreamString signalFullPath;
            signalFullPath.Printf("%s.In.%s", currentPath.Buffer(),
                                  sname.Buffer());
            bool traceable = false;
            bool forcable = false;
            (void)IsInstrumented(signalFullPath.Buffer(), traceable, forcable);

            nodeJson += "{\"Name\": \"In.";
            EscapeJson(sname.Buffer(), nodeJson);
            nodeJson += "\", \"Class\": \"InputSignal\", \"Type\": \"";
            EscapeJson(stype ? stype : "Unknown", nodeJson);
            nodeJson.Printf("\", \"Dimensions\": %u, \"Elements\": %u", dims,
                            elems);
            nodeJson.Printf(", \"IsTraceable\": %s, \"IsForcable\": %s}",
                            traceable ? "true" : "false",
                            forcable ? "true" : "false");
          }
          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);

            StreamString signalFullPath;
            signalFullPath.Printf("%s.Out.%s", currentPath.Buffer(),
                                  sname.Buffer());
            bool traceable = false;
            bool forcable = false;
            (void)IsInstrumented(signalFullPath.Buffer(), traceable, forcable);

            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.Printf(", \"IsTraceable\": %s, \"IsForcable\": %s}",
                            traceable ? "true" : "false",
                            forcable ? "true" : "false");
          }
        }
        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 < aliases.Size(); 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++;
    }
  }
  UpdateBrokersActiveStatus();
  mutex.FastUnLock();
  return count;
}

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

uint32 DebugService::TraceSignal(const char8 *name, bool enable,
                                 uint32 decimation) {
  mutex.FastLock();
  uint32 count = 0;
  for (uint32 i = 0; i < aliases.Size(); i++) {
    printf("<debug>%s\n", aliases[i].name.Buffer());
    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++;
    }
  }
  if (count == 0) {
    printf("<!!> signal %s not found\n", name);
  }
  UpdateBrokersActiveStatus();
  mutex.FastUnLock();
  return count;
}

void DebugService::ConsumeStepIfNeeded(const char8 *gamName,
                                       const char8 *threadName) {
  if (stepRemaining == 0u) return;
  mutex.FastLock();
  // If a thread filter is set, only the matching OS thread consumes step credits.
  if (stepThreadFilter.Size() > 0u &&
      (threadName == NULL_PTR(const char8 *) || stepThreadFilter != threadName)) {
    mutex.FastUnLock();
    return;
  }
  if (stepRemaining > 0u) {
    stepRemaining--;
    if (stepRemaining == 0u) {
      isPaused = true;
      pausedAtGam = (gamName != NULL_PTR(const char8 *)) ? gamName : "";
    }
  }
  mutex.FastUnLock();
}

void DebugService::Step(uint32 n, const char8 *threadName) {
  mutex.FastLock();
  stepRemaining = n;
  isPaused = false;
  stepThreadFilter = (threadName != NULL_PTR(const char8 *)) ? threadName : "";
  mutex.FastUnLock();
}

void DebugService::GetStepStatus(BasicTCPSocket *client) {
  if (client == NULL_PTR(BasicTCPSocket *)) return;
  mutex.FastLock();
  bool paused = isPaused;
  uint32 remaining = stepRemaining;
  StreamString gam = pausedAtGam;
  StreamString threadFilter = stepThreadFilter;
  mutex.FastUnLock();
  StreamString resp;
  resp.Printf("{\"Paused\": %s, \"PausedAtGam\": \"%s\", \"StepRemaining\": %u, \"StepThread\": \"%s\"}\nOK STEP_STATUS\n",
              paused ? "true" : "false",
              gam.Buffer(),
              remaining,
              threadFilter.Buffer());
  uint32 s = resp.Size();
  (void)client->Write(resp.Buffer(), s);
}

void DebugService::GetSignalValue(const char8 *name, BasicTCPSocket *client) {
  if (client == NULL_PTR(BasicTCPSocket *)) return;
  mutex.FastLock();
  DebugSignalInfo *sig = NULL_PTR(DebugSignalInfo *);
  for (uint32 i = 0u; i < aliases.Size(); i++) {
    if (aliases[i].name == name ||
        SuffixMatch(aliases[i].name.Buffer(), name)) {
      sig = signals[aliases[i].signalIndex];
      break;
    }
  }
  if (sig == NULL_PTR(DebugSignalInfo *)) {
    mutex.FastUnLock();
    StreamString resp;
    resp.Printf("{\"Error\": \"Signal not found: %s\"}\nOK VALUE\n", name);
    uint32 s = resp.Size();
    (void)client->Write(resp.Buffer(), s);
    return;
  }
  TypeDescriptor td = sig->type;
  uint32 nElem = sig->numberOfElements;
  uint32 byteSize = (td.numberOfBits > 0u) ? (td.numberOfBits / 8u) : 1u;
  uint32 totalBytes = byteSize * nElem;
  if (totalBytes > 1024u) { totalBytes = 1024u; nElem = totalBytes / byteSize; }
  // Copy bytes while holding the mutex to avoid data races with the RT thread
  uint8 localBuf[1024];
  memset(localBuf, 0, sizeof(localBuf));
  if (sig->memoryAddress != NULL_PTR(void *)) {
    memcpy(localBuf, sig->memoryAddress, totalBytes);
  }
  mutex.FastUnLock();

  // Build the value string via CharString TypeConvert — the same path used by
  // EnrichWithConfig/JsonifyDatabase, which is known to work for all types.
  StreamString valueStr;
  if (nElem > 1u) {
    // Array or matrix: produce comma-separated text
    for (uint32 i = 0u; i < nElem; i++) {
      char8 elemBuf[128] = {'\0'};
      AnyType srcElem(td, 0u, (void *)(localBuf + i * byteSize));
      AnyType dstStr(CharString, 0u, elemBuf);
      dstStr.SetNumberOfElements(0u, 128u);
      (void)TypeConvert(dstStr, srcElem);
      if (i > 0u) valueStr += ", ";
      valueStr += elemBuf;
    }
  } else {
    char8 elemBuf[256] = {'\0'};
    AnyType srcElem(td, 0u, (void *)localBuf);
    AnyType dstStr(CharString, 0u, elemBuf);
    dstStr.SetNumberOfElements(0u, 256u);
    (void)TypeConvert(dstStr, srcElem);
    valueStr = elemBuf;
  }

  StreamString resp;
  resp += "{\"Name\": \"";
  resp += name;
  resp += "\", \"Value\": \"";
  // Escape the value text (quotes inside a string value)
  const char8 *vp = valueStr.Buffer();
  while (vp != NULL_PTR(const char8 *) && *vp != '\0') {
    if (*vp == '"') resp += "\\\"";
    else if (*vp == '\\') resp += "\\\\";
    else { char8 tmp[2] = { *vp, '\0' }; resp += tmp; }
    vp++;
  }
  resp += "\", \"Elements\": ";
  resp.Printf("%u}\nOK VALUE\n", nElem);
  uint32 s = resp.Size();
  (void)client->Write(resp.Buffer(), s);
}

uint32 DebugService::SetBreak(const char8 *name, uint8 op, float64 threshold) {
  mutex.FastLock();
  uint32 count = 0;
  for (uint32 i = 0; i < aliases.Size(); i++) {
    if (aliases[i].name == name ||
        SuffixMatch(aliases[i].name.Buffer(), name)) {
      DebugSignalInfo *s = signals[aliases[i].signalIndex];
      s->breakThreshold = threshold;
      s->breakOp = op;
      count++;
    }
  }
  if (count > 0)
    UpdateBrokersBreakStatus();
  mutex.FastUnLock();
  return count;
}

uint32 DebugService::ClearBreak(const char8 *name) {
  mutex.FastLock();
  uint32 count = 0;
  for (uint32 i = 0; i < aliases.Size(); i++) {
    if (aliases[i].name == name ||
        SuffixMatch(aliases[i].name.Buffer(), name)) {
      signals[aliases[i].signalIndex]->breakOp = BREAK_OFF;
      count++;
    }
  }
  if (count > 0)
    UpdateBrokersBreakStatus();
  mutex.FastUnLock();
  return count;
}

bool DebugService::IsInstrumented(const char8 *fullPath, bool &traceable,
                                 bool &forcable) {
  mutex.FastLock();
  bool found = false;
  for (uint32 i = 0; i < aliases.Size(); i++) {
    if (aliases[i].name == fullPath ||
        SuffixMatch(aliases[i].name.Buffer(), fullPath)) {
      found = true;
      break;
    }
  }
  mutex.FastUnLock();
  traceable = found;
  forcable = found;
  return found;
}

uint32 DebugService::RegisterMonitorSignal(const char8 *path, uint32 periodMs) {
  mutex.FastLock();
  uint32 count = 0;

  // Check if already monitored
  for (uint32 j = 0; j < monitoredSignals.Size(); j++) {
    if (monitoredSignals[j].path == path) {
      monitoredSignals[j].periodMs = periodMs;
      mutex.FastUnLock();
      return 1;
    }
  }

  // Path resolution: find the DataSource object
  StreamString fullPath = path;
  fullPath.Seek(0);
  char8 term;
  Vec<StreamString> parts;
  StreamString token;
  while (fullPath.GetToken(token, ".", term)) {
    parts.Push(token);
    token = "";
  }

  if (parts.Size() >= 2) {
    StreamString signalName = parts[parts.Size() - 1u];
    StreamString dsPath;
    for (uint32 i = 0; i < parts.Size() - 1u; i++) {
      dsPath += parts[i];
      if (i < parts.Size() - 2u)
        dsPath += ".";
    }

    ReferenceT<DataSourceI> ds =
        ObjectRegistryDatabase::Instance()->Find(dsPath.Buffer());
    if (ds.IsValid()) {
      uint32 idx = 0;
      if (ds->GetSignalIndex(idx, signalName.Buffer())) {
        MonitoredSignal m;
        m.dataSource = ds;
        m.signalIdx = idx;
        m.path = path;
        m.periodMs = periodMs;
        m.lastPollTime = 0;
        m.size = 0;
        (void)ds->GetSignalByteSize(idx, m.size);
        if (m.size == 0)
          m.size = 4;

        // Use high-bit for polled signals to avoid conflict with brokered ones
        m.internalID = 0x80000000 | monitoredSignals.Size();
        
        // Re-use existing ID if signal is also instrumented via broker
        for (uint32 i = 0; i < aliases.Size(); i++) {
          if (aliases[i].name == path ||
              SuffixMatch(aliases[i].name.Buffer(), path)) {
            m.internalID = signals[aliases[i].signalIndex]->internalID;
            break;
          }
        }

        monitoredSignals.Push(m);
        count = 1;
      }
    }
  }

  mutex.FastUnLock();
  return count;
}

uint32 DebugService::UnmonitorSignal(const char8 *path) {
  mutex.FastLock();
  uint32 count = 0;
  for (uint32 i = 0; i < monitoredSignals.Size(); i++) {
    if (monitoredSignals[i].path == path ||
        SuffixMatch(monitoredSignals[i].path.Buffer(), path)) {
      (void)monitoredSignals.Remove(i);
      i--;
      count++;
    }
  }
  mutex.FastUnLock();
  return count;
}

void DebugService::Discover(BasicTCPSocket *client) {
  if (client) {
    StreamString header = "{\n \"Signals\": [\n";
    uint32 s = header.Size();
    (void)client->Write(header.Buffer(), s);
    mutex.FastLock();
    uint32 total = 0;
    for (uint32 i = 0; i < aliases.Size(); i++) {
      if (total > 0) {
        uint32 commaSize = 2;
        (void)client->Write(",\n", commaSize);
      }
      StreamString line;
      DebugSignalInfo *sig = signals[aliases[i].signalIndex];
      const char8 *typeName =
          TypeDescriptor::GetTypeNameFromTypeDescriptor(sig->type);
      line.Printf("  {\"name\": \"%s\", \"id\": %d, \"type\": \"%s\", \"dimensions\": %u, \"elements\": %u",
                  aliases[i].name.Buffer(), sig->internalID,
                  typeName ? typeName : "Unknown", sig->numberOfDimensions, sig->numberOfElements);
      EnrichWithConfig(aliases[i].name.Buffer(), line);
      line += "}";
      s = line.Size();
      (void)client->Write(line.Buffer(), s);
      total++;
    }

    // Export monitored signals not already in aliases
    for (uint32 i = 0; i < monitoredSignals.Size(); i++) {
      bool found = false;
      for (uint32 j = 0; j < aliases.Size(); j++) {
        if (aliases[j].name == monitoredSignals[i].path) {
          found = true;
          break;
        }
      }
      if (!found) {
        if (total > 0) {
          uint32 commaSize = 2;
          (void)client->Write(",\n", commaSize);
        }
        StreamString line;
        const char8 *typeName = TypeDescriptor::GetTypeNameFromTypeDescriptor(
            monitoredSignals[i].dataSource->GetSignalType(
                monitoredSignals[i].signalIdx));
        
        uint8 dims = 0;
        uint32 elems = 1;
        (void)monitoredSignals[i].dataSource->GetSignalNumberOfDimensions(monitoredSignals[i].signalIdx, dims);
        (void)monitoredSignals[i].dataSource->GetSignalNumberOfElements(monitoredSignals[i].signalIdx, elems);

        line.Printf("  {\"name\": \"%s\", \"id\": %u, \"type\": \"%s\", \"dimensions\": %u, \"elements\": %u",
                    monitoredSignals[i].path.Buffer(),
                    monitoredSignals[i].internalID,
                    typeName ? typeName : "Unknown", dims, elems);
        EnrichWithConfig(monitoredSignals[i].path.Buffer(), line);
        line += "}";
        s = line.Size();
        (void)client->Write(line.Buffer(), s);
        total++;
      }
    }
    mutex.FastUnLock();
    StreamString footer = " ]\n}\nOK DISCOVER\n";
    s = footer.Size();
    (void)client->Write(footer.Buffer(), s);
  }
}

void DebugService::ListNodes(const char8 *path, BasicTCPSocket *client) {
  if (!client)
    return;
  Reference ref =
      (path == NULL_PTR(const char8 *) || StringHelper::Length(path) == 0 ||
       StringHelper::Compare(path, "/") == 0)
          ? ObjectRegistryDatabase::Instance()
          : ObjectRegistryDatabase::Instance()->Find(path);
  if (ref.IsValid()) {
    StreamString out;
    out.Printf("Nodes under %s:\n", path ? path : "/");
    ReferenceContainer *container =
        dynamic_cast<ReferenceContainer *>(ref.operator->());
    if (container) {
      for (uint32 i = 0; i < container->Size(); i++) {
        Reference child = container->Get(i);
        if (child.IsValid())
          out.Printf("  %s [%s]\n", child->GetName(),
                     child->GetClassProperties()->GetName());
      }
    }
    const char *okMsg = "OK LS\n";
    out += okMsg;
    uint32 s = out.Size();
    (void)client->Write(out.Buffer(), s);
  } else {
    const char *msg = "ERROR: Path not found\n";
    uint32 s = StringHelper::Length(msg);
    (void)client->Write(msg, s);
  }
}

} // namespace MARTe