#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 <assert.h>
#include <stdio.h>

using namespace MARTe;

const char8 * const config_text = 
"+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 = 100 "
"                }"
"            }"
"            OutputSignals = {"
"                Counter = {"
"                    DataSource = DDB "
"                    Type = uint32 "
"                }"
"            }"
"        }"
"    }"
"    +Data = {"
"        Class = ReferenceContainer "
"        DefaultDataSource = DDB "
"        +Timer = {"
"            Class = LinuxTimer "
"            SleepTime = 10000 "
"            Signals = {"
"                Counter = { Type = uint32 }"
"                Time = { Type = uint32 }"
"            }"
"        }"
"        +DDB = {"
"            Class = GAMDataSource "
"            Signals = { Counter = { 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 100Hz Trace Validation Test ---\n");

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

    ConfigurationDatabase cdb;
    StreamString ss = config_text;
    ss.Seek(0);
    StandardParser parser(ss, cdb);
    if (!parser.Parse()) {
        printf("ERROR: Failed to parse configuration\n");
        return;
    }
    
    if (!ObjectRegistryDatabase::Instance()->Initialise(cdb)) {
        printf("ERROR: Failed to initialise ObjectRegistryDatabase.\n");
        return;
    }

    ReferenceT<DebugService> service = ObjectRegistryDatabase::Instance()->Find("DebugService");
    if (!service.IsValid()) {
        printf("ERROR: DebugService not found\n");
        return;
    }
    
    ReferenceT<RealTimeApplication> app = ObjectRegistryDatabase::Instance()->Find("App");
    if (!app.IsValid()) {
        printf("ERROR: App not found\n");
        return;
    }
    
    if (!app->ConfigureApplication()) {
        printf("ERROR: Failed to configure application\n");
        return;
    }

    if (app->PrepareNextState("State1") != ErrorManagement::NoError) {
        printf("ERROR: Failed to prepare state State1\n");
        return;
    }
    
    if (app->StartNextStateExecution() != ErrorManagement::NoError) {
        printf("ERROR: Failed to start execution\n");
        return;
    }

    printf("Application and DebugService are active.\n");
    Sleep::MSec(1000); 

    // DIRECT ACTIVATION: Use the public TraceSignal method
    printf("Activating trace directly...\n");
    // We try multiple potential paths to be safe
    uint32 traceCount = 0;
    traceCount += service->TraceSignal("App.Data.Timer.Counter", true, 1);
    traceCount += service->TraceSignal("Timer.Counter", true, 1);
    traceCount += service->TraceSignal("Counter", true, 1);
    
    printf("Trace enabled (Matched Aliases: %u)\n", traceCount);

    // 4. Setup UDP Listener
    BasicUDPSocket listener;
    if (!listener.Open()) { printf("ERROR: Failed to open UDP socket\n"); return; }
    if (!listener.Listen(8081)) { printf("ERROR: Failed to listen on UDP 8081\n"); return; }

    // 5. Validate for 10 seconds
    printf("Validating telemetry for 10 seconds...\n");
    uint32 lastVal = 0;
    bool first = true;
    uint32 packetCount = 0;
    uint32 discontinuityCount = 0;
    
    float64 startTime = HighResolutionTimer::Counter() * HighResolutionTimer::Period();
    float64 globalTimeout = startTime + 30.0;
    
    while ((HighResolutionTimer::Counter() * HighResolutionTimer::Period() - startTime) < 10.0) {
        if (HighResolutionTimer::Counter() * HighResolutionTimer::Period() > globalTimeout) {
            printf("CRITICAL ERROR: Global test timeout reached.\n");
            break;
        }

        char buffer[2048];
        uint32 size = 2048;
        TimeoutType timeout(200); 
        
        if (listener.Read(buffer, size, timeout)) {
            TraceHeader *h = (TraceHeader*)buffer;
            if (h->magic == 0xDA7A57AD && h->count > 0) {
                uint32 offset = sizeof(TraceHeader);
                // Packet format: [Header][ID:4][Size:4][Value:N]
                uint32 val = *(uint32*)(&buffer[offset + 8]);
                
                if (!first) {
                    if (val != lastVal + 1) {
                        discontinuityCount++;
                    }
                }
                lastVal = val;
                first = false;
                packetCount++;
                
                if (packetCount % 200 == 0) {
                    printf("Received %u packets... Current Value: %u\n", packetCount, val);
                }
            }
        }
    }

    printf("Test Finished.\n");
    printf("Total Packets Received: %u (Expected ~1000)\n", packetCount);
    printf("Discontinuities: %u\n", discontinuityCount);
    
    float64 actualFreq = (float64)packetCount / 10.0;
    printf("Average Frequency: %.2f Hz\n", actualFreq);

    if (packetCount < 100) {
        printf("FAILURE: Almost no packets received. Telemetry is broken.\n");
    } else if (packetCount < 800) {
        printf("WARNING: Too few packets received (Expected 1000, Got %u).\n", packetCount);
    } else if (discontinuityCount > 20) {
        printf("FAILURE: Too many discontinuities (%u).\n", discontinuityCount);
    } else {
        printf("VALIDATION SUCCESSFUL!\n");
    }

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

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