#include "TcpLogger.h"
#include "Threads.h"
#include "StringHelper.h"
#include "ConfigurationDatabase.h"

namespace MARTe {

CLASS_REGISTER(TcpLogger, "1.0")

TcpLogger::TcpLogger() : 
    ReferenceContainer(), LoggerConsumerI(), EmbeddedServiceMethodBinderI(),
    service(*this)
{
    port = 8082;
    readIdx = 0;
    writeIdx = 0;
    workerThreadId = InvalidThreadIdentifier;
    for (uint32 i=0; i<MAX_CLIENTS; i++) {
        activeClients[i] = NULL_PTR(BasicTCPSocket*);
    }
}

TcpLogger::~TcpLogger() {
    service.Stop();
    server.Close();

    clientsMutex.FastLock();
    for (uint32 i=0; i<MAX_CLIENTS; i++) {
        if (activeClients[i] != NULL_PTR(BasicTCPSocket*)) {
            activeClients[i]->Close();
            delete activeClients[i];
            activeClients[i] = NULL_PTR(BasicTCPSocket*);
        }
    }
    clientsMutex.FastUnLock();
}

bool TcpLogger::ExportData(StructuredDataI & data) {
    bool ok = data.Write("Port", static_cast<uint32>(port));
    return ok;
}

bool TcpLogger::Initialise(StructuredDataI & data) {
    if (!ReferenceContainer::Initialise(data)) return false;
    
    if (!data.Read("Port", port)) {
        (void)data.Read("TcpPort", port);
    }

    (void)eventSem.Create();

    ConfigurationDatabase threadData;
    threadData.Write("Timeout", (uint32)1000);
    if (!service.Initialise(threadData)) return false;

    if (!server.Open()) return false;
    if (!server.Listen(port)) {
        return false;
    }
    printf("[TcpLogger] Listening on port %u\n", port);

    if (service.Start() != ErrorManagement::NoError) {
        return false;
    }

    return true;
}

void TcpLogger::ConsumeLogMessage(LoggerPage *logPage) {
    if (logPage == NULL_PTR(LoggerPage*)) return;

    // 1. Mirror to stdout
    StreamString levelStr;
    ErrorManagement::ErrorCodeToStream(logPage->errorInfo.header.errorType, levelStr);
    printf("[%s] %s\n", levelStr.Buffer(), logPage->errorStrBuffer);
    fflush(stdout);

    // 2. Queue for TCP
    InsertLogIntoQueue(logPage->errorInfo, logPage->errorStrBuffer);
}

void TcpLogger::InsertLogIntoQueue(const ErrorManagement::ErrorInformation &info, const char8 * const description) {
    uint32 next = (writeIdx + 1) % QUEUE_SIZE;
    if (next != readIdx) {
        TcpLogEntry &entry = queue[writeIdx % QUEUE_SIZE];
        entry.info = info;
        StringHelper::Copy(entry.description, description);
        writeIdx = next;
        (void)eventSem.Post();
    }
}

ErrorManagement::ErrorType TcpLogger::Execute(ExecutionInfo & info) {
    if (info.GetStage() == ExecutionInfo::TerminationStage) return ErrorManagement::NoError;
    if (info.GetStage() == ExecutionInfo::StartupStage) {
        workerThreadId = Threads::Id();
        return ErrorManagement::NoError;
    }

    // Each Execute() call does one cycle. The MARTe2 framework loops Execute()
    // so we must NOT spin in an infinite internal loop here — doing so prevents
    // the framework from ever delivering the TerminationStage and causes
    // Stop() to time out, leaving threads running after the destructor.

    // 1. Check for new connections (1 ms timeout → returns promptly)
    BasicTCPSocket *newClient = server.WaitConnection(1);
    if (newClient != NULL_PTR(BasicTCPSocket *)) {
        clientsMutex.FastLock();
        bool added = false;
        for (uint32 i=0; i<MAX_CLIENTS; i++) {
            if (activeClients[i] == NULL_PTR(BasicTCPSocket*)) {
                activeClients[i] = newClient;
                added = true;
                break;
            }
        }
        clientsMutex.FastUnLock();
        if (!added) {
            newClient->Close();
            delete newClient;
        } else {
            (void)newClient->SetBlocking(false);
        }
    }

    // 2. Stream queued entries to clients
    bool hadData = false;
    while (readIdx != writeIdx) {
        hadData = true;
        uint32 idx = readIdx % QUEUE_SIZE;
        TcpLogEntry &entry = queue[idx];

        StreamString level;
        ErrorManagement::ErrorCodeToStream(entry.info.header.errorType, level);

        StreamString packet;
        packet.Printf("LOG %s %s\n", level.Buffer(), entry.description);
        uint32 size = packet.Size();

        clientsMutex.FastLock();
        for (uint32 j=0; j<MAX_CLIENTS; j++) {
            if (activeClients[j] != NULL_PTR(BasicTCPSocket*)) {
                uint32 s = size;
                if (!activeClients[j]->Write(packet.Buffer(), s)) {
                    activeClients[j]->Close();
                    delete activeClients[j];
                    activeClients[j] = NULL_PTR(BasicTCPSocket*);
                }
            }
        }
        clientsMutex.FastUnLock();
        readIdx = (readIdx + 1) % QUEUE_SIZE;
    }

    if (!hadData) {
        // Brief wait so we don't busy-spin; return so Stop() can take effect
        (void)eventSem.Wait(TimeoutType(10));
        eventSem.Reset();
    }
    return ErrorManagement::NoError;
}

}
