#ifndef DEBUGBROKERWRAPPER_H
#define DEBUGBROKERWRAPPER_H

#include "BrokerI.h"
#include "DataSourceI.h"
#include "DebugService.h"
#include "FastPollingMutexSem.h"
#include "HighResolutionTimer.h"
#include "MemoryMapBroker.h"
#include "ObjectBuilder.h"
#include "ObjectRegistryDatabase.h"
#include "Vec.h"

// Original broker headers
#include "MemoryMapAsyncOutputBroker.h"
#include "MemoryMapAsyncTriggerOutputBroker.h"
#include "MemoryMapInputBroker.h"
#include "MemoryMapInterpolatedInputBroker.h"
#include "MemoryMapMultiBufferInputBroker.h"
#include "MemoryMapMultiBufferOutputBroker.h"
#include "MemoryMapOutputBroker.h"
#include "MemoryMapSynchronisedInputBroker.h"
#include "MemoryMapSynchronisedMultiBufferInputBroker.h"
#include "MemoryMapSynchronisedMultiBufferOutputBroker.h"
#include "MemoryMapSynchronisedOutputBroker.h"

namespace MARTe {

/**
 * @brief Helper for optimized signal processing within brokers.
 */
class DebugBrokerHelper {
public:
  static void Process(DebugService *service,
                      DebugSignalInfo **signalInfoPointers,
                      Vec<uint32> &activeIndices, Vec<uint32> &activeSizes,
                      FastPollingMutexSem &activeMutex) {
    if (service == NULL_PTR(DebugService *))
      return;

    // Re-establish break logic
    while (service->IsPaused()) {
      Sleep::MSec(10);
    }

    activeMutex.FastLock();
    uint32 n = activeIndices.Size();
    if (n > 0 && signalInfoPointers != NULL_PTR(DebugSignalInfo **)) {
      // Capture timestamp ONCE per broker cycle for lowest impact
      uint64 ts = (uint64)((float64)HighResolutionTimer::Counter() *
                           HighResolutionTimer::Period() * 1000000.0);

      for (uint32 i = 0; i < n; i++) {
        uint32 idx = activeIndices[i];
        uint32 size = activeSizes[i];
        DebugSignalInfo *s = signalInfoPointers[idx];
        if (s != NULL_PTR(DebugSignalInfo *)) {
          service->ProcessSignal(s, size, ts);
        }
      }
    }
    activeMutex.FastUnLock();
  }

