#include "DebugService.h"
#include "DebugBrokerWrapper.h"
#include "MemoryMapInputBroker.h"
#include "HighResolutionTimer.h"
#include "ObjectRegistryDatabase.h"
#include "StandardParser.h"
#include <stdio.h>
#include <assert.h>

using namespace MARTe;

namespace MARTe {
void RunBenchmark() {
    printf("--- MARTe2 Debug Performance Benchmark V5 (Wait-Free/Branchless) ---\n");
    printf("Testing with 100 signals, 1,000,000 cycles per test.\n\n");

    const uint32 NUM_SIGNALS = 100;
    const uint32 NUM_CYCLES = 1000000;

    DebugService* service = new DebugService();
    service->traceBuffer.Init(128 * 1024 * 1024); 
    ConfigurationDatabase cfg;
    cfg.Write("ControlPort", (uint32)0);
    cfg.Write("StreamPort", (uint32)0);
    assert(service->Initialise(cfg));

    volatile uint32 srcMem[NUM_SIGNALS];
    volatile uint32 dstMem[NUM_SIGNALS];
    for(uint32 i=0; i<NUM_SIGNALS; i++) srcMem[i] = i;

    printf("1. Baseline (Pure Copy): ");
    uint64 start = HighResolutionTimer::Counter();
    for(uint32 c=0; c<NUM_CYCLES; c++) {
        for(uint32 i=0; i<NUM_SIGNALS; i++) {
            dstMem[i] = srcMem[i];
        }
    }
    uint64 end = HighResolutionTimer::Counter();
    float64 baselineTime = (float64)(end - start) * HighResolutionTimer::Period();
    float64 baselineNs = (baselineTime / NUM_CYCLES) * 1e9;
    printf("%.3f ms (avg: %.3f ns)\n", baselineTime * 1000.0, baselineNs);

    DebugMemoryMapInputBroker debugBroker;
    debugBroker.service = service;
    DebugSignalInfo** ptrs = new DebugSignalInfo*[NUM_SIGNALS];
    for(uint32 i=0; i<NUM_SIGNALS; i++) {
        StreamString name;
        name = "Sig";
        // Convert i to string without Printf
        if (i < 10) { name += (char)('0' + i); }
        else { name += (char)('0' + (i/10)); name += (char)('0' + (i%10)); }
        
        ptrs[i] = service->RegisterSignal((void*)&srcMem[i], UnsignedInteger32Bit, name.Buffer());
    }
    volatile bool anyActiveFlag = false;
    service->RegisterBroker(ptrs, NUM_SIGNALS, NULL_PTR(MemoryMapBroker*), &anyActiveFlag);
    debugBroker.infoPtr = &service->brokers[service->numberOfBrokers - 1];
    service->UpdateBrokersActiveStatus();
    assert(anyActiveFlag == false);

    printf("2. Debug Idle (Wait-Free Skip): ");
    start = HighResolutionTimer::Counter();
    for(uint32 c=0; c<NUM_CYCLES; c++) {
        for(uint32 i=0; i<NUM_SIGNALS; i++) dstMem[i] = srcMem[i];
        if (anyActiveFlag || service->IsPaused()) {
            DebugBrokerHelper::Process(service, *debugBroker.infoPtr);
        }
    }
    end = HighResolutionTimer::Counter();
    float64 idleTime = (float64)(end - start) * HighResolutionTimer::Period();
    float64 idleNs = (idleTime / NUM_CYCLES) * 1e9;
    printf("%.3f ms (avg: %.3f ns) | Delta: +%.3f ns\n", 
        idleTime * 1000.0, idleNs, idleNs - baselineNs);

    for(uint32 i=0; i<NUM_SIGNALS; i++) {
        service->signals[i].isTracing = true;
    }
    service->UpdateBrokersActiveStatus();
    assert(anyActiveFlag == true);

    printf("3. Debug Load (100 signals branchless): ");
    start = HighResolutionTimer::Counter();
    for(uint32 c=0; c<NUM_CYCLES; c++) {
        for(uint32 i=0; i<NUM_SIGNALS; i++) dstMem[i] = srcMem[i];
        if (anyActiveFlag || service->IsPaused()) {
            DebugBrokerHelper::Process(service, *debugBroker.infoPtr);
        }
        if ((c % 1000) == 0) {
            uint32 tid, tsize; uint64 tts; uint8 tbuf[16];
            while(service->traceBuffer.Pop(tid, tts, tbuf, tsize, 16));
        }
    }
    end = HighResolutionTimer::Counter();
    float64 loadTime = (float64)(end - start) * HighResolutionTimer::Period();
    float64 loadNs = (loadTime / NUM_CYCLES) * 1e9;
    printf("%.3f ms (avg: %.3f ns) | Delta: +%.3f ns (+%.3f ns/signal)\n", 
        loadTime * 1000.0, loadNs, loadNs - baselineNs, (loadNs - baselineNs)/NUM_SIGNALS);

    printf("\nBenchmark complete.\n");
    
    delete[] ptrs;
    delete service;
}
}

int main() {
    MARTe::RunBenchmark();
    return 0;
}