#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 "ObjectBuilder.h"
#include "ObjectRegistryDatabase.h"
#include "StreamString.h"
#include "TimeoutType.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;
  activeClient = NULL_PTR(BasicTCPSocket *);
}

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];
  }
  this->Purge();
}

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

  // Try to capture full configuration autonomously if data is a
  // ConfigurationDatabase
  ConfigurationDatabase *cdb = dynamic_cast<ConfigurationDatabase *>(&data);
  if (cdb != NULL_PTR(ConfigurationDatabase *)) {
    // Save current position
    StreamString currentPath;
    // In MARTe2 ConfigurationDatabase there isn't a direct GetCurrentPath,
    // but we can at least try to copy from root if we are at root.
    // For now, we rely on explicit SetFullConfig or documentary injection.
  }

  // Copy local branch as fallback
  (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;

    if (logPort > 0) {
      Reference tcpLogger(
          "TcpLogger", GlobalObjectsDatabase::Instance()->GetStandardHeap());
      if (tcpLogger.IsValid()) {
        ConfigurationDatabase loggerConfig;
        loggerConfig.Write("Port", (uint32)logPort);
        if (tcpLogger->Initialise(loggerConfig)) {
          this->Insert(tcpLogger);

          Reference loggerService(
              "LoggerService",
              GlobalObjectsDatabase::Instance()->GetStandardHeap());
          if (loggerService.IsValid()) {
            ConfigurationDatabase serviceConfig;
            serviceConfig.Write("CPUs", (uint32)1);
            ReferenceContainer *lc =
                dynamic_cast<ReferenceContainer *>(loggerService.operator->());
            if (lc != NULL_PTR(ReferenceContainer *)) {
              lc->Insert(tcpLogger);
            }
            if (loggerService->Initialise(serviceConfig)) {
              this->Insert(loggerService);
            }
          }
        }
      }
    }
  }
  return true;
}

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

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) {
  printf("<debug> registering: %s\n", 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;
    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) {
  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;
  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();
  }
}

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) {
    while (activeClient == NULL_PTR(BasicTCPSocket *)) {
      BasicTCPSocket *newClient = tcpServer.WaitConnection(TTInfiniteWait);
      if (newClient != NULL_PTR(BasicTCPSocket *)) {
        // Single connection mode: disconnect any existing client first
        activeClient = newClient;
      }
    }
    // Single connection mode: only check client 0
    {
      if (activeClient != NULL_PTR(BasicTCPSocket *)) {
        // 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
            if (activeClient != NULL_PTR(BasicTCPSocket *)) {
              activeClient->Close();
              delete activeClient;
              activeClient = NULL_PTR(BasicTCPSocket *);
            }
          }
        }
      }
    }
    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());
  (void)udpSocket.SetDestination(dest);
  uint8 packetBuffer[4096];
  uint32 packetOffset = 0;
  uint32 sequenceNumber = 0;
  while (info.GetStage() == ExecutionInfo::MainStage) {
    // 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();

    uint32 id, size;
    uint64 ts;
    uint8 sampleData[1024];
    bool hasData = false;
    while ((info.GetStage() == ExecutionInfo::MainStage) &&
           traceBuffer.Pop(id, ts, sampleData, size, 1024)) {
      hasData = true;
      if (packetOffset == 0) {
        TraceHeader header;
        header.magic = 0xDA7A57AD;
        header.seq = sequenceNumber++;
        header.timestamp = HighResolutionTimer::Counter();
        header.count = 0;
        memcpy(packetBuffer, &header, sizeof(TraceHeader));
        packetOffset = sizeof(TraceHeader);
      }
      if (packetOffset + 16 + size > 1400) {
        uint32 toWrite = packetOffset;
        (void)udpSocket.Write((char8 *)packetBuffer, toWrite);
        TraceHeader header;
        header.magic = 0xDA7A57AD;
        header.seq = sequenceNumber++;
        header.timestamp = HighResolutionTimer::Counter();
        header.count = 0;
        memcpy(packetBuffer, &header, sizeof(TraceHeader));
        packetOffset = sizeof(TraceHeader);
      }
      memcpy(&packetBuffer[packetOffset], &id, 4);
      memcpy(&packetBuffer[packetOffset + 4], &ts, 8);
      memcpy(&packetBuffer[packetOffset + 12], &size, 4);
      memcpy(&packetBuffer[packetOffset + 16], sampleData, size);
      packetOffset += (16 + size);
      ((TraceHeader *)packetBuffer)->count++;
    }
    if (packetOffset > 0) {
      uint32 toWrite = packetOffset;
      (void)udpSocket.Write((char8 *)packetBuffer, toWrite);
      packetOffset = 0;
    }
    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 == "DISCOVER")
      Discover(client);
    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;
  fullConfig.MoveToRoot();

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

    if (fullConfig.MoveRelative(part.Buffer())) {
      // Found exact
    } else {
      bool found = false;
      if (part == "In") {
        if (fullConfig.MoveRelative("InputSignals")) {
          found = true;
        }
      } else if (part == "Out") {
        if (fullConfig.MoveRelative("OutputSignals")) {
          found = true;
        }
      }

      if (!found) {
        StreamString prefixed;
        prefixed.Printf("+%s", part.Buffer());
        if (fullConfig.MoveRelative(prefixed.Buffer())) {
          // Found prefixed
        } else {
          return; // Not found
        }
      }
    }
  }

  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;
  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 {
    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);
        EnrichWithConfig(aliases[i].name.Buffer(), json);
        found = true;
        break;
      }
    }
    mutex.FastUnLock();
    if (!found)
      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;
}

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