/**
 * @file UDPStreamerClient.cpp
 * @brief Source file for class UDPStreamerClient
 * @date 24/06/2026
 * @author Martino Ferrari
 *
 * @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
 * the Development of Fusion Energy ('Fusion for Energy').
 * Licensed under the EUPL, Version 1.1 or - as soon they will be approved
 * by the European Commission - subsequent versions of the EUPL (the "Licence")
 * You may not use this work except in compliance with the Licence.
 * You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
 *
 * @warning Unless required by applicable law or agreed to in writing,
 * software distributed under the Licence is distributed on an "AS IS"
 * basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
 * or implied. See the Licence permissions and limitations under the Licence.
 *
 * @details This source file contains the definition of all the methods for
 * the class UDPStreamerClient (public, protected, and private).
 */

#define DLL_API

/*---------------------------------------------------------------------------*/
/*                         Standard header includes                          */
/*---------------------------------------------------------------------------*/
#include <string.h>

/*---------------------------------------------------------------------------*/
/*                         Project header includes                           */
/*---------------------------------------------------------------------------*/
#include "AdvancedErrorManagement.h"
#include "ConfigurationDatabase.h"
#include "GlobalObjectsDatabase.h"
#include "MemoryOperationsHelper.h"
#include "UDPSProtocol.h"
#include "UDPStreamerClient.h"

/*---------------------------------------------------------------------------*/
/*                           Static definitions                              */
/*---------------------------------------------------------------------------*/

