//
// Created by cfif on 12.11.2024.
//

#include "SerialPortCanComInt.h"
#include "SystemDelayInterface.h"
#include "CanSerialPortFrame.h"

void vSerialPortCanComInit(
        tSerialPortCanComInt *env,
        tSerialPortFrameIO *CanIO,
        uint32_t rxBufferLength
) {
    env->CanIO = CanIO;
    env->queue = osMessageQueueNew(rxBufferLength, 1, NULL);
}

static uint16_t vSerialPortReceiveQueue(tSerialPortCanComInt *env, uint8_t *data, uint16_t size, uint32_t timeout,
                                        osMessageQueueId_t queueId) {
    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;

}

static uint16_t vSerialPortReceive(tSerialPortCanComInt *env, uint8_t *data, uint16_t size, uint32_t timeout) {
    return vSerialPortReceiveQueue(env, data, size, timeout, env->queue);
}

static uint16_t vSerialPortTransmit(tSerialPortCanComInt *env, uint8_t *data, uint16_t size, uint32_t timeout) {

    CanSerialPortFrameSetId1(env->CanIO->env, 0x77);
    uint16_t sent = env->CanIO->transmit1(env->CanIO->env, data, size, timeout);

    return sent;
}

tSerialPortIO vSerialPortCanComIntGetIo(tSerialPortCanComInt *env) {
    tSerialPortIO io = {
            .env = env,
            .receive = (SerialPortIOTransaction) vSerialPortReceive,
            .transmit = (SerialPortIOTransaction) vSerialPortTransmit
    };
    return io;
}