#include "squeue.h"

bool squeue_init(SQueue *q, uint16_t capacity, size_t element_size) {
    q->buffer = malloc(element_size * capacity);
    if (!q->buffer) {
        return false;
    }

    q->head = q->tail = q->count = 0;
    q->capacity = capacity;
    q->element_size = element_size;

    return true;
}

void squeue_free(SQueue *q) {
    free(q->buffer);
    q->buffer = NULL;
    q->head = q->tail = q->count = q->capacity = 0;
    q->element_size = 0;
}

bool squeue_push(SQueue *q, const void *elem) {
    if (q->count == q->capacity) {
        return false;
    }

    void *dest = (uint8_t*)q->buffer + q->tail * q->element_size;
    memcpy(dest, elem, q->element_size);
    q->tail = (q->tail + 1) % q->capacity;
    q->count++;

    return true;
}

bool squeue_pop(SQueue *q, void *out) {
    if (q->count == 0) {
        return false;
    }

    void *src = (uint8_t*)q->buffer + q->head * q->element_size;
    memcpy(out, src, q->element_size);
    q->head = (q->head + 1) % q->capacity;
    q->count--;
    
    return true;
}

bool squeue_peek(const SQueue *q, void *out) {
    if (q->count == 0) {
        return false;
    }

    void *src = (uint8_t*)q->buffer + q->head * q->element_size;
    memcpy(out, src, q->element_size);
    return true;
}