namespace MARTe {

/** Default server address when none is specified. */
static const char8 *const UDPS_CLIENT_DEFAULT_ADDR = "127.0.0.1";

/** Default port used when none is specified. */
static const uint16 UDPS_CLIENT_DEFAULT_PORT = 44500u;

/** Default data-port offset: dataPort = port + this when DataPort is absent. */
static const uint16 UDPS_CLIENT_DEFAULT_DP_OFFSET = 1u;

/** Default max payload per UDP datagram (bytes). */
static const uint32 UDPS_CLIENT_DEFAULT_MAX_PAYLOAD = 1400u;

/** Bytes prepended to each DATA payload for the HRT packet timestamp. */
static const uint32 UDPS_CLIENT_TIMESTAMP_BYTES = 8u;

/*---------------------------------------------------------------------------*/
/*                           Decode helpers                                  */
/*---------------------------------------------------------------------------*/

/**
 * @brief Reads one quantised wire element (1 or 2 bytes) as a float64,
 * mirroring UDPSourceSession::DecodeRawValue for the quantised type codes.
 */
static float64 DecodeQuantRaw(const uint8 *ptr, uint8 quantType) {
    float64 raw = 0.0;
    switch (quantType) {
        case UDPS_QUANT_UINT8: {
            uint8 v = 0u; (void) memcpy(&v, ptr, 1u); raw = static_cast<float64>(v);
            break;
        }
        case UDPS_QUANT_INT8: {
            int8 v = 0; (void) memcpy(&v, ptr, 1u); raw = static_cast<float64>(v);
            break;
        }
        case UDPS_QUANT_UINT16: {
            uint16 v = 0u; (void) memcpy(&v, ptr, 2u); raw = static_cast<float64>(v);
            break;
        }
        case UDPS_QUANT_INT16: {
            int16 v = 0; (void) memcpy(&v, ptr, 2u); raw = static_cast<float64>(v);
            break;
        }
        default:
            break;
    }
    return raw;
}

/**
 * @brief Dequantises a raw value, mirroring UDPSourceSession::DequantizeValue.
 */
static float64 DequantValue(float64 raw, uint8 quantType,
                            float64 rangeMin, float64 rangeMax) {
    const float64 range = rangeMax - rangeMin;
    float64 val;
    switch (quantType) {
        case UDPS_QUANT_UINT8:
            val = rangeMin + (raw / 255.0) * range;
            break;
        case UDPS_QUANT_INT8:
            val = rangeMin + ((raw + 127.0) / 254.0) * range;
            break;
        case UDPS_QUANT_UINT16:
            val = rangeMin + (raw / 65535.0) * range;
            break;
        case UDPS_QUANT_INT16:
            val = rangeMin + ((raw + 32767.0) / 65534.0) * range;
            break;
        default:
            val = raw;
            break;
    }
    return val;
}

/*---------------------------------------------------------------------------*/
/*                           Method definitions                              */
/*---------------------------------------------------------------------------*/

UDPStreamerClient::UDPStreamerClient() :
        MemoryDataSourceI(),
        UDPSClientListener(),
        client() {
    serverAddress   = UDPS_CLIENT_DEFAULT_ADDR;
    port            = UDPS_CLIENT_DEFAULT_PORT;
    maxPayloadSize  = UDPS_CLIENT_DEFAULT_MAX_PAYLOAD;
    cpuMask         = 0xFFFFFFFFu;
    stackSize       = THREADS_DEFAULT_STACKSIZE;
    dataPort        = UDPS_CLIENT_DEFAULT_PORT + UDPS_CLIENT_DEFAULT_DP_OFFSET;
    useMulticast    = false;
    numSigs         = 0u;
    signalInfos     = NULL_PTR(UDPStreamerClientSignal *);
    totalSrcBytes   = 0u;
    configValidated = false;
    publishMode     = UDPS_PUBLISH_STRICT;
    receiverUp      = false;
    readyBuffer     = NULL_PTR(uint8 *);
    scratchBuffer   = NULL_PTR(uint8 *);
    newDataReady    = false;

    if (!dataSem.Create()) {
        REPORT_ERROR(ErrorManagement::FatalError, "Could not create EventSem.");
    }
    bufMutex.Create(false);
}

UDPStreamerClient::~UDPStreamerClient() {
    (void) dataSem.Post();

    if (receiverUp) {
        (void) client.Stop();
        receiverUp = false;
    }

    HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
    if (signalInfos != NULL_PTR(UDPStreamerClientSignal *)) {
        delete[] signalInfos;
        signalInfos = NULL_PTR(UDPStreamerClientSignal *);
    }
    if (readyBuffer != NULL_PTR(uint8 *)) {
        heap->Free(reinterpret_cast<void *&>(readyBuffer));
    }
    if (scratchBuffer != NULL_PTR(uint8 *)) {
        heap->Free(reinterpret_cast<void *&>(scratchBuffer));
    }

    (void) dataSem.Close();
}

bool UDPStreamerClient::Initialise(StructuredDataI &data) {
    bool ok = MemoryDataSourceI::Initialise(data);

    if (ok) {
        StreamString addr = "";
        if (!data.Read("ServerAddress", addr) || (addr.Size() == 0u)) {
            addr = UDPS_CLIENT_DEFAULT_ADDR;
            REPORT_ERROR(ErrorManagement::Information,
                         "ServerAddress not specified; using default %s.",
                         UDPS_CLIENT_DEFAULT_ADDR);
        }
        serverAddress = addr;
    }

    if (ok) {
        if (!data.Read("Port", port)) {
            port = UDPS_CLIENT_DEFAULT_PORT;
            REPORT_ERROR(ErrorManagement::Information,
                         "Port not specified; using default %u.",
                         static_cast<uint32>(port));
        }
    }

    if (ok) {
        if (!data.Read("MaxPayloadSize", maxPayloadSize)) {
            maxPayloadSize = UDPS_CLIENT_DEFAULT_MAX_PAYLOAD;
        }
    }

    if (ok) {
        if (!data.Read("CPUMask", cpuMask)) {
            cpuMask = 0xFFFFFFFFu;
        }
    }

    if (ok) {
        if (!data.Read("StackSize", stackSize)) {
            stackSize = THREADS_DEFAULT_STACKSIZE;
        }
    }

    if (ok) {
        StreamString mcastStr = "";
        (void) data.Read("MulticastGroup", mcastStr);
        if (mcastStr.Size() > 0u) {
            multicastGroup = mcastStr;
            useMulticast   = true;
            uint16 dp = 0u;
            if (!data.Read("DataPort", dp)) {
                dp = port + UDPS_CLIENT_DEFAULT_DP_OFFSET;
            }
            dataPort = dp;
            REPORT_ERROR(ErrorManagement::Information,
                         "Multicast mode: group=%s, server=%s, controlPort=%u, dataPort=%u.",
                         multicastGroup.Buffer(), serverAddress.Buffer(),
                         static_cast<uint32>(port), static_cast<uint32>(dataPort));
        }
        else {
            useMulticast = false;
        }
    }

    /* Forward the transport parameters to the shared UDPSClient receiver,
     * translating the DataSource parameter names to the UDPSClient keys. */
    if (ok) {
        ConfigurationDatabase cdb;
        ok = cdb.Write("ServerAddr", serverAddress);
        if (ok) { ok = cdb.Write("Port", static_cast<uint32>(port)); }
        if (ok && useMulticast) {
            ok = cdb.Write("MulticastGroup", multicastGroup);
            if (ok) { ok = cdb.Write("DataPort", static_cast<uint32>(dataPort)); }
        }
        if (ok) { ok = cdb.Write("MaxPayloadSize", maxPayloadSize); }
        if (ok) { ok = cdb.Write("CPUMask", cpuMask); }
        if (ok) { ok = cdb.Write("StackSize", stackSize); }
        if (ok) { ok = cdb.MoveToRoot(); }
        if (ok) { ok = client.Initialise(cdb); }
        if (!ok) {
            REPORT_ERROR(ErrorManagement::ParametersError,
                         "Could not configure the UDPSClient receiver.");
        }
        client.SetListener(this);
    }

    return ok;
}

bool UDPStreamerClient::SetConfiguredDatabase(StructuredDataI &data) {
    bool ok = MemoryDataSourceI::SetConfiguredDatabase(data);
    if (!ok) {
        return false;
    }

    numSigs = GetNumberOfSignals();
    if (numSigs == 0u) {
        REPORT_ERROR(ErrorManagement::ParametersError,
                     "At least one signal must be defined.");
        return false;
    }

    signalInfos = new UDPStreamerClientSignal[numSigs];

    totalSrcBytes = 0u;
    for (uint32 i = 0u; (i < numSigs) && ok; i++) {
        StreamString sigName;
        ok = GetSignalName(i, sigName);
        if (!ok) {
            REPORT_ERROR(ErrorManagement::FatalError,
                         "Could not get name for signal %u.", i);
            break;
        }

        uint32 nelems = 1u;
        (void) GetSignalNumberOfElements(i, nelems);
        if (nelems == 0u) { nelems = 1u; }

        uint32 bsz = 0u;
        (void) GetSignalByteSize(i, bsz);

        signalInfos[i].name          = sigName;
        signalInfos[i].type          = GetSignalType(i);
        signalInfos[i].quantType     = UDPS_QUANT_NONE;
        signalInfos[i].numElements   = nelems;
        signalInfos[i].rangeMin      = 0.0;
        signalInfos[i].rangeMax      = 1.0;
        signalInfos[i].srcByteSize   = bsz;
        signalInfos[i].wireElemBytes = (nelems > 0u) ? (bsz / nelems) : bsz;
        signalInfos[i].bufferOffset  = totalSrcBytes;
        totalSrcBytes += bsz;
    }

    return ok;
}

bool UDPStreamerClient::AllocateMemory() {
    bool ok = MemoryDataSourceI::AllocateMemory();
    if (!ok) {
        return false;
    }

    if (totalSrcBytes == 0u) {
        totalSrcBytes = stateMemorySize;
    }

    HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();

    readyBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalSrcBytes));
    if (readyBuffer == NULL_PTR(uint8 *)) {
        REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate readyBuffer.");
        return false;
    }
    (void) MemoryOperationsHelper::Set(readyBuffer, 0, totalSrcBytes);

    scratchBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalSrcBytes));
    if (scratchBuffer == NULL_PTR(uint8 *)) {
        REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate scratchBuffer.");
        return false;
    }
    (void) MemoryOperationsHelper::Set(scratchBuffer, 0, totalSrcBytes);

    return true;
}

