#include "buffered_peripheral.h"
#include <stdlib.h>

void buffered_transceiver_init(buffered_transceiver_t *transceiver,
                               queue_t *rx_queue, queue_t *tx_queue,
                               void *peripheral, buffered_transceiver_vtable_t vtable) {
  transceiver->rx_queue = rx_queue;
  transceiver->tx_queue = tx_queue;
  transceiver->peripheral = peripheral;
  transceiver->vtable = vtable;
}

uint16_t buffered_transceiver_transmit(buffered_transceiver_t *transceiver,
                                            void *buffer, uint16_t size) {
  uint16_t sent = 0;

  while (sent < size && queue_space(transceiver->tx_queue) > 0) {
    queue_enqueue(transceiver->tx_queue, buffer + sent * transceiver->tx_queue->element_size);
    sent++;
  }

  buffered_transceiver_trigger_transmit(transceiver, 1);

  return sent;
}

uint16_t buffered_transceiver_receive(buffered_transceiver_t *transceiver,
                                      void *buffer, uint16_t max_size) {
  // Get all data from buffer
  buffered_transceiver_trigger_receive(transceiver, 1);
  uint16_t received = 0;

  while (queue_count(transceiver->rx_queue) > 0) {
    queue_dequeue_safely(transceiver->rx_queue, buffer + received*transceiver->rx_queue->element_size);
    received++;
  }

  return received;
}

void buffered_transceiver_trigger_receive(buffered_transceiver_t *transceiver, uint16_t size) {
  void* element = alloca(transceiver->rx_queue->element_size);

  while (transceiver->vtable.can_receive(transceiver->peripheral) > 0 && queue_space(transceiver->rx_queue) > 0) {
    transceiver->vtable.receive(transceiver->peripheral, &element, 1);
    queue_enqueue(transceiver->rx_queue, &element);
  }
}

void buffered_transceiver_trigger_transmit(buffered_transceiver_t *transceiver,
                                           uint16_t size) {
  while (transceiver->vtable.can_transmit(transceiver->peripheral) &&
         queue_count(transceiver->tx_queue) > 0) {
    void* data = alloca(transceiver->tx_queue->element_size);
    queue_dequeue_safely(transceiver->tx_queue, data);
    transceiver->vtable.transmit(transceiver->peripheral, data, 1);
  }
}