//
// Created by xemon on 13.11.22.
//
#include "SerialPortP2p.h"

void SerialPortP2p_Init(tSerialPortP2p *env, uint16_t firstToSecondBufferLen, uint16_t secondToFirstBufferLen) {
    env->firstToSecondQueue = osMessageQueueNew(firstToSecondBufferLen, 1, NULL);
    env->secondToFirstQueue = osMessageQueueNew(secondToFirstBufferLen, 1, NULL);
}

static uint16_t vSerialPortReceiveQueue(uint8_t *data, uint16_t size, uint32_t timeout, osMessageQueueId_t queueId) {
    uint16_t received = 0;

    uint32_t endMs = SystemGetMs() + timeout;
    uint32_t leftMs;

    uint16_t granted = 0;

    if (!timeout) {
        granted = osMessageQueueGetCount(queueId);
    }

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

    return received;
}

static uint16_t vSerialPortReceiveFirst(tSerialPortP2p *env, uint8_t *data, uint16_t size, uint32_t timeout) {
    return vSerialPortReceiveQueue(data, size, timeout, env->secondToFirstQueue);
}

static uint16_t vSerialPortReceiveSecond(tSerialPortP2p *env, uint8_t *data, uint16_t size, uint32_t timeout) {
    return vSerialPortReceiveQueue(data, size, timeout, env->firstToSecondQueue);
}

static uint16_t vSerialPortTransmitQueue(uint8_t *data, uint16_t size, uint32_t timeout, osMessageQueueId_t queueId) {
    uint16_t sent = 0;

    uint32_t endMs = SystemGetMs() + timeout;
    uint32_t leftMs;

    uint16_t granted = 0;

    if (!timeout) {
        granted = osMessageQueueGetSpace(queueId);
    }

    while (size && (((timeout == SystemWaitForever) || (endMs > SystemGetMs())) || (sent < granted))) {

        if (timeout == SystemWaitForever) {
            leftMs = SystemWaitForever;
        } else {
            leftMs = endMs - SystemGetMs();
        }

        if (osMessageQueuePut(queueId, data, 0x00, leftMs) == osOK) {
            --size;
            ++sent;
            ++data;
        } else if (timeout == 0) {
            return sent;
        }
    }
    return sent;
}


static uint16_t vSerialPortTransmitFirst(tSerialPortP2p *env, uint8_t *data, uint16_t size, uint32_t timeout) {
    return vSerialPortTransmitQueue(data, size, timeout, env->firstToSecondQueue);
}

static uint16_t vSerialPortTransmitSecond(tSerialPortP2p *env, uint8_t *data, uint16_t size, uint32_t timeout) {
    return vSerialPortTransmitQueue(data, size, timeout, env->secondToFirstQueue);
}

tSerialPortIO SerialPortP2p_GetIoFirst(tSerialPortP2p *env) {
    tSerialPortIO io = {
            .env = env,
            .receive = (SerialPortIOTransaction) vSerialPortReceiveFirst,
            .transmit = (SerialPortIOTransaction) vSerialPortTransmitFirst
    };
    return io;
}

tSerialPortIO SerialPortP2p_GetIoSecond(tSerialPortP2p *env) {
    tSerialPortIO io = {
            .env = env,
            .receive = (SerialPortIOTransaction) vSerialPortReceiveSecond,
            .transmit = (SerialPortIOTransaction) vSerialPortTransmitSecond
    };
    return io;
}