const char8 *UDPStreamerClient::GetBrokerName(StructuredDataI &data,
                                              const SignalDirection direction) {
    const char8 *brokerName = "";
    if (direction == InputSignals) {
        brokerName = "MemoryMapSynchronisedInputBroker";
    }
    return brokerName;
}

bool UDPStreamerClient::PrepareNextState(const char8 *const currentStateName,
                                         const char8 *const nextStateName) {
    bool ok = true;
    if (!receiverUp) {
        ok = client.Start();
        if (ok) {
            receiverUp = true;
        }
        else {
            REPORT_ERROR(ErrorManagement::FatalError,
                         "Could not start the UDPSClient receiver.");
        }
    }
    return ok;
}

bool UDPStreamerClient::Synchronise() {
    ErrorManagement::ErrorType waitErr = dataSem.ResetWait(TimeoutType(10u));
    if (waitErr == ErrorManagement::NoError) {
        bufMutex.FastLock(TTInfiniteWait);
        (void) MemoryOperationsHelper::Copy(memory, readyBuffer, totalSrcBytes);
        newDataReady = false;
        bufMutex.FastUnLock();
    }
    return true;
}

/*---------------------------------------------------------------------------*/
/*                       UDPSClientListener callbacks                        */
/*---------------------------------------------------------------------------*/

