#include "AdvancedErrorManagement.h"
#include "Atomic.h"
#include "ClassRegistryItem.h"
#include "ConfigurationDatabase.h"
#include "DataSourceI.h"
#include "DebugBrokerWrapper.h"
#include "DebugServiceBase.h"
#include "GAM.h"
#include "Message.h"
#include "MessageI.h"
#include "ObjectRegistryDatabase.h"
#include "ReferenceT.h"
#include "StreamString.h"
#include "TimeoutType.h"
#include "TypeConversion.h"

namespace MARTe {

// DebugServiceI static member — defined here (only once, shared by all
// transports).
DebugServiceI *DebugServiceI::instance = NULL_PTR(DebugServiceI *);

// ---------------------------------------------------------------------------
// DebugServiceI::GetFullObjectName — static utility used by broker wrappers
// ---------------------------------------------------------------------------

bool DebugServiceI::GetFullObjectName(const Object &obj,
                                      StreamString &fullPath) {
  fullPath = "";
  // FindPathInContainer is a file-scope static below; forward-declare it here.
  // The actual definition appears after this function.
  extern bool DSB_FindPath(ReferenceContainer *, const Object *,
                           StreamString &);
  if (DSB_FindPath(ObjectRegistryDatabase::Instance(), &obj, fullPath)) {
    return true;
  }
  const char8 *name = obj.GetName();
  if (name != NULL_PTR(const char8 *))
    fullPath = name;
  return true;
}

// ---------------------------------------------------------------------------
// File-scope helper functions (NOT exported)
// ---------------------------------------------------------------------------

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

// Returns true if `str` ends with `suffix` at a word boundary (i.e. the char
// immediately before the match is '.', or the match covers the entire string).
static bool SuffixMatch(const char8 *str, const char8 *suffix) {
  uint32 sLen   = StringHelper::Length(str);
  uint32 sufLen = StringHelper::Length(suffix);
  if (sufLen > sLen)
    return false;
  const char8 *tail = str + (sLen - sufLen);
  if (StringHelper::Compare(tail, suffix) != 0)
    return false;
  return (sLen == sufLen || *(tail - 1u) == '.');
}

// Match a stored alias against a user-supplied name.
// Three cases are tried in order:
//   1. Exact match         ("App.DS.Sig"  == "App.DS.Sig")
//   2. Alias is longer     (alias "App.DS.Sig" ends with user name "Sig")
//      → user used a short / unqualified name in the command.
//   3. User name is longer (user name "App.GAM.In.Sig" ends with alias "GAM.In.Sig")
//      → alias was registered with a short fallback path (FindByNameRecursive
//        failed during broker init) but the client uses the full ORD path.
static bool AliasMatch(const char8 *aliasName, const char8 *userName) {
  if (StringHelper::Compare(aliasName, userName) == 0)
    return true;
  if (SuffixMatch(aliasName, userName))   // case 2: alias ⊇ userName
    return true;
  if (SuffixMatch(userName, aliasName))   // case 3: userName ⊇ alias
    return true;
  return false;
}

// Named with DSB_ prefix so GetFullObjectName can forward-declare it.
bool DSB_FindPath(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())
      continue;
    if (ref.operator->() == target) {
      path = ref->GetName();
      return true;
    }
    ReferenceContainer *sub =
        dynamic_cast<ReferenceContainer *>(ref.operator->());
    if (sub != NULL_PTR(ReferenceContainer *)) {
      if (DSB_FindPath(sub, target, path)) {
        StreamString full;
        full.Printf("%s.%s", ref->GetName(), path.Buffer());
        path = full;
        return true;
      }
    }
  }
  return false;
}

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 *cn = child->GetClassProperties()->GetName();
    if (cn != NULL_PTR(const char8 *))
      (void)cdb.Write("Class", cn);

    {
      ConfigurationDatabase exp;
      if (child->ExportData(exp)) {
        exp.MoveToRoot();
        uint32 ne = exp.GetNumberOfChildren();
        for (uint32 j = 0u; j < ne; j++) {
          const char8 *ek = exp.GetChildName(j);
          if (StringHelper::Compare(ek, "Class") == 0 ||
              StringHelper::Compare(ek, "Name") == 0 ||
              StringHelper::Compare(ek, "IsContainer") == 0)
            continue;
          if (exp.MoveRelative(ek)) {
            exp.MoveToAncestor(1u);
            continue;
          }
          AnyType at = exp.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 *rc =
        dynamic_cast<ReferenceContainer *>(child.operator->());
    if (rc != NULL_PTR(ReferenceContainer *))
      BuildCDBFromContainer(rc, cdb);
    (void)cdb.MoveToAncestor(1u);
  }
}

/* HI-8: Guard against double-patching (e.g. two DebugService instances).
 * Once the registry has been patched, subsequent PatchRegistry() calls are
 * no-ops.  Original builders are not saved/restored — the debug wrappers
 * persist for the process lifetime (intentional for transparent debugging). */
static bool registryPatched = false;

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

// ---------------------------------------------------------------------------
// DebugServiceBase constructor / destructor
// ---------------------------------------------------------------------------

DebugServiceBase::DebugServiceBase() : ReferenceContainer() {
  isPaused = false;
  stepRemaining = 0u;
  manualConfigSet = false;
  discoverCacheValid = false;
  treeCacheValid = false;
}

DebugServiceBase::~DebugServiceBase() {
  for (uint32 i = 0u; i < signals.GetNumberOfElements(); i++) {
    if (signals[i] != NULL_PTR(DebugSignalInfo *)) {
      delete signals[i];
    }
  }
}

// ---------------------------------------------------------------------------
// PatchRegistry
// ---------------------------------------------------------------------------

void DebugServiceBase::PatchRegistry() {
  /* HI-8: skip if already patched (prevents double-patch leak when multiple
   * DebugService instances are created). */
  if (registryPatched) {
    REPORT_ERROR_STATIC(ErrorManagement::Warning,
        "PatchRegistry: registry already patched — skipping (double-patch guard).");
    return;
  }
  registryPatched = true;
  PatchItemInternal("MemoryMapInputBroker",
                    new DebugMemoryMapInputBrokerBuilder());
  PatchItemInternal("MemoryMapOutputBroker",
                    new DebugMemoryMapOutputBrokerBuilder());
  PatchItemInternal("MemoryMapSynchronisedInputBroker",
                    new DebugMemoryMapSynchronisedInputBrokerBuilder());
  PatchItemInternal("MemoryMapSynchronisedOutputBroker",
                    new DebugMemoryMapSynchronisedOutputBrokerBuilder());
  PatchItemInternal("MemoryMapInterpolatedInputBroker",
                    new DebugMemoryMapInterpolatedInputBrokerBuilder());
  PatchItemInternal("MemoryMapMultiBufferInputBroker",
                    new DebugMemoryMapMultiBufferInputBrokerBuilder());
  PatchItemInternal("MemoryMapMultiBufferOutputBroker",
                    new DebugMemoryMapMultiBufferOutputBrokerBuilder());
  PatchItemInternal(
      "MemoryMapSynchronisedMultiBufferInputBroker",
      new DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder());
  PatchItemInternal(
      "MemoryMapSynchronisedMultiBufferOutputBroker",
      new DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder());
  PatchItemInternal("MemoryMapAsyncOutputBroker",
                    new DebugMemoryMapAsyncOutputBrokerBuilder());
  PatchItemInternal("MemoryMapAsyncTriggerOutputBroker",
                    new DebugMemoryMapAsyncTriggerOutputBrokerBuilder());
}

// ---------------------------------------------------------------------------
// RT-path implementations
// ---------------------------------------------------------------------------

DebugSignalInfo *DebugServiceBase::RegisterSignal(void *memoryAddress,
                                                  TypeDescriptor type,
                                                  const char8 *name,
                                                  uint8 numberOfDimensions,
                                                  uint32 numberOfElements) {
  mutex.FastLock();
  DebugSignalInfo *res = NULL_PTR(DebugSignalInfo *);
  uint32 sigIdx = 0xFFFFFFFFu;
  for (uint32 i = 0u; i < signals.GetNumberOfElements(); i++) {
    if (signals[i]->memoryAddress == memoryAddress) {
      res = signals[i];
      sigIdx = i;
      break;
    }
  }
  if (res == NULL_PTR(DebugSignalInfo *)) {
    sigIdx = signals.GetNumberOfElements();
    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 = 1u;
    res->decimationCounter = 0;
    res->breakOp = BREAK_OFF;
    res->breakThreshold = 0.0;
    signals.Append(res);
    // Invalidate caches — new signal registered after the last build.
    discoverCacheValid = false;
    treeCacheValid     = false;
  }
  if (sigIdx != 0xFFFFFFFFu) {
    bool found = false;
    for (uint32 i = 0u; i < aliases.GetNumberOfElements(); i++) {
      if (aliases[i].name == name) {
        found = true;
        break;
      }
    }
    if (!found) {
      SignalAlias a;
      a.name = name;
      a.signalIndex = sigIdx;
      aliases.Append(a);
    }
  }
  mutex.FastUnLock();
  return res;
}

void DebugServiceBase::ProcessSignal(DebugSignalInfo *signalInfo, uint32 size,
                                     uint64 timestamp) {
  if (signalInfo == NULL_PTR(DebugSignalInfo *))
    return;
  if (signalInfo->isForcing) {
    uint32 nEl = signalInfo->numberOfElements;
    /* HI-4: clamp size to forcedValue buffer to prevent OOB read */
    uint32 forceSize = size;
    if (forceSize > static_cast<uint32>(sizeof(signalInfo->forcedValue))) {
      forceSize = static_cast<uint32>(sizeof(signalInfo->forcedValue));
    }
    if (nEl <= 1u) {
      // Scalar — single memcpy (clamped to forcedValue bounds).
      memcpy(signalInfo->memoryAddress, signalInfo->forcedValue, forceSize);
    } else {
      // Array — copy only the elements whose bit is set in forcedMask.
      // HI-4: cap loop at 256 elements (forcedMask is 32 bytes = 256 bits).
      uint32 elemBytes = forceSize / nEl;
      uint32 nElCapped = (nEl > 256u) ? 256u : nEl;
      for (uint32 e = 0u; e < nElCapped; e++) {
        if (signalInfo->forcedMask[e >> 3u] & (uint8)(1u << (e & 7u))) {
          memcpy((uint8 *)signalInfo->memoryAddress + e * elemBytes,
                 signalInfo->forcedValue             + e * elemBytes,
                 elemBytes);
        }
      }
    }
  }
  if (signalInfo->isTracing) {
    Atomic::Add((volatile int32 *)&signalInfo->decimationCounter, 1);
    if ((uint32)signalInfo->decimationCounter >= signalInfo->decimationFactor) {
      int32 old =
          Atomic::Exchange((volatile int32 *)&signalInfo->decimationCounter, 0);
      if ((uint32)old >= signalInfo->decimationFactor) {
        tracePushMutex.FastLock();
        traceBuffer.Push(signalInfo->internalID, timestamp,
                         (uint8 *)signalInfo->memoryAddress, size);
        tracePushMutex.FastUnLock();
      }
    }
  }
}

void DebugServiceBase::RegisterBroker(
    DebugSignalInfo **signalPointers, uint32 numSignals,
    MemoryMapBroker *broker, volatile bool *anyActiveFlag,
    Vector<uint32> *activeIndices, Vector<uint32> *activeSizes,
    FastPollingMutexSem *activeMutex, volatile bool *anyBreakFlag,
    Vector<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.Append(b);
  mutex.FastUnLock();
}

void DebugServiceBase::ConsumeStepIfNeeded(const char8 *gamName,
                                           const char8 *threadName) {
  if (stepRemaining == 0u)
    return;
  mutex.FastLock();
  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();
}

// ---------------------------------------------------------------------------
// Control-path implementations
// ---------------------------------------------------------------------------

uint32 DebugServiceBase::ForceSignal(const char8 *name, const char8 *valueStr) {
  // Parse optional array-element suffix: "SignalName[idx]" → baseName + elemIdx.
  // elemIdx == -1 means "force all elements".
  const char8 *bracketPos = StringHelper::SearchChar(name, '[');
  StreamString baseName;
  int32        elemIdx = -1;

  if (bracketPos != NULL_PTR(const char8 *)) {
    uint32 baseLen = (uint32)(bracketPos - name);
    (void)baseName.Write(name, baseLen);
    const char8 *closePos =
        StringHelper::SearchChar(bracketPos + 1u, ']');
    if (closePos != NULL_PTR(const char8 *)) {
      uint32 idxLen = (uint32)(closePos - bracketPos - 1u);
      StreamString idxStr;
      (void)idxStr.Write(bracketPos + 1u, idxLen);
      uint32 parsed = 0u;
      AnyType iv(UnsignedInteger32Bit, 0u, &parsed);
      AnyType is_src(CharString, 0u, idxStr.Buffer());
      if (TypeConvert(iv, is_src))
        elemIdx = (int32)parsed;
    }
  } else {
    baseName = name;
  }
  const char8 *effectiveName = baseName.Buffer();

  mutex.FastLock();
  uint32 count = 0u;
  for (uint32 i = 0u; i < aliases.GetNumberOfElements(); i++) {
    if (AliasMatch(aliases[i].name.Buffer(), effectiveName)) {
      DebugSignalInfo *s         = signals[aliases[i].signalIndex];
      uint32           elemBytes = (uint32)(s->type.numberOfBits) / 8u;
      uint32           totalBytes = elemBytes * s->numberOfElements;
      if (elemBytes == 0u || totalBytes > (uint32)sizeof(s->forcedValue)) {
        REPORT_ERROR_STATIC(
            ErrorManagement::Warning,
            "ForceSignal: signal '%s' requires %u bytes but forcedValue "
            "buffer is only %u bytes — force request ignored.",
            s->name.Buffer(), totalBytes, (uint32)sizeof(s->forcedValue));
        continue;
      }
      s->isForcing = true;

      if (elemIdx >= 0 && (uint32)elemIdx < s->numberOfElements) {
        // Force a single element — write to its slot and set its mask bit.
        uint8 *dest = (uint8 *)s->forcedValue + (uint32)elemIdx * elemBytes;
        AnyType dv(s->type, 0u, (void *)dest);
        AnyType sv(CharString, 0u, valueStr);
        (void)TypeConvert(dv, sv);
        s->forcedMask[(uint32)elemIdx >> 3u] |= (uint8)(1u << ((uint32)elemIdx & 7u));
      } else {
        // Force all elements to the same value — convert once, replicate, set all bits.
        AnyType dv(s->type, 0u, s->forcedValue);
        AnyType sv(CharString, 0u, valueStr);
        (void)TypeConvert(dv, sv);
        for (uint32 k = 1u; k < s->numberOfElements; k++) {
          memcpy(s->forcedValue + k * elemBytes, s->forcedValue, elemBytes);
        }
        uint32 fullBytes = s->numberOfElements / 8u;
        uint32 remBits   = s->numberOfElements % 8u;
        memset(s->forcedMask, 0xFF, fullBytes);
        if (remBits > 0u) s->forcedMask[fullBytes] = (uint8)((1u << remBits) - 1u);
      }
      count++;
    }
  }
  UpdateBrokersActiveStatus();
  mutex.FastUnLock();
  return count;
}

uint32 DebugServiceBase::UnforceSignal(const char8 *name) {
  // Parse optional "[idx]" suffix.  If present, clear only that element's bit;
  // if absent, clear all bits and the isForcing flag entirely.
  const char8 *bracketPos = StringHelper::SearchChar(name, '[');
  StreamString baseName;
  int32 elemIdx = -1;

  if (bracketPos != NULL_PTR(const char8 *)) {
    uint32 baseLen = (uint32)(bracketPos - name);
    (void)baseName.Write(name, baseLen);
    const char8 *closePos = StringHelper::SearchChar(bracketPos + 1u, ']');
    if (closePos != NULL_PTR(const char8 *)) {
      uint32 idxLen = (uint32)(closePos - bracketPos - 1u);
      StreamString idxStr;
      (void)idxStr.Write(bracketPos + 1u, idxLen);
      uint32 parsed = 0u;
      AnyType iv(UnsignedInteger32Bit, 0u, &parsed);
      AnyType is_src(CharString, 0u, idxStr.Buffer());
      if (TypeConvert(iv, is_src)) elemIdx = (int32)parsed;
    }
  } else {
    baseName = name;
  }
  const char8 *effectiveName = baseName.Buffer();

  mutex.FastLock();
  uint32 count = 0u;
  for (uint32 i = 0u; i < aliases.GetNumberOfElements(); i++) {
    if (AliasMatch(aliases[i].name.Buffer(), effectiveName)) {
      DebugSignalInfo *s = signals[aliases[i].signalIndex];
      if (elemIdx >= 0 && (uint32)elemIdx < s->numberOfElements) {
        // Clear just this element's force bit.
        s->forcedMask[(uint32)elemIdx >> 3u] &= (uint8)(~(1u << ((uint32)elemIdx & 7u)));
        // Deactivate forcing entirely if no bits remain.
        uint32 numBytes = (s->numberOfElements + 7u) / 8u;
        bool anySet = false;
        for (uint32 b = 0u; b < numBytes && !anySet; b++) anySet = (s->forcedMask[b] != 0u);
        if (!anySet) s->isForcing = false;
      } else {
        // Unforce all elements.
        memset(s->forcedMask, 0, sizeof(s->forcedMask));
        s->isForcing = false;
      }
      count++;
    }
  }
  UpdateBrokersActiveStatus();
  mutex.FastUnLock();
  return count;
}

uint32 DebugServiceBase::TraceSignal(const char8 *name, bool enable,
                                     uint32 decimation) {
  mutex.FastLock();
  uint32 count = 0u;
  for (uint32 i = 0u; i < aliases.GetNumberOfElements(); i++) {
    if (AliasMatch(aliases[i].name.Buffer(), name)) {
      DebugSignalInfo *s = signals[aliases[i].signalIndex];
      s->isTracing = enable;
      s->decimationFactor = decimation;
      s->decimationCounter = 0;
      count++;
    }
  }
  UpdateBrokersActiveStatus();
  mutex.FastUnLock();
  return count;
}

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

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

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

uint32 DebugServiceBase::RegisterMonitorSignal(const char8 *path,
                                               uint32 periodMs) {
  mutex.FastLock();

  // If already monitored, just update period
  for (uint32 j = 0u; j < monitoredSignals.GetNumberOfElements(); j++) {
    if (monitoredSignals[j].path == path) {
      monitoredSignals[j].periodMs = periodMs;
      mutex.FastUnLock();
      return 1u;
    }
  }

  // Tokenise path to separate DataSource path from signal name
  StreamString fullPath = path;
  fullPath.Seek(0u);
  char8 term;
  Vector<StreamString> parts;
  StreamString token;
  while (fullPath.GetToken(token, ".", term)) {
    parts.Append(token);
    token = "";
  }

  uint32 count = 0u;
  if (parts.GetNumberOfElements() >= 2u) {
    StreamString signalName = parts[parts.GetNumberOfElements() - 1u];
    StreamString dsPath;
    for (uint32 i = 0u; i < parts.GetNumberOfElements() - 1u; i++) {
      dsPath += parts[i];
      if (i < parts.GetNumberOfElements() - 2u)
        dsPath += ".";
    }
    ReferenceT<DataSourceI> ds =
        ObjectRegistryDatabase::Instance()->Find(dsPath.Buffer());
    if (ds.IsValid()) {
      uint32 idx = 0u;
      if (ds->GetSignalIndex(idx, signalName.Buffer())) {
        MonitoredSignal m;
        m.dataSource = ds;
        m.signalIdx = idx;
        m.path = path;
        m.periodMs = periodMs;
        m.lastPollTime = 0u;
        m.size = 0u;
        (void)ds->GetSignalByteSize(idx, m.size);
        if (m.size == 0u)
          m.size = 4u;
        m.internalID = 0x80000000u | monitoredSignals.GetNumberOfElements();
        // Re-use existing brokered signal ID if available
        for (uint32 i = 0u; i < aliases.GetNumberOfElements(); i++) {
          if (AliasMatch(aliases[i].name.Buffer(), path)) {
            m.internalID = signals[aliases[i].signalIndex]->internalID;
            break;
          }
        }
        monitoredSignals.Append(m);
        count = 1u;
      }
    }
  }
  mutex.FastUnLock();
  return count;
}

uint32 DebugServiceBase::UnmonitorSignal(const char8 *path) {
  mutex.FastLock();
  uint32 count = 0u;
  for (uint32 i = 0u; i < monitoredSignals.GetNumberOfElements(); ) {
    if (monitoredSignals[i].path == path ||
        SuffixMatch(monitoredSignals[i].path.Buffer(), path)) {
      // Shift elements after i one position left, then shrink by one.
      uint32 n = monitoredSignals.GetNumberOfElements();
      for (uint32 k = i; k + 1u < n; k++) {
        monitoredSignals[k] = monitoredSignals[k + 1u];
      }
      monitoredSignals.SetSize(n - 1u);
      count++;
      // Do NOT increment i — re-examine the element now at position i.
    } else {
      i++;
    }
  }
  mutex.FastUnLock();
  return count;
}

// ---------------------------------------------------------------------------
// Broker index maintenance
// ---------------------------------------------------------------------------

void DebugServiceBase::UpdateBrokersActiveStatus() {
  for (uint32 i = 0u; i < brokers.GetNumberOfElements(); i++) {
    if (brokers[i].signalPointers == NULL_PTR(DebugSignalInfo **))
      continue;
    uint32 count = 0u;
    Vector<uint32> tempInd, tempSizes;
    for (uint32 j = 0u; j < brokers[i].numSignals; j++) {
      DebugSignalInfo *s = brokers[i].signalPointers[j];
      if (s != NULL_PTR(DebugSignalInfo *) && (s->isTracing || s->isForcing)) {
        tempInd.Append(j);
        tempSizes.Append((brokers[i].broker != NULL_PTR(MemoryMapBroker *))
                           ? brokers[i].broker->GetCopyByteSize(j)
                           : 4u);
        count++;
      }
    }
    if (brokers[i].activeMutex)
      brokers[i].activeMutex->FastLock();
    if (brokers[i].activeIndices) {
      brokers[i].activeIndices->SetSize(0u);
      for (uint32 k = 0u; k < tempInd.GetNumberOfElements(); k++)
        brokers[i].activeIndices->Append(tempInd[k]);
    }
    if (brokers[i].activeSizes) {
      brokers[i].activeSizes->SetSize(0u);
      for (uint32 k = 0u; k < tempSizes.GetNumberOfElements(); k++)
        brokers[i].activeSizes->Append(tempSizes[k]);
    }
    if (brokers[i].anyActiveFlag)
      *(brokers[i].anyActiveFlag) = (count > 0u);
    if (brokers[i].activeMutex)
      brokers[i].activeMutex->FastUnLock();
  }
}

void DebugServiceBase::UpdateBrokersBreakStatus() {
  for (uint32 i = 0u; i < brokers.GetNumberOfElements(); i++) {
    if (brokers[i].signalPointers == NULL_PTR(DebugSignalInfo **))
      continue;
    Vector<uint32> tempBreak;
    uint32 count = 0u;
    for (uint32 j = 0u; j < brokers[i].numSignals; j++) {
      DebugSignalInfo *s = brokers[i].signalPointers[j];
      if (s != NULL_PTR(DebugSignalInfo *) && s->breakOp != BREAK_OFF) {
        tempBreak.Append(j);
        count++;
      }
    }
    if (brokers[i].activeMutex)
      brokers[i].activeMutex->FastLock();
    if (brokers[i].breakIndices) {
      brokers[i].breakIndices->SetSize(0u);
      for (uint32 k = 0u; k < tempBreak.GetNumberOfElements(); k++)
        brokers[i].breakIndices->Append(tempBreak[k]);
    }
    if (brokers[i].anyBreakFlag)
      *(brokers[i].anyBreakFlag) = (count > 0u);
    if (brokers[i].activeMutex)
      brokers[i].activeMutex->FastUnLock();
  }
}

// ---------------------------------------------------------------------------
// Step control
// ---------------------------------------------------------------------------

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

// ---------------------------------------------------------------------------
// Command-channel output helpers
// ---------------------------------------------------------------------------

void DebugServiceBase::GetStepStatus(StreamString &out) {
  mutex.FastLock();
  bool paused = isPaused;
  uint32 rem = stepRemaining;
  StreamString gam = pausedAtGam;
  StreamString thr = stepThreadFilter;
  mutex.FastUnLock();
  out.Printf("{\"Paused\":%s,\"PausedAtGam\":\"%s\","
             "\"StepRemaining\":%u,\"StepThread\":\"%s\"}\nOK STEP_STATUS\n",
             paused ? "true" : "false", gam.Buffer(), rem, thr.Buffer());
}

void DebugServiceBase::GetSignalValue(const char8 *name, StreamString &out) {
  mutex.FastLock();
  DebugSignalInfo *sig = NULL_PTR(DebugSignalInfo *);
  for (uint32 i = 0u; i < aliases.GetNumberOfElements(); i++) {
    if (AliasMatch(aliases[i].name.Buffer(), name)) {
      sig = signals[aliases[i].signalIndex];
      break;
    }
  }
  if (sig == NULL_PTR(DebugSignalInfo *)) {
    mutex.FastUnLock();
    out.Printf("{\"Error\":\"Signal not found: %s\"}\nOK VALUE\n", name);
    return;
  }
  TypeDescriptor td = sig->type;
  uint32 nElem = sig->numberOfElements;
  uint32 bySz = (td.numberOfBits > 0u) ? (td.numberOfBits / 8u) : 1u;
  bool trunc = (nElem > GET_VALUE_MAX_ELEMENTS);
  if (trunc)
    nElem = GET_VALUE_MAX_ELEMENTS;
  uint32 total = bySz * nElem;
  if (total > 1024u) {
    total = 1024u;
    nElem = total / bySz;
  }
  uint8 lb[1024];
  memset(lb, 0, sizeof(lb));
  if (sig->memoryAddress != NULL_PTR(void *))
    memcpy(lb, sig->memoryAddress, total);
  mutex.FastUnLock();

  StreamString valueStr;
  if (nElem > 1u) {
    for (uint32 i = 0u; i < nElem; i++) {
      char8 eb[128] = {'\0'};
      AnyType se(td, 0u, (void *)(lb + i * bySz));
      AnyType ds(CharString, 0u, eb);
      ds.SetNumberOfElements(0u, 128u);
      (void)TypeConvert(ds, se);
      if (i > 0u)
        valueStr += ", ";
      valueStr += eb;
    }
  } else {
    char8 eb[256] = {'\0'};
    AnyType se(td, 0u, (void *)lb);
    AnyType ds(CharString, 0u, eb);
    ds.SetNumberOfElements(0u, 256u);
    (void)TypeConvert(ds, se);
    valueStr = eb;
  }

  out += "{\"Name\":\"";
  out += name;
  out += "\",\"Value\":\"";
  const char8 *vp = valueStr.Buffer();
  while (vp != NULL_PTR(const char8 *) && *vp != '\0') {
    if (*vp == '"')
      out += "\\\"";
    else if (*vp == '\\')
      out += "\\\\";
    else {
      char8 tmp[2] = {*vp, '\0'};
      out += tmp;
    }
    vp++;
  }
  out.Printf("\",\"Elements\":%u,\"Truncated\":%s}\nOK VALUE\n", nElem,
             trunc ? "true" : "false");
}

void DebugServiceBase::Discover(StreamString &out) {
  if (!discoverCacheValid)
    BuildDiscoverCache();
  out += discoverCache;
}

void DebugServiceBase::BuildDiscoverCache() {
  discoverCacheValid = false;
  discoverCache      = "";

  // -----------------------------------------------------------------------
  // Phase 1 — snapshot signal metadata under the mutex (fast, no allocs on
  //           the RT path because this runs only during initialisation).
  // -----------------------------------------------------------------------
  Vector<StreamString> enames;
  Vector<uint32>       eids;
  Vector<const char8*> etypes;
  Vector<uint8>        edims;
  Vector<uint32>       eelems;

  mutex.FastLock();
  for (uint32 i = 0u; i < aliases.GetNumberOfElements(); i++) {
    DebugSignalInfo *sig = signals[aliases[i].signalIndex];
    enames.Append(aliases[i].name);
    eids.Append(sig->internalID);
    etypes.Append(TypeDescriptor::GetTypeNameFromTypeDescriptor(sig->type));
    edims.Append(sig->numberOfDimensions);
    eelems.Append(sig->numberOfElements);
  }
  for (uint32 i = 0u; i < monitoredSignals.GetNumberOfElements(); i++) {
    bool found = false;
    for (uint32 j = 0u; j < aliases.GetNumberOfElements(); j++) {
      if (aliases[j].name == monitoredSignals[i].path) { found = true; break; }
    }
    if (!found) {
      uint8  dims  = 0u;
      uint32 elems = 1u;
      (void)monitoredSignals[i].dataSource->GetSignalNumberOfDimensions(
          monitoredSignals[i].signalIdx, dims);
      (void)monitoredSignals[i].dataSource->GetSignalNumberOfElements(
          monitoredSignals[i].signalIdx, elems);
      enames.Append(monitoredSignals[i].path);
      eids.Append(monitoredSignals[i].internalID);
      etypes.Append(TypeDescriptor::GetTypeNameFromTypeDescriptor(
          monitoredSignals[i].dataSource->GetSignalType(
              monitoredSignals[i].signalIdx)));
      edims.Append(dims);
      eelems.Append(elems);
    }
  }
  mutex.FastUnLock();

  // -----------------------------------------------------------------------
  // Phase 2 — build chunked response (no mutex; fullConfig is single-threaded
  //           at initialisation time when this is called from SetFullConfig).
  // -----------------------------------------------------------------------
  uint32 total = enames.GetNumberOfElements();

  if (total == 0u) {
    discoverCache = "{\"Signals\":[]}\nOK DISCOVER\n";
    discoverCacheValid = true;
    return;
  }

  for (uint32 start = 0u; start < total; start += DISCOVER_CHUNK_SIGNALS) {
    uint32 end     = start + DISCOVER_CHUNK_SIGNALS;
    if (end > total) end = total;
    bool   isFinal = (end >= total);

    discoverCache += "{\"Signals\":[\n";
    for (uint32 idx = start; idx < end; idx++) {
      if (idx > start) discoverCache += ",\n";
      discoverCache.Printf(
          "  {\"name\":\"%s\",\"id\":%u,\"type\":\"%s\","
          "\"dimensions\":%u,\"elements\":%u",
          enames[idx].Buffer(), eids[idx],
          etypes[idx] ? etypes[idx] : "Unknown",
          edims[idx], eelems[idx]);
      EnrichWithConfig(enames[idx].Buffer(), discoverCache);
      discoverCache += "}";
    }
    discoverCache += "\n]}";
    discoverCache += isFinal ? "\nOK DISCOVER\n" : "\nOK DISCOVER_PART\n";
  }

  discoverCacheValid = true;
}

void DebugServiceBase::BuildTreeCache() {
  // No-op: tree is now served per-node via ExportTreeNode.
}

// ---------------------------------------------------------------------------
// ExportTreeNode — serve one level of the object tree
// ---------------------------------------------------------------------------

void DebugServiceBase::ExportTreeNode(const char8 *path, StreamString &out) {
  bool isRoot =
      (path == NULL_PTR(const char8 *) || StringHelper::Length(path) == 0);

  out += "{\"Path\":\"";
  EscapeJson(isRoot ? "" : path, out);
  out += "\",\"Children\":[\n";

  uint32 count = 0u;

  ReferenceContainer *container = NULL_PTR(ReferenceContainer *);
  DataSourceI *       ds        = NULL_PTR(DataSourceI *);
  GAM *               gam       = NULL_PTR(GAM *);

  if (isRoot) {
    container = ObjectRegistryDatabase::Instance();
  } else {
    Reference ref = ObjectRegistryDatabase::Instance()->Find(path);
    if (ref.IsValid()) {
      container = dynamic_cast<ReferenceContainer *>(ref.operator->());
      ds        = dynamic_cast<DataSourceI *>(ref.operator->());
      gam       = dynamic_cast<GAM *>(ref.operator->());
    }
  }

  // ReferenceContainer children
  if (container != NULL_PTR(ReferenceContainer *)) {
    uint32 n = container->Size();
    for (uint32 i = 0u; i < n; i++) {
      Reference child = container->Get(i);
      if (!child.IsValid())
        continue;
      const char8 *cname = child->GetName();
      if (cname == NULL_PTR(const char8 *))
        cname = "unnamed";

      ReferenceContainer *irc =
          dynamic_cast<ReferenceContainer *>(child.operator->());
      DataSourceI *ids = dynamic_cast<DataSourceI *>(child.operator->());
      GAM *        igam = dynamic_cast<GAM *>(child.operator->());
      bool hasCh =
          (irc != NULL_PTR(ReferenceContainer *) && irc->Size() > 0u) ||
          ids != NULL_PTR(DataSourceI *) || igam != NULL_PTR(GAM *);

      if (count > 0u)
        out += ",\n";
      out += "{\"Name\":\"";
      EscapeJson(cname, out);
      out += "\",\"Class\":\"";
      EscapeJson(child->GetClassProperties()->GetName(), out);
      out.Printf("\",\"HasChildren\":%s}", hasCh ? "true" : "false");
      count++;
    }
  }

  // DataSourceI signals
  if (ds != NULL_PTR(DataSourceI *)) {
    uint32 ns = ds->GetNumberOfSignals();
    for (uint32 j = 0u; j < ns; j++) {
      StreamString sn;
      (void)ds->GetSignalName(j, sn);
      const char8 *st = TypeDescriptor::GetTypeNameFromTypeDescriptor(
          ds->GetSignalType(j));
      uint8  d  = 0u;
      (void)ds->GetSignalNumberOfDimensions(j, d);
      uint32 el = 0u;
      (void)ds->GetSignalNumberOfElements(j, el);
      StreamString sfp;
      sfp.Printf("%s.%s", path, sn.Buffer());
      bool tr = false, fo = false;
      (void)IsInstrumented(sfp.Buffer(), tr, fo);

      if (count > 0u)
        out += ",\n";
      out += "{\"Name\":\"";
      EscapeJson(sn.Buffer(), out);
      out += "\",\"Class\":\"Signal\",\"Type\":\"";
      EscapeJson(st ? st : "Unknown", out);
      out.Printf("\",\"Dimensions\":%u,\"Elements\":%u,"
                 "\"IsTraceable\":%s,\"IsForcable\":%s,\"HasChildren\":false}",
                 d, el, tr ? "true" : "false", fo ? "true" : "false");
      count++;
    }
  }

  // GAM input signals
  if (gam != NULL_PTR(GAM *)) {
    uint32 nIn = gam->GetNumberOfInputSignals();
    for (uint32 j = 0u; j < nIn; j++) {
      StreamString sn;
      (void)gam->GetSignalName(InputSignals, j, sn);
      const char8 *st = TypeDescriptor::GetTypeNameFromTypeDescriptor(
          gam->GetSignalType(InputSignals, j));
      uint32 d = 0u;
      (void)gam->GetSignalNumberOfDimensions(InputSignals, j, d);
      uint32 el = 0u;
      (void)gam->GetSignalNumberOfElements(InputSignals, j, el);
      StreamString sfp;
      sfp.Printf("%s.In.%s", path, sn.Buffer());
      bool tr = false, fo = false;
      (void)IsInstrumented(sfp.Buffer(), tr, fo);

      if (count > 0u)
        out += ",\n";
      out += "{\"Name\":\"In.";
      EscapeJson(sn.Buffer(), out);
      out += "\",\"Class\":\"InputSignal\",\"Type\":\"";
      EscapeJson(st ? st : "Unknown", out);
      out.Printf("\",\"Dimensions\":%u,\"Elements\":%u,"
                 "\"IsTraceable\":%s,\"IsForcable\":%s,\"HasChildren\":false}",
                 d, el, tr ? "true" : "false", fo ? "true" : "false");
      count++;
    }

    uint32 nOut = gam->GetNumberOfOutputSignals();
    for (uint32 j = 0u; j < nOut; j++) {
      StreamString sn;
      (void)gam->GetSignalName(OutputSignals, j, sn);
      const char8 *st = TypeDescriptor::GetTypeNameFromTypeDescriptor(
          gam->GetSignalType(OutputSignals, j));
      uint32 d = 0u;
      (void)gam->GetSignalNumberOfDimensions(OutputSignals, j, d);
      uint32 el = 0u;
      (void)gam->GetSignalNumberOfElements(OutputSignals, j, el);
      StreamString sfp;
      sfp.Printf("%s.Out.%s", path, sn.Buffer());
      bool tr = false, fo = false;
      (void)IsInstrumented(sfp.Buffer(), tr, fo);

      if (count > 0u)
        out += ",\n";
      out += "{\"Name\":\"Out.";
      EscapeJson(sn.Buffer(), out);
      out += "\",\"Class\":\"OutputSignal\",\"Type\":\"";
      EscapeJson(st ? st : "Unknown", out);
      out.Printf("\",\"Dimensions\":%u,\"Elements\":%u,"
                 "\"IsTraceable\":%s,\"IsForcable\":%s,\"HasChildren\":false}",
                 d, el, tr ? "true" : "false", fo ? "true" : "false");
      count++;
    }
  }

  out += "\n]}\nOK TREE\n";
}

void DebugServiceBase::InfoNode(const char8 *path, StreamString &out) {
  Reference ref = ObjectRegistryDatabase::Instance()->Find(path);
  out += "{";
  if (ref.IsValid()) {
    out += "\"Name\":\"";
    EscapeJson(ref->GetName(), out);
    out += "\",\"Class\":\"";
    EscapeJson(ref->GetClassProperties()->GetName(), out);
    out += "\"";
    ConfigurationDatabase db;
    if (ref->ExportData(db)) {
      out += ",\"Config\":{";
      db.MoveToRoot();
      uint32 nc = db.GetNumberOfChildren();
      for (uint32 i = 0u; i < nc; i++) {
        const char8 *cn = db.GetChildName(i);
        AnyType at = db.GetType(cn);
        char8 buf[1024];
        AnyType st(CharString, 0u, buf);
        st.SetNumberOfElements(0, 1024);
        if (TypeConvert(st, at)) {
          out += "\"";
          EscapeJson(cn, out);
          out += "\":\"";
          EscapeJson(buf, out);
          out += "\"";
          if (i < nc - 1u)
            out += ",";
        }
      }
      out += "}";
    }
    EnrichWithConfig(path, out);
  } else {
    StreamString enrichAlias;
    mutex.FastLock();
    bool found = false;
    for (uint32 i = 0u; i < aliases.GetNumberOfElements(); i++) {
      if (AliasMatch(aliases[i].name.Buffer(), path)) {
        DebugSignalInfo *s = signals[aliases[i].signalIndex];
        const char8 *tn =
            TypeDescriptor::GetTypeNameFromTypeDescriptor(s->type);
        out.Printf("\"Name\":\"%s\",\"Class\":\"Signal\",\"Type\":\"%s\","
                   "\"ID\":%u",
                   s->name.Buffer(), tn ? tn : "Unknown", s->internalID);
        enrichAlias = aliases[i].name;
        found = true;
        break;
      }
    }
    mutex.FastUnLock();
    if (found)
      EnrichWithConfig(enrichAlias.Buffer(), out);
    else
      out += "\"Error\":\"Object not found\"";
  }
  out += "}\nOK INFO\n";
}

void DebugServiceBase::ListNodes(const char8 *path, StreamString &out) {
  Reference ref =
      (path == NULL_PTR(const char8 *) || StringHelper::Length(path) == 0 ||
       StringHelper::Compare(path, "/") == 0)
          ? ObjectRegistryDatabase::Instance()
          : ObjectRegistryDatabase::Instance()->Find(path);
  out.Printf("Nodes under %s:\n", path ? path : "/");
  if (ref.IsValid()) {
    ReferenceContainer *rc =
        dynamic_cast<ReferenceContainer *>(ref.operator->());
    if (rc != NULL_PTR(ReferenceContainer *)) {
      uint32 n = rc->Size();
      for (uint32 i = 0u; i < n; i++) {
        Reference c = rc->Get(i);
        if (c.IsValid()) {
          out.Printf("  %s [%s]\n", c->GetName(),
                     c->GetClassProperties()->GetName());
        }
      }
    }
  } else {
    out += "  (not found)\n";
  }
  out += "OK LS\n";
}

void DebugServiceBase::ServeConfig(StreamString &out) {
  if (!manualConfigSet)
    RebuildConfigFromRegistry();
  fullConfig.MoveToRoot();
  JsonifyDatabase(fullConfig, out);
  out += "\nOK CONFIG\n";
}

// ---------------------------------------------------------------------------
// Config management
// ---------------------------------------------------------------------------

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

void DebugServiceBase::RebuildConfigFromRegistry() {
  fullConfig = ConfigurationDatabase();
  BuildCDBFromContainer(ObjectRegistryDatabase::Instance(), fullConfig);
  // Let the transport subclass write back its specific port numbers etc.
  RebuildTransportConfig();
}

// ---------------------------------------------------------------------------
// Tree export
// ---------------------------------------------------------------------------

uint32 DebugServiceBase::ExportTree(ReferenceContainer *container,
                                    StreamString &json,
                                    const char8 *pathPrefix) {
  if (container == NULL_PTR(ReferenceContainer *))
    return 0u;
  uint32 size = container->Size();
  uint32 valid = 0u;
  for (uint32 i = 0u; i < size; i++) {
    Reference child = container->Get(i);
    if (!child.IsValid())
      continue;
    if (valid > 0u)
      json += ",\n";
    const char8 *cname = child->GetName();
    if (cname == NULL_PTR(const char8 *))
      cname = "unnamed";
    StreamString cp;
    if (pathPrefix != NULL_PTR(const char8 *))
      cp.Printf("%s.%s", pathPrefix, cname);
    else
      cp = cname;

    StreamString nj;
    nj += "{\"Name\":\"";
    EscapeJson(cname, nj);
    nj += "\",\"Class\":\"";
    EscapeJson(child->GetClassProperties()->GetName(), nj);
    nj += "\"";

    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 *)) {
      nj += ",\"Children\":[\n";
      uint32 sc = 0u;
      if (inner != NULL_PTR(ReferenceContainer *))
        sc += ExportTree(inner, nj, cp.Buffer());
      if (ds != NULL_PTR(DataSourceI *)) {
        uint32 ns = ds->GetNumberOfSignals();
        for (uint32 j = 0u; j < ns; j++) {
          if (sc > 0u) {
            nj += ",\n";
          }
          sc++;
          StreamString sn;
          (void)ds->GetSignalName(j, sn);
          const char8 *st = TypeDescriptor::GetTypeNameFromTypeDescriptor(
              ds->GetSignalType(j));
          uint8 d = 0u;
          (void)ds->GetSignalNumberOfDimensions(j, d);
          uint32 el = 0u;
          (void)ds->GetSignalNumberOfElements(j, el);
          StreamString sfp;
          sfp.Printf("%s.%s", cp.Buffer(), sn.Buffer());
          bool tr = false, fo = false;
          (void)IsInstrumented(sfp.Buffer(), tr, fo);
          nj += "{\"Name\":\"";
          EscapeJson(sn.Buffer(), nj);
          nj += "\",\"Class\":\"Signal\",\"Type\":\"";
          EscapeJson(st ? st : "Unknown", nj);
          nj.Printf("\",\"Dimensions\":%u,\"Elements\":%u,"
                    "\"IsTraceable\":%s,\"IsForcable\":%s}",
                    d, el, tr ? "true" : "false", fo ? "true" : "false");
        }
      }
      if (gam != NULL_PTR(GAM *)) {
        uint32 nIn = gam->GetNumberOfInputSignals();
        for (uint32 j = 0u; j < nIn; j++) {
          if (sc > 0u) {
            nj += ",\n";
          }
          sc++;
          StreamString sn;
          (void)gam->GetSignalName(InputSignals, j, sn);
          const char8 *st = TypeDescriptor::GetTypeNameFromTypeDescriptor(
              gam->GetSignalType(InputSignals, j));
          uint32 d = 0u;
          (void)gam->GetSignalNumberOfDimensions(InputSignals, j, d);
          uint32 el = 0u;
          (void)gam->GetSignalNumberOfElements(InputSignals, j, el);
          StreamString sfp;
          sfp.Printf("%s.In.%s", cp.Buffer(), sn.Buffer());
          bool tr = false, fo = false;
          (void)IsInstrumented(sfp.Buffer(), tr, fo);
          nj += "{\"Name\":\"In.";
          EscapeJson(sn.Buffer(), nj);
          nj += "\",\"Class\":\"InputSignal\",\"Type\":\"";
          EscapeJson(st ? st : "Unknown", nj);
          nj.Printf("\",\"Dimensions\":%u,\"Elements\":%u,"
                    "\"IsTraceable\":%s,\"IsForcable\":%s}",
                    d, el, tr ? "true" : "false", fo ? "true" : "false");
        }
        uint32 nOut = gam->GetNumberOfOutputSignals();
        for (uint32 j = 0u; j < nOut; j++) {
          if (sc > 0u) {
            nj += ",\n";
          }
          sc++;
          StreamString sn;
          (void)gam->GetSignalName(OutputSignals, j, sn);
          const char8 *st = TypeDescriptor::GetTypeNameFromTypeDescriptor(
              gam->GetSignalType(OutputSignals, j));
          uint32 d = 0u;
          (void)gam->GetSignalNumberOfDimensions(OutputSignals, j, d);
          uint32 el = 0u;
          (void)gam->GetSignalNumberOfElements(OutputSignals, j, el);
          StreamString sfp;
          sfp.Printf("%s.Out.%s", cp.Buffer(), sn.Buffer());
          bool tr = false, fo = false;
          (void)IsInstrumented(sfp.Buffer(), tr, fo);
          nj += "{\"Name\":\"Out.";
          EscapeJson(sn.Buffer(), nj);
          nj += "\",\"Class\":\"OutputSignal\",\"Type\":\"";
          EscapeJson(st ? st : "Unknown", nj);
          nj.Printf("\",\"Dimensions\":%u,\"Elements\":%u,"
                    "\"IsTraceable\":%s,\"IsForcable\":%s}",
                    d, el, tr ? "true" : "false", fo ? "true" : "false");
        }
      }
      nj += "\n]";
    }
    nj += "}";
    json += nj;
    valid++;
  }
  return valid;
}

