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

using namespace MARTe;

void TestFullTracePipeline() {
    printf("Starting Full Trace Pipeline Test...\n");
    
    ObjectRegistryDatabase::Instance()->Purge();

    // 1. Setup Service
    DebugService service;
    ConfigurationDatabase config;
    config.Write("ControlPort", (uint16)8082);
    config.Write("StreamPort", (uint16)8083);
    config.Write("LogPort", (uint16)8084);
    config.Write("StreamIP", "127.0.0.1");
    assert(service.Initialise(config));
    Sleep::MSec(500);

    // 2. Register a mock signal
    uint32 mockValue = 0;
    DebugSignalInfo* sig = service.RegisterSignal(&mockValue, UnsignedInteger32Bit, "TraceTest.Signal");
    assert(sig != NULL_PTR(DebugSignalInfo*));
    printf("Signal registered with ID: %u\n", sig->internalID);

    // 3. Enable Trace manually
    sig->isTracing = true;
    sig->decimationFactor = 1;

    // 4. Setup a local UDP listener
    BasicUDPSocket listener;
    assert(listener.Open());
    assert(listener.Listen(8083));

    // 5. Simulate cycles
    printf("Simulating cycles...\n");
    for (int i=0; i<50; i++) {
        mockValue = 1000 + i;
        uint64 ts = (uint64)((float64)HighResolutionTimer::Counter() * HighResolutionTimer::Period() * 1000000.0);
        service.ProcessSignal(sig, sizeof(uint32), ts);
        Sleep::MSec(10);
    }

    // 6. Try to read from UDP
    char buffer[2048];
    uint32 size = 2048;
    TimeoutType timeout(1000); // 1s
    if (listener.Read(buffer, size, timeout)) {
        printf("SUCCESS: Received %u bytes over UDP!\n", size);
        
        TraceHeader *h = (TraceHeader*)buffer;
        printf("Header: Magic=0x%X, Count=%u, Seq=%u\n", h->magic, h->count, h->seq);
        
        uint32 offset = sizeof(TraceHeader);
        if (size >= offset + 16) {
            uint32 recId = *(uint32*)(&buffer[offset]);
            uint64 recTs = *(uint64*)(&buffer[offset + 4]);
            uint32 recSize = *(uint32*)(&buffer[offset + 12]);
            printf("Data: ID=%u, TS=%llu, Size=%u\n", recId, (unsigned long long)recTs, recSize);
            if (size >= offset + 16 + recSize) {
                if (recSize == 4) {
                    uint32 recVal = *(uint32*)(&buffer[offset + 16]);
                    printf("Value=%u\n", recVal);
                }
            }
        }
    } else {
        printf("FAILURE: No UDP packets received.\n");
    }

    listener.Close();
}