//
// Created by cfif on 24.05.2024.
//
#include <SystemDelayInterface.h>
#include "GsmWithGnss.h"

static uint16_t vSerialPortReceiveQueue(osMessageQueueId_t queueId, uint8_t *data, uint16_t size, uint32_t timeout) {
    uint16_t received = 0;
    if (timeout) {
        uint32_t endMs = SystemGetMs() + timeout;
        uint32_t leftMs;

        while (size && ((timeout == SystemWaitForever) || (endMs > SystemGetMs()))) {
            leftMs = endMs - SystemGetMs();
            if (osMessageQueueGet(queueId, data, NULL, leftMs) == osOK) {
                --size;
                ++received;
                ++data;
            }
        }

    } else {
        while (size) {
            if (osMessageQueueGet(queueId, data, NULL, 0) == osOK) {
                --size;
                ++received;
                ++data;
            } else {
                return received;
            }
        }
    }

    return received;
}


size_t Gsm_Socket_SSL_read(tGsmWithGnss *env, uint8_t contextId, uint8_t *data, size_t size, uint32_t timeout) {
    if (contextId == 255)
        return 0;

    size_t len = vSerialPortReceiveQueue(env->slots[contextId].rxDataQueue, data, size, timeout);
    return len;
}