// ---------------------------------------------------------------------------
// EnrichWithConfig
// ---------------------------------------------------------------------------

void DebugServiceBase::EnrichWithConfig(const char8 *path, StreamString &json) {
  if (path == NULL_PTR(const char8 *))
    return;
  if (!manualConfigSet)
    RebuildConfigFromRegistry();
  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;
    }
    bool found = false;
    if (fullConfig.MoveRelative(part.Buffer())) {
      found = true;
    }
    if (!found) {
      StreamString pref;
      pref.Printf("+%s", part.Buffer());
      if (fullConfig.MoveRelative(pref.Buffer()))
        found = true;
    }
    if (!found) {
      if (part == "In") {
        if (fullConfig.MoveRelative("InputSignals"))
          found = true;
        else if (fullConfig.MoveRelative("+InputSignals"))
          found = true;
      } else if (part == "Out") {
        if (fullConfig.MoveRelative("OutputSignals"))
          found = true;
        else if (fullConfig.MoveRelative("+OutputSignals"))
          found = true;
      }
    }
    if (!found) {
      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 += "\"";
    }
  }
}

// ---------------------------------------------------------------------------
// JsonifyDatabase (static)
// ---------------------------------------------------------------------------

void DebugServiceBase::JsonifyDatabase(ConfigurationDatabase &db,
                                       StreamString &json) {
  json += "{";
  uint32 n = db.GetNumberOfChildren();
  bool first = true;
  for (uint32 i = 0u; i < n; i++) {
    const char8 *key = db.GetChildName(i);
    if (key == NULL_PTR(const char8 *))
      continue;
    if (!first)
      json += ", ";
    first = false;
    json += "\"";
    EscapeJson(key, json);
    json += "\": ";
    if (db.MoveRelative(key)) {
      JsonifyDatabase(db, json);
      (void)db.MoveToAncestor(1u);
    } else {
      AnyType at = db.GetType(key);
      if (at.GetDataPointer() != NULL_PTR(void *)) {
        char8 buf[512] = {'\0'};
        AnyType dst(CharString, 0u, buf);
        dst.SetNumberOfElements(0u, 512u);
        if (TypeConvert(dst, at)) {
          json += "\"";
          EscapeJson(buf, json);
          json += "\"";
        } else {
          json += "null";
        }
      } else {
        json += "null";
      }
    }
  }
  json += "}";
}

