#ifndef DEBUGBROKERWRAPPER_H
#define DEBUGBROKERWRAPPER_H

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

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

namespace MARTe {

/**
 * @brief Helper for optimized signal processing within brokers.
 */
class DebugBrokerHelper {
public:
    static void Process(DebugService* service, DebugSignalInfo** signalInfoPointers, Vector<uint32>& activeIndices, Vector<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.GetNumberOfElements();
        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];
                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, Vector<uint32>* activeIndices, Vector<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);
            MemoryMapBroker* mmb = dynamic_cast<MemoryMapBroker*>(broker);

            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";
                
                // Register canonical name
                StreamString dsFullName;
                dsFullName.Printf("%s.%s", dsPath.Buffer(), signalName.Buffer());
                service->RegisterSignal(addr, type, dsFullName.Buffer()); 

                // Register alias
                if (functionName != NULL_PTR(const char8*)) {
                    StreamString gamFullName;
                    const char8* dirStr = (direction == InputSignals) ? "In" : "Out";
                    Reference gamRef = ObjectRegistryDatabase::Instance()->Find(functionName);
                    if (gamRef.IsValid()) {
                        StreamString absGamPath;
                        DebugService::GetFullObjectName(*(gamRef.operator->()), absGamPath);
                        gamFullName.Printf("%s.%s.%s", absGamPath.Buffer(), dirStr, signalName.Buffer());
                    } else {
                        gamFullName.Printf("%s.%s.%s", functionName, dirStr, signalName.Buffer());
                    }
                    signalInfoPointers[i] = service->RegisterSignal(addr, type, gamFullName.Buffer());
                } else {
                    signalInfoPointers[i] = service->RegisterSignal(addr, type, dsFullName.Buffer());
                }
            }
            
            // 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);
        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, optim);
        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;
    Vector<uint32> activeIndices;
    Vector<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;
    Vector<uint32> activeIndices;
    Vector<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;
    Vector<uint32> activeIndices;
    Vector<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;
    Vector<uint32> activeIndices;
    Vector<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

}

#endif