#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 <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 "
"                }"
"            }"
"            OutputSignals = {"
"                Counter = {"
"                    DataSource = DDB "
"                    Type = uint32 "
"                }"
"            }"
"        }"
"    }"
"    +Data = {"
"        Class = ReferenceContainer "
"        DefaultDataSource = DDB "
"        +Timer = {"
"            Class = LinuxTimer "
"            SleepTime = 10000 "
"            Signals = {"
"                Counter = { 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");

    // 1. Load Configuration
    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;
    }

    // 2. Start Application
    ReferenceT<RealTimeApplication> app = ObjectRegistryDatabase::Instance()->Find("App");
    if (!app.IsValid()) {
        printf("ERROR: App not found\n");
        return;
    }
    
    // We try to use State1 directly as many MARTe2 apps start in the first defined state if no transition is needed
    if (app->PrepareNextState("State1") != ErrorManagement::NoError) {
        printf("ERROR: Failed to prepare state State1\n");
        // We will try to investigate why, but for now we continue
    }
    
    if (app->StartNextStateExecution() != ErrorManagement::NoError) {
        printf("ERROR: Failed to start execution. Maybe it needs an explicit state?\n");
        // return;
    }

    printf("Application started at 100Hz.\n");
    Sleep::MSec(1000); 

    // 3. Enable Trace via TCP (Simulating GUI)
    BasicTCPSocket client;
    if (client.Connect("127.0.0.1", 8080)) {
        const char* cmd = "TRACE Root.App.Data.Timer.Counter 1\n";
        uint32 s = StringHelper::Length(cmd);
        client.Write(cmd, s);
        
        char resp[1024]; s = 1024;
        TimeoutType timeout(1000);
        if (client.Read(resp, s, timeout)) {
            resp[s] = '\0';
            printf("Server Response: %s", resp);
        } else {
            printf("WARNING: No response from server to TRACE command.\n");
        }
        client.Close();
    } else {
        printf("ERROR: Failed to connect to DebugService on 8080\n");
        // continue anyway to see if it's already working
    }

    // 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 30 seconds
    printf("Validating telemetry for 30 seconds...\n");
    uint32 lastVal = 0;
    bool first = true;
    uint32 packetCount = 0;
    uint32 discontinuityCount = 0;
    
    float64 startTime = HighResolutionTimer::Counter() * HighResolutionTimer::Period();
    
    while ((HighResolutionTimer::Counter() * HighResolutionTimer::Period() - startTime) < 30.0) {
        char buffer[2048];
        uint32 size = 2048;
        TimeoutType timeout(500); 
        
        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 % 500 == 0) {
                    printf("Received %u packets... Current Value: %u\n", packetCount, val);
                }
            }
        }
    }

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

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

    app->StopCurrentStateExecution();
    listener.Close();
}

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