#include "DebugService.h"
#include "DebugCore.h"
#include "ObjectRegistryDatabase.h"
#include "StandardParser.h"
#include "StreamString.h"
#include "BasicUDPSocket.h"
#include "BasicTCPSocket.h"
#include "RealTimeApplication.h"
#include "GlobalObjectsDatabase.h"
#include "RealTimeLoader.h"
#include "HighResolutionTimer.h"
#include <assert.h>
#include <stdio.h>

using namespace MARTe;

// Removed '+' prefix from names for simpler lookup
const char8 * const simple_config = 
"DebugService = {"
"    Class = DebugService "
"    ControlPort = 8080 "
"    UdpPort = 8081 "
"    StreamIP = \"127.0.0.1\" "
"}"
"App = {"
"    Class = RealTimeApplication "
"    +Functions = {"
"        Class = ReferenceContainer "
"        +GAM1 = {"
"            Class = IOGAM "
"            InputSignals = {"
"                Counter = { DataSource = Timer Type = uint32 Frequency = 1000 }"
"                Time = { DataSource = Timer Type = uint32 }"
"            }"
"            OutputSignals = {"
"                Counter = { DataSource = DDB Type = uint32 }"
"                Time = { DataSource = DDB Type = uint32 }"
"            }"
"        }"
"    }"
"    +Data = {"
"        Class = ReferenceContainer "
"        DefaultDataSource = DDB "
"        +Timer = { Class = LinuxTimer SleepTime = 1000 Signals = { Counter = { Type = uint32 } Time = { Type = uint32 } } }"
"        +DDB = { Class = GAMDataSource Signals = { Counter = { Type = uint32 } Time = { Type = uint32 } } }"
"        +DAMS = { Class = TimingDataSource }"
"    }"
"    +States = {"
"        Class = ReferenceContainer "
"        +State1 = { Class = RealTimeState +Threads = { Class = ReferenceContainer +Thread1 = { Class = RealTimeThread Functions = {GAM1} } } }"
"    }"
"    +Scheduler = { Class = GAMScheduler TimingDataSource = DAMS }"
"}";

void RunValidationTest() {
    printf("--- MARTe2 1kHz Lossless Trace Validation Test ---\n");

    ObjectRegistryDatabase::Instance()->Purge();

    ConfigurationDatabase cdb;
    StreamString ss = simple_config;
    ss.Seek(0);
    StandardParser parser(ss, cdb);
    assert(parser.Parse());
    
    cdb.MoveToRoot();
    uint32 n = cdb.GetNumberOfChildren();
    for (uint32 i=0; i<n; i++) {
        const char8* name = cdb.GetChildName(i);
        ConfigurationDatabase child;
        cdb.MoveRelative(name);
        cdb.Copy(child);
        cdb.MoveToAncestor(1u);
        
        StreamString className;
        child.Read("Class", className);
        
        Reference ref(className.Buffer(), GlobalObjectsDatabase::Instance()->GetStandardHeap());
        ref->SetName(name);
        assert(ref->Initialise(child));
        ObjectRegistryDatabase::Instance()->Insert(ref);
    }

    Reference serviceGeneric = ObjectRegistryDatabase::Instance()->Find("DebugService");
    Reference appGeneric = ObjectRegistryDatabase::Instance()->Find("App");
    
    if (!serviceGeneric.IsValid() || !appGeneric.IsValid()) {
        printf("ERROR: Objects NOT FOUND even without prefix\n");
        return;
    }

    DebugService *service = dynamic_cast<DebugService*>(serviceGeneric.operator->());
    RealTimeApplication *app = dynamic_cast<RealTimeApplication*>(appGeneric.operator->());
    
    assert(service);
    assert(app);

    if (!app->ConfigureApplication()) {
        printf("ERROR: ConfigureApplication failed.\n");
        return;
    }
    
    assert(app->PrepareNextState("State1") == ErrorManagement::NoError);
    assert(app->StartNextStateExecution() == ErrorManagement::NoError);

    printf("Application started at 1kHz. Enabling Traces...\n");
    Sleep::MSec(500); 

    // The registered name in DebugBrokerWrapper depends on GetFullObjectName
    // With App as root, it should be App.Data.Timer.Counter
    service->TraceSignal("App.Data.Timer.Counter", true, 1);

    BasicUDPSocket listener;
    listener.Open();
    listener.Listen(8081);

    printf("Validating for 10 seconds...\n");
    
    uint32 lastCounter = 0;
    bool first = true;
    uint32 totalSamples = 0;
    uint32 discontinuities = 0;
    uint32 totalPackets = 0;

    float64 startTest = HighResolutionTimer::Counter() * HighResolutionTimer::Period();
    
    while ((HighResolutionTimer::Counter() * HighResolutionTimer::Period() - startTest) < 10.0) {
        char buffer[4096];
        uint32 size = 4096;
        if (listener.Read(buffer, size, TimeoutType(100))) {
            totalPackets++;
            TraceHeader *h = (TraceHeader*)buffer;
            if (h->magic != 0xDA7A57AD) continue;

            uint32 offset = sizeof(TraceHeader);
            for (uint32 i=0; i<h->count; i++) {
                if (offset + 16 > size) break;
                
                uint32 sigId = *(uint32*)(&buffer[offset]);
                uint32 sigSize = *(uint32*)(&buffer[offset + 12]);
                
                if (offset + 16 + sigSize > size) break;
                
                if (sigId == 0 && sigSize == 4) { 
                    uint32 val = *(uint32*)(&buffer[offset + 16]);
                    if (!first) {
                        if (val != lastCounter + 1) {
                            discontinuities++;
                        }
                    }
                    lastCounter = val;
                    totalSamples++;
                }
                
                offset += (16 + sigSize);
            }
            first = false;
        }
    }

    printf("\n--- Test Results ---\n");
    printf("Total UDP Packets: %u\n", totalPackets);
    printf("Total Counter Samples: %u\n", totalSamples);
    printf("Counter Discontinuities: %u\n", discontinuities);

    if (totalSamples < 9000) {
        printf("FAILURE: Underflow - samples missing (%u).\n", totalSamples);
    } else if (discontinuities > 10) { 
        printf("FAILURE: Excessive discontinuities detected! (%u)\n", discontinuities);
    } else {
        printf("VALIDATION SUCCESSFUL: 1kHz Lossless Tracing Verified.\n");
    }

    app->StopCurrentStateExecution();
    ObjectRegistryDatabase::Instance()->Purge();
}

int main() {
    RunValidationTest();
    return 0;
}