// Mini queue implementation by Jacob 'proj' Repp 

// data is devided up into MAX_QUEUES (MAX_QUEUES cannot exceed 255) 
// blocksize (DATA_SIZE/MAX_QUEUES) cannot exceed 256 bytes
// each block may contain an index to the next block in the chain
// initially data is partitioned into a list of free blocks
// when enqueuing the last block in a chain is found and elements added
// when dequeuing we will remove from head block until empty at which point next block will be 
//   collapsed into head block and added to free list
// when dequeuing we will allow QUEUE_MAX_ELEMENTS-1 of wasted space in head block
// when enqueuing you may have an overhead of QUEUE_MAX_ELEMENTS + 2 for an empty trailing
//   block on your list 

#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <string.h>

#define UNIT_TEST 1 // define for unit test

#define DATA_SIZE 2048
#define MAX_QUEUES 32 
#define BLOCK_SIZE (DATA_SIZE/MAX_QUEUES)

#define BLOCK_START         ((queue_t*)_data)
#define BLOCK_END           ((queue_t*)(_data + DATA_SIZE))
#define BLOCK_AT(i)         ((queue_t*)(_data + (i * BLOCK_SIZE)))
#define Z                   0xff

#define E_OUT_OF_MEMORY -1
#define E_QUEUE_EMPTY   -2

void out_of_memory()
{
    printf("out of memory");
    exit(1);
}

typedef unsigned char byte;

typedef struct
{
// 3 bytes reserved for record keeping
#define QUEUE_MAX_ELEMENTS (BLOCK_SIZE - 3)
#define QUEUE_IS_FULL(pq) ((((pq)->tail + 1) % QUEUE_MAX_ELEMENTS) == (pq)->head)
#define QUEUE_IS_EMPTY(pq) ((pq)->tail == (pq)->head)

    byte next_block;
    byte head;
    byte tail;
    byte elements[QUEUE_MAX_ELEMENTS]; 
} queue_t;

#ifdef UNIT_TEST
// Guard bytes are reserved during testing around the datablock 
// to verify that memory is not corrupted
#define CHECK_SIZE 16
static byte _precheck[CHECK_SIZE];
#endif

static byte _data[DATA_SIZE];

#ifdef UNIT_TEST
static byte _postcheck[CHECK_SIZE];
#endif

static byte _freelist;
static byte _init;

queue_t *queue_create()
{
    queue_t *pq, *pqend;

    if(!_init)
    {
        // link the initial blocks
        pq = BLOCK_START;
        pqend = BLOCK_END;
        _freelist = 0;

        for(pq = BLOCK_START, pqend = BLOCK_END - 1; pq < pqend; ++pq)
        {
            pq->next_block = ++_freelist;
        }

        // last block points to sentinal
        pq->next_block = Z;

        // finished init
        _freelist = 0;
        _init = 1;
    }

    if(Z == _freelist)
    {
        out_of_memory();
        return NULL;
    }


    // return the first free queue block and move head
    pq = BLOCK_AT(_freelist);
    _freelist = pq->next_block;

    // terminate new queue
    pq->next_block = Z;
    pq->head = 0;
    pq->tail = 0;

    return pq;
}

int enqueue(queue_t *pq, byte v)
{
    // find last in chain
    while(Z != pq->next_block)
    {
        pq = BLOCK_AT(pq->next_block);
    }
    
    // if we are out of blocks and queue space return error
    if(Z == _freelist && QUEUE_IS_FULL(pq))
    {
        out_of_memory();
        return E_OUT_OF_MEMORY;
    }

    if(!QUEUE_IS_FULL(pq))
    {
        // add element and increment block tail
        pq->elements[pq->tail] = v;
        pq->tail = (pq->tail + 1) % QUEUE_MAX_ELEMENTS;
    }
    else
    {
        // add next block with space and add element
        pq->next_block = _freelist;
        pq = BLOCK_AT(_freelist);

        // update free list and terminate element just removed
        _freelist = pq->next_block;
        pq->next_block = Z;

        // put element into block
        pq->head = 0;
        pq->tail = 0;
        pq->elements[pq->tail++] = v;
    }

    return 0;
}