  // Pass numCopies explicitly so we can mock it
  static void
  InitSignals(BrokerI *broker, DataSourceI &dataSourceIn,
              DebugService *&service, DebugSignalInfo **&signalInfoPointers,
              uint32 numCopies, MemoryMapBrokerCopyTableEntry *copyTable,
              const char8 *functionName, SignalDirection direction,
              volatile bool *anyActiveFlag, Vec<uint32> *activeIndices,
              Vec<uint32> *activeSizes, FastPollingMutexSem *activeMutex) {
    if (numCopies > 0) {
      signalInfoPointers = new DebugSignalInfo *[numCopies];
      for (uint32 i = 0; i < numCopies; i++)
        signalInfoPointers[i] = NULL_PTR(DebugSignalInfo *);
    }

    ReferenceContainer *root = ObjectRegistryDatabase::Instance();
    Reference serviceRef = root->Find("DebugService");
    if (serviceRef.IsValid()) {
      service = dynamic_cast<DebugService *>(serviceRef.operator->());
    }

    if (service && (copyTable != NULL_PTR(MemoryMapBrokerCopyTableEntry *))) {

      StreamString dsPath;
      DebugService::GetFullObjectName(dataSourceIn, dsPath);
      fprintf(stderr, ">> %s broker for %s [%d]\n",
              direction == InputSignals ? "Input" : "Output", dsPath.Buffer(),
              numCopies);
      MemoryMapBroker *mmb = dynamic_cast<MemoryMapBroker *>(broker);
      if (mmb == NULL_PTR(MemoryMapBroker *)) {
        fprintf(stderr, ">> Impossible to get broker pointer!!\n");
      }

      for (uint32 i = 0; i < numCopies; i++) {
        void *addr = copyTable[i].dataSourcePointer;
        TypeDescriptor type = copyTable[i].type;

        uint32 dsIdx = i;
        if (mmb != NULL_PTR(MemoryMapBroker *)) {
          dsIdx = mmb->GetDSCopySignalIndex(i);
        }

        StreamString signalName;
        if (!dataSourceIn.GetSignalName(dsIdx, signalName))
          signalName = "Unknown";
        fprintf(stderr, ">> registering %s.%s [%p]\n", dsPath.Buffer(),
                signalName.Buffer(), mmb);

        uint8 dims = 0;
        uint32 elems = 1;
        (void)dataSourceIn.GetSignalNumberOfDimensions(dsIdx, dims);
        (void)dataSourceIn.GetSignalNumberOfElements(dsIdx, elems);

        // Register canonical name
        StreamString dsFullName;
        dsFullName.Printf("%s.%s", dsPath.Buffer(), signalName.Buffer());
        service->RegisterSignal(addr, type, dsFullName.Buffer(), dims, elems);

        // Register alias
        if (functionName != NULL_PTR(const char8 *)) {
          StreamString gamFullName;
          const char8 *dirStr =
              (direction == InputSignals) ? "InputSignals" : "OutputSignals";
          const char8 *dirStrShort = (direction == InputSignals) ? "In" : "Out";

          // Try to find the GAM with different path variations
          Reference gamRef =
              ObjectRegistryDatabase::Instance()->Find(functionName);
          if (!gamRef.IsValid()) {
            // Try with "App.Functions." prefix
            StreamString tryPath;
            tryPath.Printf("App.Functions.%s", functionName);
            gamRef = ObjectRegistryDatabase::Instance()->Find(tryPath.Buffer());
          }
          if (!gamRef.IsValid()) {
            // Try with "Functions." prefix
            StreamString tryPath;
            tryPath.Printf("Functions.%s", functionName);
            gamRef = ObjectRegistryDatabase::Instance()->Find(tryPath.Buffer());
          }

          if (gamRef.IsValid()) {
            StreamString absGamPath;
            DebugService::GetFullObjectName(*(gamRef.operator->()), absGamPath);
            // Register short path (In/Out) for GUI compatibility
            gamFullName.Printf("%s.%s.%s", absGamPath.Buffer(), dirStrShort,
                               signalName.Buffer());
            signalInfoPointers[i] =
                service->RegisterSignal(addr, type, gamFullName.Buffer(), dims, elems);
          } else {
            // Fallback to short form
            gamFullName.Printf("%s.%s.%s", functionName, dirStrShort,
                               signalName.Buffer());
            signalInfoPointers[i] =
                service->RegisterSignal(addr, type, gamFullName.Buffer(), dims, elems);
          }
        } else {
          signalInfoPointers[i] =
              service->RegisterSignal(addr, type, dsFullName.Buffer(), dims, elems);
        }
      }

      // Register broker in DebugService for optimized control
      service->RegisterBroker(signalInfoPointers, numCopies, mmb, anyActiveFlag,
                              activeIndices, activeSizes, activeMutex);
    }
  }
};

/**
 * @brief Template class to instrument any MARTe2 Broker.
 */
template <typename BaseClass> class DebugBrokerWrapper : public BaseClass {
public:
  DebugBrokerWrapper() : BaseClass() {
    service = NULL_PTR(DebugService *);
    signalInfoPointers = NULL_PTR(DebugSignalInfo **);
    numSignals = 0;
    anyActive = false;
  }

  virtual ~DebugBrokerWrapper() {
    if (signalInfoPointers)
      delete[] signalInfoPointers;
  }

  virtual bool Execute() {
    bool ret = BaseClass::Execute();
    if (ret && (anyActive || (service && service->IsPaused()))) {
      DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
                                 activeSizes, activeMutex);
    }
    return ret;
  }

  virtual bool Init(SignalDirection direction, DataSourceI &ds,
                    const char8 *const name, void *gamMem) {
    bool ret = BaseClass::Init(direction, ds, name, gamMem);
    fprintf(stderr, ">> INIT BROKER %s %s\n", name,
            direction == InputSignals ? "In" : "Out");
    if (ret) {
      numSignals = this->GetNumberOfCopies();
      DebugBrokerHelper::InitSignals(this, ds, service, signalInfoPointers,
                                     numSignals, this->copyTable, name,
                                     direction, &anyActive, &activeIndices,
                                     &activeSizes, &activeMutex);
    }
    return ret;
  }

  virtual bool Init(SignalDirection direction, DataSourceI &ds,
                    const char8 *const name, void *gamMem, const bool optim) {
    bool ret = BaseClass::Init(direction, ds, name, gamMem, false);
    fprintf(stderr, ">> INIT optimized BROKER %s %s\n", name,
            direction == InputSignals ? "In" : "Out");
    if (ret) {
      numSignals = this->GetNumberOfCopies();
      DebugBrokerHelper::InitSignals(this, ds, service, signalInfoPointers,
                                     numSignals, this->copyTable, name,
                                     direction, &anyActive, &activeIndices,
                                     &activeSizes, &activeMutex);
    }
    return ret;
  }