// ---------------------------------------------------------------------------
// HandleCommand — unified command dispatcher
// ---------------------------------------------------------------------------

void DebugServiceBase::HandleCommand(const StreamString &cmdIn,
                                     StreamString &out) {
  StreamString cmd = cmdIn;
  StreamString token;
  cmd.Seek(0u);
  char8 term;
  const char8 *delims = " \r\n";
  if (!cmd.GetToken(token, delims, term))
    return;

  if (token == "FORCE") {
    StreamString name, val;
    if (cmd.GetToken(name, delims, term) && cmd.GetToken(val, delims, term)) {
      uint32 c = ForceSignal(name.Buffer(), val.Buffer());
      out.Printf("OK FORCE %u\n", c);
    }
  } else if (token == "UNFORCE") {
    StreamString name;
    if (cmd.GetToken(name, delims, term)) {
      uint32 c = UnforceSignal(name.Buffer());
      out.Printf("OK UNFORCE %u\n", c);
    }
  } else if (token == "TRACE") {
    StreamString name, state, decim;
    if (cmd.GetToken(name, delims, term) && cmd.GetToken(state, delims, term)) {
      bool en = (state == "1");
      uint32 d = 1u;
      if (cmd.GetToken(decim, delims, term)) {
        AnyType dv(UnsignedInteger32Bit, 0u, &d);
        AnyType ds(CharString, 0u, decim.Buffer());
        (void)TypeConvert(dv, ds);
      }
      uint32 c = TraceSignal(name.Buffer(), en, d);
      out.Printf("OK TRACE %u\n", c);
    }
  } else if (token == "BREAK") {
    StreamString name, opStr;
    if (cmd.GetToken(name, delims, term) && cmd.GetToken(opStr, delims, term)) {
      uint32 c = 0u;
      if (opStr == "OFF") {
        c = ClearBreak(name.Buffer());
      } else {
        StreamString thrStr;
        if (cmd.GetToken(thrStr, 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 thr = 0.0;
            AnyType tv(Float64Bit, 0u, &thr);
            AnyType ts(CharString, 0u, thrStr.Buffer());
            (void)TypeConvert(tv, ts);
            c = SetBreak(name.Buffer(), op, thr);
          }
        }
      }
      out.Printf("OK BREAK %u\n", c);
    }
  } else if (token == "PAUSE") {
    SetPaused(true);
    out += "OK\n";
  } else if (token == "RESUME") {
    SetPaused(false);
    out += "OK\n";
  } else if (token == "STEP") {
    StreamString nStr;
    uint32 n = 1u;
    if (cmd.GetToken(nStr, delims, term)) {
      AnyType nv(UnsignedInteger32Bit, 0u, &n);
      AnyType ns(CharString, 0u, nStr.Buffer());
      (void)TypeConvert(nv, ns);
    }
    StreamString thr;
    const char8 *ta = NULL_PTR(const char8 *);
    if (cmd.GetToken(thr, delims, term) && thr.Size() > 0u)
      ta = thr.Buffer();
    Step(n, ta);
    out.Printf("OK STEP %u\n", n);
  } else if (token == "STEP_STATUS") {
    GetStepStatus(out);
  } else if (token == "VALUE") {
    StreamString sn;
    if (cmd.GetToken(sn, delims, term)) {
      GetSignalValue(sn.Buffer(), out);
    } else {
      out += "{\"Error\":\"Missing signal name\"}\nOK VALUE\n";
    }
  } else if (token == "DISCOVER") {
    Discover(out);
  } else if (token == "TREE") {
    StreamString path;
    (void)cmd.GetToken(path, delims, term);
    ExportTreeNode(path.Size() > 0u ? path.Buffer() : NULL_PTR(const char8 *),
                   out);
  } else if (token == "INFO") {
    StreamString path;
    if (cmd.GetToken(path, delims, term))
      InfoNode(path.Buffer(), out);
  } else if (token == "LS") {
    StreamString path;
    if (cmd.GetToken(path, delims, term))
      ListNodes(path.Buffer(), out);
    else
      ListNodes(NULL_PTR(const char8 *), out);
  } else if (token == "CONFIG") {
    ServeConfig(out);
  } else if (token == "MONITOR") {
    StreamString sub, name, period;
    if (cmd.GetToken(sub, delims, term) && sub == "SIGNAL" &&
        cmd.GetToken(name, delims, term) &&
        cmd.GetToken(period, delims, term)) {
      uint32 p = 100u;
      AnyType pv(UnsignedInteger32Bit, 0u, &p);
      AnyType ps(CharString, 0u, period.Buffer());
      (void)TypeConvert(pv, ps);
      uint32 c = RegisterMonitorSignal(name.Buffer(), p);
      out.Printf("OK MONITOR %u\n", c);
    }
  } else if (token == "UNMONITOR") {
    StreamString sub, name;
    if (cmd.GetToken(sub, delims, term) && sub == "SIGNAL" &&
        cmd.GetToken(name, delims, term)) {
      uint32 c = UnmonitorSignal(name.Buffer());
      out.Printf("OK UNMONITOR %u\n", c);
    }
  } else if (token == "SERVICE_INFO") {
    GetServiceInfo(out);
  } else if (token == "MSG") {
    // MSG <destination> <function> <wait(0|1)> [key=value\nkey=value...]
    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 → real newline)
      StreamString payload;
      rawPayload.Seek(0u);
      char8 c;
      while (rawPayload.Size() > rawPayload.Position()) {
        uint32 rs = 1u;
        if (rawPayload.Read(&c, rs)) {
          if (c == '\\') {
            char8 next;
            if (rawPayload.Read(&next, rs)) {
              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");

      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());
              static const uint32 KEY_BUF_SIZE = 256u;
              static const uint32 VAL_BUF_SIZE = 1024u;
              if (eqPos >= KEY_BUF_SIZE) {
                REPORT_ERROR_STATIC(
                    ErrorManagement::Warning,
                    "MSG: key length %u exceeds buffer (%u) — line skipped.",
                    eqPos, KEY_BUF_SIZE);
                line = "";
                continue;
              }
              (void)line.Seek(0u);
              char8 keyBuf[KEY_BUF_SIZE];
              MemoryOperationsHelper::Set(keyBuf, '\0', KEY_BUF_SIZE);
              uint32 keyReadSize = eqPos;
              (void)line.Read(keyBuf, keyReadSize);
              (void)line.Seek(eqPos + 1u);
              uint32 valLen = (uint32)(line.Size() - eqPos - 1u);
              if (valLen >= VAL_BUF_SIZE) {
                REPORT_ERROR_STATIC(
                    ErrorManagement::Warning,
                    "MSG: value length %u truncated to %u for key '%s'.",
                    valLen, VAL_BUF_SIZE - 1u, keyBuf);
                valLen = VAL_BUF_SIZE - 1u;
              }
              char8 valBuf[VAL_BUF_SIZE];
              MemoryOperationsHelper::Set(valBuf, '\0', VAL_BUF_SIZE);
              (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;
              }
              // 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) {
          (void)msg->Insert(paramCdb);
        }
        Reference destObj =
            ObjectRegistryDatabase::Instance()->Find(dest.Buffer());
        if (destObj.IsValid()) {
          StreamString myPath;
          Object *sender = this;
          if (!GetFullObjectName(*this, myPath)) {
            sender = NULL_PTR(Object *);
          }
          if (wait) {
            err = MessageI::WaitForReply(msg, TTInfiniteWait);
          } else {
            (void)MessageI::SendMessage(msg, sender);
            err = ErrorManagement::NoError;
          }
        } else {
          REPORT_ERROR_STATIC(ErrorManagement::Warning,
                              "MSG: destination '%s' not found in ORD.",
                              dest.Buffer());
        }
      } else {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "MSG: Message initialisation failed.");
      }

      if (err == ErrorManagement::NoError)
        out += "OK MSG\n";
      else
        out += "ERROR MSG\n";
    }
  }
}

// ---------------------------------------------------------------------------
// Out-of-class constant definitions (C++98 ODR requirement)
// ---------------------------------------------------------------------------
const uint32 DebugServiceBase::GET_VALUE_MAX_ELEMENTS;
const uint32 DebugServiceBase::DISCOVER_CHUNK_SIGNALS;

} // namespace MARTe