#include "TestCommon.h"
#include "BasicTCPSocket.h"
#include "HighResolutionTimer.h"
#include "StringHelper.h"
#include "TimeoutType.h"
#include "UDPSProtocol.h"
#include <sys/socket.h>

namespace MARTe {

const char8 * const debug_test_config = 
"DebugService = {"
"    Class = DebugService "
"    ControlPort = 8095 "
"    UdpPort = 8096 "
"    StreamIP = \"127.0.0.1\" "
"}"
"App = {"
"    Class = RealTimeApplication "
"    +Functions = {"
"        Class = ReferenceContainer "
"        +GAM1 = {"
"            Class = IOGAM "
"            InputSignals = {"
"                Counter = { DataSource = Timer Type = uint32 Frequency = 1000 }"
"            }"
"            OutputSignals = {"
"                Counter = { DataSource = DDB Type = uint32 }"
"            }"
"        }"
"        +GAM2 = {"
"            Class = IOGAM "
"            InputSignals = {"
"                Counter = { DataSource = TimerSlow Type = uint32 Frequency = 10 }"
"            }"
"            OutputSignals = {"
"                Counter = { DataSource = Logger Type = uint32 }"
"            }"
"        }"
"    }"
"    +Data = {"
"        Class = ReferenceContainer "
"        DefaultDataSource = DDB "
"        +Timer = { Class = LinuxTimer SleepTime = 1000 Signals = { Counter = { Type = uint32 } } }"
"        +TimerSlow = { Class = LinuxTimer SleepTime = 100000 Signals = { Counter = { Type = uint32 } } }"
"        +Logger = { Class = LoggerDataSource 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 GAM2} } } }"
"    }"
"    +Scheduler = { Class = GAMScheduler TimingDataSource = DAMS }"
"}";

bool SendCommandGAM(uint16 port, const char8* cmd, StreamString &reply) {
    BasicTCPSocket client;
    if (!client.Open()) return false;
    if (!client.Connect("127.0.0.1", port)) return false;
    
    uint32 s = StringHelper::Length(cmd);
    if (!client.Write(cmd, s)) return false;
    
    char buffer[16384];
    uint32 size = 16384;
    TimeoutType timeout(5000);
    if (client.Read(buffer, size, timeout)) {
        reply.Write(buffer, size);
        client.Close();
        return true;
    }
    client.Close();
    return false;
}

bool ReadUDPSTraceScalar(BasicUDPSocket &sock, uint32 overallTimeoutMs, uint32 &value) {
    uint64 start = HighResolutionTimer::Counter();
    float64 budgetS = (float64)overallTimeoutMs / 1000.0;
    uint32 quantumMs = (overallTimeoutMs < 50u) ? overallTimeoutMs : 50u;
    if (quantumMs == 0u) quantumMs = 1u;

    for (;;) {
        float64 elapsed = (float64)(HighResolutionTimer::Counter() - start) * HighResolutionTimer::Period();
        if (elapsed >= budgetS) break;

        char buffer[2048];
        uint32 size = 2048;
        TimeoutType timeout(quantumMs);
        if (sock.Read(buffer, size, timeout)) {
            if (size >= UDPS_HEADER_SIZE) {
                UDPSPacketHeader hdr;
                memcpy(&hdr, buffer, UDPS_HEADER_SIZE);
                if (hdr.magic == UDPS_MAGIC && hdr.type == UDPS_TYPE_DATA) {
                    uint32 off = UDPS_HEADER_SIZE + 8u; // skip 8-byte HRT timestamp
                    if (size >= off + sizeof(uint32)) {
                        memcpy(&value, buffer + off, sizeof(uint32));
                        return true;
                    }
                }
            }
            // CONFIG packet, undersized datagram, or bad magic — keep polling.
        }
    }
    return false;
}

void GrowUDPRecvBuffer(BasicUDPSocket &sock, uint32 bytes) {
    Handle fd = sock.GetReadHandle();
    if (fd >= 0) {
        int32 sz = static_cast<int32>(bytes);
        setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &sz, sizeof(sz));
    }
}

}