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

queue_t *queue_malloc(uint16_t element_size, uint16_t length) {
  return (queue_t*)malloc(sizeof(queue_t) + element_size * length);
}

void queue_init(queue_t* queue, uint16_t element_size, uint16_t length) {
  queue->curr_write_ptr = 0;
  queue->curr_read_ptr = 0;
  queue->element_size = element_size;
  queue->length = length;
  queue->space = length;
}

bool queue_enqueue(queue_t *queue, void *element) {
  if (queue->space == 0) {
    return false;
  }

  for (uint16_t i = 0; i < queue->element_size; i++) {
    queue->elements[queue->curr_write_ptr + i] = *(uint8_t*)(element + i);
  }

  queue->space--;
  queue->curr_write_ptr += queue->element_size;
  queue->curr_write_ptr %= queue->length * queue->element_size;
  return true;
}

void *queue_dequeue(queue_t *queue) {
  void* element = queue_peek(queue);

  if (element != NULL) {
    queue->curr_read_ptr += queue->element_size;
    queue->curr_read_ptr %= queue->length * queue->element_size;
    queue->space++;
  }

  return element;
}

bool queue_dequeue_safely(queue_t *queue, void *element) {
  uint8_t* queue_element = (uint8_t*)queue_peek(queue);

  if (queue_element == NULL) {
    return false;
  }

  uint8_t* target_element = (uint8_t*)element;

  // First copy
  for (uint16_t i = 0; i < queue->element_size; i++) {
    *(target_element + i) = *(queue_element + i);
  }

  // Then "commit"
  if (element != NULL) {
    queue->curr_read_ptr += queue->element_size;
    queue->curr_read_ptr %= queue->length * queue->element_size;
    queue->space++;
    // From now on, the element might be rewritten in queue,
    // but is has already been copied.
  }

  return true;
}

void *queue_peek(queue_t *queue) {
  if (queue->space == queue->length) {
    return NULL;
  }

  return &queue->elements[queue->curr_read_ptr];
}

uint16_t queue_count(queue_t *queue) {
  return queue->length - queue->space;
}

uint16_t queue_space(queue_t *queue) {
  return queue->space;
}