#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;
}

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

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