#ifndef _PIUMA_LIB_QUEUE_H_
#define _PIUMA_LIB_QUEUE_H_

#include "types.h"
#include <assert.h>

struct queue_header
{
	u64 start;
	u64 size;

	u64 capacity;
};


#define QUEUE_HEADER_PTR(queue) ((queue_header *)(((u8*)queue) - sizeof(queue_header)))

#define QUEUE_TYPE(type) type *
#define Queue_Alloc(alloc_func, type, capacity) ((type*) _queue_alloc(alloc_func, sizeof(type), capacity))
#define Queue_Free(free_func, queue) free_func(QUEUE_HEADER_PTR(queue))

#define Queue_Pop(queue) (queue[_queue_pop_index((u8*)queue)])
#define Queue_Push(queue, element) { queue[_queue_at_index((u8*)queue, QUEUE_HEADER_PTR(queue)->size)] = element; _queue_push_fix_indices((u8*)queue); }
#define Queue_At(queue, index) (queue[_queue_at_index((u8*)queue, index)])
#define Queue_Size(queue) _queue_size((u8*)queue)
#define Queue_Capacity(queue) _queue_capacity((u8*)queue)



typedef void * (*alloc_func_t)(u64);
typedef void (*free_func_t)(void *);

inline u8 * _queue_alloc(alloc_func_t alloc_func, u64 sizeof_type, u64 capacity)
{
	u8 *data;
	queue_header *header;

	data   = (u8 *)alloc_func(sizeof(queue_header) + sizeof_type * capacity);
	header = (queue_header *)data;

	header->capacity = capacity;
	header->start    = 0;
	header->size     = 0;

	return data + sizeof(queue_header);
}

inline u64 _queue_pop_index(u8 *queue)
{
	queue_header *header = QUEUE_HEADER_PTR(queue);
	assert(header->size > 0);

	u64 element_index = header->start;
	header->start = (header->start + 1) % header->capacity;
	header->size--;

	return element_index;
}

inline void _queue_push_fix_indices(u8 *queue)
{
	queue_header *header = QUEUE_HEADER_PTR(queue);

	header->size++;
	if(header->size > header->capacity)
	{
		// Queue is full. Remove oldest element
		header->start = (header->start + 1) % header->capacity;
		header->size  = header->capacity;
	}
}

inline u64 _queue_at_index(u8 *queue, u64 index)
{
	queue_header *header = QUEUE_HEADER_PTR(queue);

	return (header->start + index) % header->capacity;
}

inline u64 _queue_size(u8 *queue)
{
	queue_header *header = QUEUE_HEADER_PTR(queue);
	return header->size;
}

inline u64 _queue_capacity(u8 *queue)
{
	queue_header *header = QUEUE_HEADER_PTR(queue);
	return header->capacity;
}


#endif