void UDPStreamerClient::OnUDPSConfig(const uint8 *payload, uint32 payloadSize) {
    if (payloadSize < 4u) {
        REPORT_ERROR(ErrorManagement::ParametersError,
                     "CONFIG payload too small (%u bytes).", payloadSize);
        return;
    }

    uint32 serverNumSigs = 0u;
    (void) memcpy(&serverNumSigs, payload, 4u);

    if (serverNumSigs != numSigs) {
        REPORT_ERROR(ErrorManagement::ParametersError,
                     "CONFIG signal count mismatch: server=%u, client=%u.",
                     serverNumSigs, numSigs);
        return;
    }

    const uint32 needed = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE);
    if (payloadSize < needed) {
        REPORT_ERROR(ErrorManagement::ParametersError,
                     "CONFIG payload truncated (%u < %u bytes).", payloadSize, needed);
        return;
    }

    bool ok = true;
    for (uint32 i = 0u; (i < numSigs) && ok; i++) {
        UDPSSignalDescriptor desc;
        (void) memcpy(&desc, payload + 4u + (i * UDPS_SIGNAL_DESC_SIZE),
                      UDPS_SIGNAL_DESC_SIZE);

        char8 nameBuf[UDPS_MAX_SIGNAL_NAME + 1u];
        (void) memcpy(nameBuf, desc.name, UDPS_MAX_SIGNAL_NAME);
        nameBuf[UDPS_MAX_SIGNAL_NAME] = '\0';
        StreamString serverName = nameBuf;

        if (signalInfos[i].name != serverName) {
            REPORT_ERROR(ErrorManagement::ParametersError,
                         "CONFIG signal %u name mismatch: server=%s, client=%s.",
                         i, serverName.Buffer(), signalInfos[i].name.Buffer());
            ok = false;
            break;
        }

        uint32 serverElems = desc.numRows * desc.numCols;
        if (serverElems == 0u) { serverElems = 1u; }
        if (serverElems != signalInfos[i].numElements) {
            REPORT_ERROR(ErrorManagement::ParametersError,
                         "CONFIG signal %u element count mismatch: server=%u, client=%u.",
                         i, serverElems, signalInfos[i].numElements);
            ok = false;
            break;
        }

        signalInfos[i].quantType = desc.quantType;
        signalInfos[i].rangeMin  = desc.rangeMin;
        signalInfos[i].rangeMax  = desc.rangeMax;
        if (desc.quantType != UDPS_QUANT_NONE) {
            uint32 q = 0u;
            if ((desc.quantType == UDPS_QUANT_UINT8) ||
                (desc.quantType == UDPS_QUANT_INT8)) {
                q = 1u;
            }
            else if ((desc.quantType == UDPS_QUANT_UINT16) ||
                     (desc.quantType == UDPS_QUANT_INT16)) {
                q = 2u;
            }
            signalInfos[i].wireElemBytes = q;
        }
    }

    if (ok) {
        const uint32 pmOffset = needed;
        publishMode = (payloadSize > pmOffset) ? payload[pmOffset] : UDPS_PUBLISH_STRICT;

        bufMutex.FastLock(TTInfiniteWait);
        configValidated = true;
        bufMutex.FastUnLock();

        REPORT_ERROR(ErrorManagement::Information,
                     "CONFIG validated: %u signals, publishMode=%u, totalSrcBytes=%u.",
                     numSigs, static_cast<uint32>(publishMode), totalSrcBytes);
    }
}

