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

using namespace MARTe;

void TestFullTracePipeline() {
    printf("Starting Full Trace Pipeline Test...\n");
    printf("sizeof(TraceHeader) = %lu\n", sizeof(TraceHeader));

    // 1. Setup Service
    DebugService service;
    ConfigurationDatabase config;
    config.Write("ControlPort", (uint16)8080);
    config.Write("StreamPort", (uint16)8081);
    config.Write("LogPort", (uint16)8082);
    config.Write("StreamIP", "127.0.0.1");
    assert(service.Initialise(config));

    // 2. Register a mock signal
    uint32 mockValue = 0;
    DebugSignalInfo* sig = service.RegisterSignal(&mockValue, UnsignedInteger32Bit, "Test.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(8081));

    // 5. Simulate cycles
    printf("Simulating cycles...\n");
    for (int i=0; i<50; i++) {
        mockValue = 1000 + i;
        service.ProcessSignal(sig, sizeof(uint32));
        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);
        for(uint32 i=0; i<size; i++) {
            printf("%02X ", (uint8)buffer[i]);
            if((i+1)%4 == 0) printf("| ");
            if((i+1)%16 == 0) printf("\n");
        }
        printf("\n");

        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 + 8) {
            uint32 recId = *(uint32*)(&buffer[offset]);
            uint32 recSize = *(uint32*)(&buffer[offset + 4]);
            printf("Data: ID=%u, Size=%u\n", recId, recSize);
            if (size >= offset + 8 + recSize) {
                if (recSize == 4) {
                    uint32 recVal = *(uint32*)(&buffer[offset + 8]);
                    printf("Value=%u\n", recVal);
                }
            }
        }
    } else {
        printf("FAILURE: No UDP packets received.\n");
    }

    listener.Close();
}

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