#ifdef __TESTING__
#include <kernel/_kernel.h>
#endif

#include <stdint.h>
#include <stdbool.h>

#include <queue.h>

void init_queue(queue *q)
{
	q->front = -1;
	q->rear = 0;
}

bool queue_is_empty(queue *q) 
	{return (q->front == q->rear - 1);} ; // TODO: if this returns true, shouldn't we just be re-init-ing because we have the space to take up that used array
bool queue_is_full(queue *q)
	{return (q->rear == MAX_QUEUE_SIZE);} // TODO: this is an error for SURE

void enqueue(queue *q, uint8_t val)
{
	if (queue_is_full(q)) {
#ifdef __TESTING__
		kwarn("Queue being added to is full!!"); // TODO add a way to print like code lines and addresses to stack trace this
#endif	
		return;	
	}
	q->items[q->rear++] = val;
}

void dequeue(queue* q)
{
	if (queue_is_empty(q)) {
#ifdef __TESTING__
		kwarn("Queue thats already empty is trying to be removed from!!!"); // TODO same as above
#endif	
		return;
	}
	q->front++;
}

uint8_t peek(queue* q)
{
	if (queue_is_empty(q)) {
#ifdef __TESTING__
		kwarn("Peeking an empty queue!");
#endif
		return -1;

	}
	return q->items[q->front + 1];
}