int dequeue(queue_t *pq, byte *pv)
{
    queue_t *pqnext;

    // if no elements in first block return
    if(QUEUE_IS_EMPTY(pq))
    {
        return E_QUEUE_EMPTY;
    }

    // remove first element
    *pv = pq->elements[pq->head];  
    pq->head = ((pq->head + 1) % QUEUE_MAX_ELEMENTS);

    // if empty and we have another block we can collapse next block here
    if(Z != pq->next_block && QUEUE_IS_EMPTY(pq))
    {
        pqnext = BLOCK_AT(pq->next_block);
        memcpy(pq, pqnext, sizeof(queue_t));

        // add removed block to free list
        pqnext->next_block = _freelist;
        _freelist = pqnext - BLOCK_START;
    }

    return 0;
}

#ifdef UNIT_TEST
void test_failed(const char *msg, ...)
{
    va_list ap;

    va_start(ap, msg);
    vprintf(msg, ap);
    va_end(ap);

    exit(1);
}

void test_checks()
{
    int i;

    for(i = 0; i < CHECK_SIZE; ++i)
    {
        if(_precheck[i] != 0xbd)
        {
            test_failed("precheck area at %i was corrupt\n", i);
        }
        if(_postcheck[i] != 0xbe)
        {
            test_failed("postcheck area at %i was corrupt\n", i);
        }
    }
}

void test_enqueue(queue_t *pq, int count)
{
    int i;

    for(i = 0; i < count; ++i)
    {
        if(E_OUT_OF_MEMORY == enqueue(pq, rand() & 0xff))
        {
            test_failed("enqueue ran out of memory at %i of %i\n", i, count);
        }
    }
}
void test_dequeue(queue_t *pq, int count)
{
    int i;
    byte v;

    for(i = 0; i < count; ++i)
    {
        if(E_QUEUE_EMPTY == dequeue(pq, &v))
        {
            test_failed("expected element at %i of %i\n", i, count);
        }
    }
}


void test_one()
{
    queue_t *a = queue_create();

    // one byte is used to differentiate between full and empty circular buffers
    test_enqueue(a, QUEUE_MAX_ELEMENTS-1 * MAX_QUEUES);
    test_dequeue(a, QUEUE_MAX_ELEMENTS-1 * MAX_QUEUES);
}

void test_three()
{
    queue_t *a = queue_create();
    queue_t *b = queue_create();
    queue_t *c = queue_create();

    test_enqueue(a, 17);
    test_enqueue(b, 17);
    test_enqueue(c, 17);

    test_dequeue(a, 17);
    test_dequeue(b, 17);
    test_dequeue(c, 17);
}

void test_interleaved()
{
    queue_t *a = queue_create();
    test_enqueue(a, QUEUE_MAX_ELEMENTS+1);

    queue_t *b = queue_create();
    test_enqueue(b, QUEUE_MAX_ELEMENTS+1);

    test_dequeue(a, QUEUE_MAX_ELEMENTS+1);
    test_dequeue(b, QUEUE_MAX_ELEMENTS+1);
}

void test_inorder()
{
    int i;
    byte v;
    queue_t *a = queue_create();

    for(i = 0; i < 256; ++i)
    {
        if(E_OUT_OF_MEMORY == enqueue(a, i))
        {
            test_failed("test_inorder ran out of memory at %i\n", i);
        }
    }
    
    for(i = 0; i < 256; ++i)
    {
        if(E_QUEUE_EMPTY == dequeue(a, &v))
        {
            test_failed("test_inorder ran out of queue at %i\n", i);
        }

        if(v != i)
        {
            test_failed("test_inorder found out of order element at %i\n", i);
        }
    }
}


int main()
{
    // initialize the pre and post check memory blocks to check for corruption
    memset(_precheck, 0xbd, CHECK_SIZE);
    memset(_postcheck, 0xbe, CHECK_SIZE);

    test_inorder();
    test_checks();
    test_one();
    test_checks();
    test_three();
    test_checks();
    test_interleaved();
    test_checks();
    test_inorder();
    test_checks();

    printf("passed functional tests\n");

    return 0;
}
#endif