#include "string.h"
#include "queue.h"

static size_t next_element(size_t index, size_t max_len) {
    return (index + 1) % max_len;
}

queue_ret_t queue_init(queue_t * queue, void * data, size_t max_data_size, size_t element_size) {
    queue->data = data;
    queue->max_data_size = max_data_size;
    queue->element_size = element_size;
    queue->queued_elements = 0;
    queue->pop_ndx = 0;
    queue->push_ndx = 0;

    return QUEUE_OK;
}

queue_ret_t queue_push(queue_t * queue, void * element) {
    if (queue->queued_elements >= queue->max_data_size) {
        return QUEUE_FULL;
    }

    memcpy(queue->data + (queue->push_ndx*queue->element_size), element, queue->element_size);

    queue->push_ndx = next_element(queue->push_ndx, queue->max_data_size);

    queue->queued_elements++;

    return QUEUE_OK;
}

queue_ret_t queue_pop(queue_t * queue, void * store_to) {
    if (queue->queued_elements == 0) {
        return QUEUE_EMPTY;
    }

    memcpy(store_to, queue->data + (queue->pop_ndx*queue->element_size), queue->element_size);

    queue->pop_ndx = next_element(queue->pop_ndx, queue->max_data_size);

    queue->queued_elements--;

    return QUEUE_OK;

}