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

using namespace MARTe;

const char8 * const validation_config = 
"DebugService = {"
"    Class = DebugService "
"    ControlPort = 8085 "
"    UdpPort = 8086 "
"    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 = validation_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 in ValidationTest\n");
        return;
    }

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

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

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

    if (service->TraceSignal("App.Data.Timer.Counter", true, 1) == 0) {
        printf("ERROR: Failed to enable trace for App.Data.Timer.Counter\n");
    }

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

    printf("Validating for 10 seconds...\n");
    uint32 totalPackets = 0;
    uint32 totalSamples = 0;
    uint32 discontinuities = 0;
    uint32 lastValue = 0xFFFFFFFF;

    uint64 start = HighResolutionTimer::Counter();
    float64 elapsed = 0;
    while (elapsed < 10.0) {
        char buffer[2048];
        uint32 size = 2048;
        if (listener.Read(buffer, size, TimeoutType(100))) {
            totalPackets++;
            TraceHeader *h = (TraceHeader*)buffer;
            uint32 offset = sizeof(TraceHeader);
            for (uint32 i=0; i<h->count; i++) {
                uint32 recId = *(uint32*)(&buffer[offset]);
                uint32 recSize = *(uint32*)(&buffer[offset + 12]);
                if (recSize == 4) {
                    uint32 val = *(uint32*)(&buffer[offset + 16]);
                    totalSamples++;
                    if (lastValue != 0xFFFFFFFF && val != lastValue + 1) {
                        discontinuities++;
                    }
                    lastValue = val;
                }
                offset += (16 + recSize);
            }
        }
        elapsed = (float64)(HighResolutionTimer::Counter() - start) * HighResolutionTimer::Period();
    }

    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 > 50) { 
        printf("FAILURE: Excessive discontinuities detected! (%u)\n", discontinuities);
    } else {
        printf("VALIDATION SUCCESSFUL: 1kHz Lossless Tracing Verified.\n");
    }

    app->StopCurrentStateExecution();
}