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

using namespace MARTe;

void RunFinalValidation() {
    printf("--- MARTe2 Debug Final Validation (End-to-End) ---\n");

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

    // 1. Initialise DebugService FIRST
    const char8 * const service_cfg = 
    "+DebugService = {"
    "    Class = DebugService "
    "    ControlPort = 8080 "
    "    UdpPort = 8081 "
    "    StreamIP = \"127.0.0.1\" "
    "}";

    StreamString ssSrv = service_cfg;
    ssSrv.Seek(0);
    ConfigurationDatabase cdbSrv;
    StandardParser parserSrv(ssSrv, cdbSrv);
    assert(parserSrv.Parse());
    
    cdbSrv.MoveToRoot();
    if (cdbSrv.MoveRelative("+DebugService")) {
        ConfigurationDatabase child;
        cdbSrv.Copy(child);
        cdbSrv.MoveToAncestor(1u);
        Reference ref("DebugService", GlobalObjectsDatabase::Instance()->GetStandardHeap());
        ref->SetName("DebugService");
        if (!ref->Initialise(child)) {
            printf("ERROR: Failed to initialise DebugService\n");
            return;
        }
        ObjectRegistryDatabase::Instance()->Insert(ref);
        printf("[Init] DebugService started.\n");
    }

    // 2. Minimal App configuration
    const char8 * const minimal_app_cfg = 
    "+App = {"
    "    Class = RealTimeApplication "
    "    +Functions = {"
    "        Class = ReferenceContainer "
    "        +GAM1 = {"
    "            Class = IOGAM "
    "            InputSignals = {"
    "                Counter = { DataSource = Timer Type = uint32 Frequency = 1000 }"
    "            }"
    "            OutputSignals = {"
    "                Counter = { DataSource = DDB Type = uint32 }"
    "            }"
    "        }"
    "    }"
    "    +Data = {"
    "        Class = ReferenceContainer "
    "        DefaultDataSource = DDB "
    "        +Timer = { Class = LinuxTimer SleepTime = 1000 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 }"
    "}";

    StreamString ssApp = minimal_app_cfg;
    ssApp.Seek(0);
    ConfigurationDatabase cdbApp;
    StandardParser parserApp(ssApp, cdbApp);
    assert(parserApp.Parse());
    
    cdbApp.MoveToRoot();
    if (cdbApp.MoveRelative("+App")) {
        ConfigurationDatabase child;
        cdbApp.Copy(child);
        cdbApp.MoveToAncestor(1u);
        Reference ref("RealTimeApplication", GlobalObjectsDatabase::Instance()->GetStandardHeap());
        ref->SetName("App");
        if (ref->Initialise(child)) {
            ObjectRegistryDatabase::Instance()->Insert(ref);
            printf("[Init] App object created.\n");
        } else {
            printf("ERROR: Failed to initialise App object.\n");
            return;
        }
    }

    Reference appRef = ObjectRegistryDatabase::Instance()->Find("App");
    RealTimeApplication* app = dynamic_cast<RealTimeApplication*>(appRef.operator->());

    // 3. Start Application
    printf("Configuring Application...\n");
    if (!app->ConfigureApplication()) {
        printf("ERROR: ConfigureApplication failed.\n");
        return;
    }
    printf("Preparing State1...\n");
    if (app->PrepareNextState("State1") != ErrorManagement::NoError) {
        printf("ERROR: Failed to prepare State1.\n");
        return;
    }
    printf("Starting State1 Execution...\n");
    if (app->StartNextStateExecution() != ErrorManagement::NoError) {
        printf("ERROR: Failed to start State1 execution.\n");
        return;
    }

    printf("Application running. Starting client simulation...\n");
    Sleep::MSec(1000);

    // 4. Act as Client: Send Commands
    BasicTCPSocket client;
    if (client.Connect("127.0.0.1", 8080, TimeoutType(2000))) {
        printf("[Client] Connected to DebugService.\n");
        
        // Command 1: TREE
        printf("[Client] Sending TREE...\n");
        uint32 cmdLen = 5;
        client.Write("TREE\n", cmdLen);
        char buf[4096]; uint32 rsize = 4096;
        if (client.Read(buf, rsize, TimeoutType(1000))) {
            printf("[Client] TREE response received (%u bytes).\n", rsize);
        }

        // Command 2: DISCOVER
        printf("[Client] Sending DISCOVER...\n");
        cmdLen = 9;
        client.Write("DISCOVER\n", cmdLen);
        rsize = 4096;
        if (client.Read(buf, rsize, TimeoutType(1000))) {
            buf[rsize] = '\0';
            printf("[Client] DISCOVER response:\n%s\n", buf);
        }

        // Command 3: TRACE
        const char* target = "App.Data.Timer.Counter";
        printf("[Client] Sending TRACE %s 1...\n", target);
        StreamString traceCmd;
        traceCmd.Printf("TRACE %s 1\n", target);
        cmdLen = traceCmd.Size();
        client.Write(traceCmd.Buffer(), cmdLen);
        
        rsize = 1024;
        if (client.Read(buf, rsize, TimeoutType(1000))) {
            buf[rsize] = '\0';
            printf("[Client] TRACE response: %s", buf);
        }
        client.Close();
    } else {
        printf("ERROR: Client failed to connect to 127.0.0.1:8080\n");
    }

    // 5. Verify Telemetry
    BasicUDPSocket telemListener;
    assert(telemListener.Open());
    assert(telemListener.Listen(8081));

    printf("Listening for UDP Telemetry on 8081...\n");
    uint32 totalSamples = 0;
    uint64 startBench = HighResolutionTimer::Counter();
    
    while (totalSamples < 50 && (HighResolutionTimer::Counter() - startBench) * HighResolutionTimer::Period() < 10.0) {
        char packet[4096];
        uint32 psize = 4096;
        if (telemListener.Read(packet, psize, TimeoutType(100))) {
            if (psize < 20) continue;
            uint32 magic = *(uint32*)(&packet[0]);
            if (magic != 0xDA7A57AD) continue;

            uint32 count = *(uint32*)(&packet[16]);
            uint32 offset = 20;
            for (uint32 j=0; j<count; j++) {
                if (offset + 16 > psize) break;
                uint32 id = *(uint32*)(&packet[offset]);
                uint64 ts = *(uint64*)(&packet[offset + 4]);
                uint32 size = *(uint32*)(&packet[offset + 12]);
                offset += 16;
                if (offset + size > psize) break;
                if (size == 4) {
                    uint32 val = *(uint32*)(&packet[offset]);
                    if (totalSamples % 10 == 0) printf("[Telemetry] Sample %u: ID=%u, TS=%lu, Val=%u\n", totalSamples, id, ts, val);
                    totalSamples++;
                }
                offset += size;
            }
        }
    }

    if (totalSamples >= 50) {
        printf("\nSUCCESS: End-to-End pipeline verified with real MARTe2 app!\n");
    } else {
        printf("\nFAILURE: Received only %u samples in 10 seconds.\n", totalSamples);
    }

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

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