  DebugService *service;
  DebugSignalInfo **signalInfoPointers;
  uint32 numSignals;
  volatile bool anyActive;
  Vec<uint32> activeIndices;
  Vec<uint32> activeSizes;
  FastPollingMutexSem activeMutex;
};

template <typename BaseClass>
class DebugBrokerWrapperNoOptim : public BaseClass {
public:
  DebugBrokerWrapperNoOptim() : BaseClass() {
    service = NULL_PTR(DebugService *);
    signalInfoPointers = NULL_PTR(DebugSignalInfo **);
    numSignals = 0;
    anyActive = false;
  }

  virtual ~DebugBrokerWrapperNoOptim() {
    if (signalInfoPointers)
      delete[] signalInfoPointers;
  }

  virtual bool Execute() {
    bool ret = BaseClass::Execute();
    if (ret && (anyActive || (service && service->IsPaused()))) {
      DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
                                 activeSizes, activeMutex);
    }
    return ret;
  }

  virtual bool Init(SignalDirection direction, DataSourceI &ds,
                    const char8 *const name, void *gamMem) {
    bool ret = BaseClass::Init(direction, ds, name, gamMem);
    if (ret) {
      numSignals = this->GetNumberOfCopies();
      DebugBrokerHelper::InitSignals(this, ds, service, signalInfoPointers,
                                     numSignals, this->copyTable, name,
                                     direction, &anyActive, &activeIndices,
                                     &activeSizes, &activeMutex);
    }
    return ret;
  }

  DebugService *service;
  DebugSignalInfo **signalInfoPointers;
  uint32 numSignals;
  volatile bool anyActive;
  Vec<uint32> activeIndices;
  Vec<uint32> activeSizes;
  FastPollingMutexSem activeMutex;
};

class DebugMemoryMapAsyncOutputBroker : public MemoryMapAsyncOutputBroker {
public:
  DebugMemoryMapAsyncOutputBroker() : MemoryMapAsyncOutputBroker() {
    service = NULL_PTR(DebugService *);
    signalInfoPointers = NULL_PTR(DebugSignalInfo **);
    numSignals = 0;
    anyActive = false;
  }
  virtual ~DebugMemoryMapAsyncOutputBroker() {
    if (signalInfoPointers)
      delete[] signalInfoPointers;
  }
  virtual bool Execute() {
    bool ret = MemoryMapAsyncOutputBroker::Execute();
    if (ret && (anyActive || (service && service->IsPaused()))) {
      DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
                                 activeSizes, activeMutex);
    }
    return ret;
  }
  virtual bool InitWithBufferParameters(const SignalDirection direction,
                                        DataSourceI &dataSourceIn,
                                        const char8 *const functionName,
                                        void *const gamMemoryAddress,
                                        const uint32 numberOfBuffersIn,
                                        const ProcessorType &cpuMaskIn,
                                        const uint32 stackSizeIn) {
    bool ret = MemoryMapAsyncOutputBroker::InitWithBufferParameters(
        direction, dataSourceIn, functionName, gamMemoryAddress,
        numberOfBuffersIn, cpuMaskIn, stackSizeIn);
    if (ret) {
      numSignals = this->GetNumberOfCopies();
      DebugBrokerHelper::InitSignals(
          this, dataSourceIn, service, signalInfoPointers, numSignals,
          this->copyTable, functionName, direction, &anyActive, &activeIndices,
          &activeSizes, &activeMutex);
    }
    return ret;
  }
  DebugService *service;
  DebugSignalInfo **signalInfoPointers;
  uint32 numSignals;
  volatile bool anyActive;
  Vec<uint32> activeIndices;
  Vec<uint32> activeSizes;
  FastPollingMutexSem activeMutex;
};