void UDPStreamerClient::OnUDPSData(const uint8 *payload, uint32 payloadSize) {
    if (!configValidated) {
        return;
    }
    if (payloadSize < UDPS_CLIENT_TIMESTAMP_BYTES) {
        return;
    }

    (void) MemoryOperationsHelper::Set(scratchBuffer, 0, totalSrcBytes);
    DecodeSnapshot(payload, payloadSize, scratchBuffer);

    bufMutex.FastLock(TTInfiniteWait);
    (void) MemoryOperationsHelper::Copy(readyBuffer, scratchBuffer, totalSrcBytes);
    newDataReady = true;
    bufMutex.FastUnLock();

    (void) dataSem.Post();
}

void UDPStreamerClient::OnUDPSConnected() {
    REPORT_ERROR(ErrorManagement::Information,
                 "UDPStreamerClient connected to %s:%u.",
                 serverAddress.Buffer(), static_cast<uint32>(port));
}

void UDPStreamerClient::OnUDPSDisconnected() {
    bufMutex.FastLock(TTInfiniteWait);
    configValidated = false;
    bufMutex.FastUnLock();
    REPORT_ERROR(ErrorManagement::Information, "UDPStreamerClient disconnected.");
}

/*---------------------------------------------------------------------------*/
/*                              Decode logic                                 */
/*---------------------------------------------------------------------------*/

void UDPStreamerClient::DecodeSnapshot(const uint8 *payload, uint32 size,
                                       uint8 *dst) {
    uint32 offset     = UDPS_CLIENT_TIMESTAMP_BYTES;
    uint32 numSamples = 1u;

    if (publishMode == UDPS_PUBLISH_ACCUMULATE) {
        if ((offset + 4u) > size) { return; }
        (void) memcpy(&numSamples, payload + offset, 4u);
        offset += 4u;
        if (numSamples == 0u) { numSamples = 1u; }
    }

    uint32 off = offset;
    for (uint32 s = 0u; s < numSigs; s++) {
        const UDPStreamerClientSignal &info = signalInfos[s];
        const uint32 wireElemBytes = info.wireElemBytes;
        if (wireElemBytes == 0u) { return; }

        const uint32 ne = info.numElements;
        const bool accScalar = (publishMode == UDPS_PUBLISH_ACCUMULATE) && (ne == 1u);
        const uint32 elemsToRead = accScalar ? numSamples : ne;

        /* HI-1: 64-bit bounds check to prevent uint32 multiply overflow */
        uint64 bytesNeeded = static_cast<uint64>(off) +
                             static_cast<uint64>(elemsToRead) *
                             static_cast<uint64>(wireElemBytes);
        if (bytesNeeded > static_cast<uint64>(size)) { return; }

        uint8 *d = dst + info.bufferOffset;

        if (info.quantType == UDPS_QUANT_NONE) {
            if (accScalar) {
                /* Publish the most recent accumulated sample. */
                const uint8 *srcLast = payload + off +
                                       ((numSamples - 1u) * wireElemBytes);
                (void) memcpy(d, srcLast, wireElemBytes);
            }
            else {
                (void) memcpy(d, payload + off, ne * wireElemBytes);
            }
        }
        else {
            const bool isF32 = (info.type == Float32Bit);
            const uint32 startElem = accScalar ? (numSamples - 1u) : 0u;
            const uint32 count     = accScalar ? 1u : ne;
            for (uint32 e = 0u; e < count; e++) {
                const uint8 *ptr = payload + off +
                                   ((startElem + e) * wireElemBytes);
                const float64 raw = DecodeQuantRaw(ptr, info.quantType);
                const float64 val = DequantValue(raw, info.quantType,
                                                 info.rangeMin, info.rangeMax);
                if (isF32) {
                    float32 f32 = static_cast<float32>(val);
                    (void) memcpy(d, &f32, 4u);
                    d += 4u;
                }
                else {
                    float64 f64 = val;
                    (void) memcpy(d, &f64, 8u);
                    d += 8u;
                }
            }
        }

        off += elemsToRead * wireElemBytes;
    }
}

CLASS_REGISTER(UDPStreamerClient, "1.0")

} /* namespace MARTe */