class DebugMemoryMapAsyncTriggerOutputBroker
    : public MemoryMapAsyncTriggerOutputBroker {
public:
  DebugMemoryMapAsyncTriggerOutputBroker()
      : MemoryMapAsyncTriggerOutputBroker() {
    service = NULL_PTR(DebugService *);
    signalInfoPointers = NULL_PTR(DebugSignalInfo **);
    numSignals = 0;
    anyActive = false;
  }
  virtual ~DebugMemoryMapAsyncTriggerOutputBroker() {
    if (signalInfoPointers)
      delete[] signalInfoPointers;
  }
  virtual bool Execute() {
    bool ret = MemoryMapAsyncTriggerOutputBroker::Execute();
    if (ret && (anyActive || (service && service->IsPaused()))) {
      DebugBrokerHelper::Process(service, signalInfoPointers, activeIndices,
                                 activeSizes, activeMutex);
    }
    return ret;
  }
  virtual bool InitWithTriggerParameters(
      const SignalDirection direction, DataSourceI &dataSourceIn,
      const char8 *const functionName, void *const gamMemoryAddress,
      const uint32 numberOfBuffersIn, const uint32 preTriggerBuffersIn,
      const uint32 postTriggerBuffersIn, const ProcessorType &cpuMaskIn,
      const uint32 stackSizeIn) {
    bool ret = MemoryMapAsyncTriggerOutputBroker::InitWithTriggerParameters(
        direction, dataSourceIn, functionName, gamMemoryAddress,
        numberOfBuffersIn, preTriggerBuffersIn, postTriggerBuffersIn, cpuMaskIn,
        stackSizeIn);
    if (ret) {
      numSignals = this->GetNumberOfCopies();
      DebugBrokerHelper::InitSignals(
          this, dataSourceIn, service, signalInfoPointers, numSignals,
          this->copyTable, functionName, direction, &anyActive, &activeIndices,
          &activeSizes, &activeMutex);
    }
    return ret;
  }
  DebugService *service;
  DebugSignalInfo **signalInfoPointers;
  uint32 numSignals;
  volatile bool anyActive;
  Vec<uint32> activeIndices;
  Vec<uint32> activeSizes;
  FastPollingMutexSem activeMutex;
};

template <typename T> class DebugBrokerBuilder : public ObjectBuilder {
public:
  virtual Object *Build(HeapI *const heap) const { return new (heap) T(); }
};

typedef DebugBrokerWrapper<MemoryMapInputBroker> DebugMemoryMapInputBroker;
// LCOV_EXCL_START
typedef DebugBrokerWrapper<MemoryMapOutputBroker> DebugMemoryMapOutputBroker;
typedef DebugBrokerWrapper<MemoryMapSynchronisedInputBroker>
    DebugMemoryMapSynchronisedInputBroker;
typedef DebugBrokerWrapper<MemoryMapSynchronisedOutputBroker>
    DebugMemoryMapSynchronisedOutputBroker;
typedef DebugBrokerWrapperNoOptim<MemoryMapInterpolatedInputBroker>
    DebugMemoryMapInterpolatedInputBroker;
typedef DebugBrokerWrapper<MemoryMapMultiBufferInputBroker>
    DebugMemoryMapMultiBufferInputBroker;
typedef DebugBrokerWrapper<MemoryMapMultiBufferOutputBroker>
    DebugMemoryMapMultiBufferOutputBroker;
typedef DebugBrokerWrapper<MemoryMapSynchronisedMultiBufferInputBroker>
    DebugMemoryMapSynchronisedMultiBufferInputBroker;
typedef DebugBrokerWrapper<MemoryMapSynchronisedMultiBufferOutputBroker>
    DebugMemoryMapSynchronisedMultiBufferOutputBroker;
// LCOV_EXCL_STOP

typedef DebugBrokerBuilder<DebugMemoryMapInputBroker>
    DebugMemoryMapInputBrokerBuilder;
// LCOV_EXCL_START
typedef DebugBrokerBuilder<DebugMemoryMapOutputBroker>
    DebugMemoryMapOutputBrokerBuilder;
typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedInputBroker>
    DebugMemoryMapSynchronisedInputBrokerBuilder;
typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedOutputBroker>
    DebugMemoryMapSynchronisedOutputBrokerBuilder;
typedef DebugBrokerBuilder<DebugMemoryMapInterpolatedInputBroker>
    DebugMemoryMapInterpolatedInputBrokerBuilder;
typedef DebugBrokerBuilder<DebugMemoryMapMultiBufferInputBroker>
    DebugMemoryMapMultiBufferInputBrokerBuilder;
typedef DebugBrokerBuilder<DebugMemoryMapMultiBufferOutputBroker>
    DebugMemoryMapMultiBufferOutputBrokerBuilder;
typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedMultiBufferInputBroker>
    DebugMemoryMapSynchronisedMultiBufferInputBrokerBuilder;
typedef DebugBrokerBuilder<DebugMemoryMapSynchronisedMultiBufferOutputBroker>
    DebugMemoryMapSynchronisedMultiBufferOutputBrokerBuilder;
typedef DebugBrokerBuilder<DebugMemoryMapAsyncOutputBroker>
    DebugMemoryMapAsyncOutputBrokerBuilder;
typedef DebugBrokerBuilder<DebugMemoryMapAsyncTriggerOutputBroker>
    DebugMemoryMapAsyncTriggerOutputBrokerBuilder;
// LCOV_EXCL_STOP

} // namespace MARTe

#endif