#define MAX_QUEUES    64
#define MAX_DATA_SIZE 2048

namespace QueueManager
{
  // Queue handles are a single byte.  Do not change the typedef!
  typedef unsigned char QueueHandle;

  // Performs any necessary initialization of the QueueManager.
  void initializeQueueManager();

  // Creates a FIFO byte queue, returns the handle for the created
  // queue.
  QueueHandle createQueue();

  // Destroy the queue specified by queueHandle.
  void destroyQueue (QueueHandle queueHandle);

  // Adds a new byte to the queue specified by queueHandle.
  void enQueue (QueueHandle queueHandle, unsigned char value);

  // Pops the next byte off the queue specified by queueHandle.
  unsigned char deQueue (QueueHandle queueHandle);

  // If you are unable to satisfy a request due to lack of memory your 
  // code should call this provided failure function (which will not return):
  void onOutOfMemory();

  // If the caller makes any illegal request your code should call this 
  // provided failure function (which will not return):
  void onIllegalOperation(const char* fmt = 0, ... );
    // eg:
    //   int errorCode;
    //   ...
    //   onIllegalOperation("Error in createQueue: %d", errorCode);
}

#include "QueueManager.h"

namespace QueueManager
{
	const int CHUNK_SIZE = 8;				// in Bytes 
	const int NUM_BITS_CHUNK_DATA_COUNT = 4;  // number of bits needed to hold numbers from 0...CHUNK_SIZE
	const int NUM_CHUNKS = MAX_DATA_SIZE / CHUNK_SIZE;
	const int NUM_BITS_CHUNK_COUNT = 8;		// number of bits needed to hold numbers from 0...NUM_CHUNKS

	//Holds the the collective queue-contents
	unsigned char Data[MAX_DATA_SIZE];

	// Queue management data structures	
	struct CHUNKS
	{
		unsigned	next	: NUM_BITS_CHUNK_COUNT;    // next linked chunk
		unsigned	isFree	: 1;			 			 // is chunk free

	}Chunks[NUM_CHUNKS];

	struct QUEUES
	{
		// first and last chunk of the queue and the indices into those chunks
		unsigned startChunk 	: NUM_BITS_CHUNK_COUNT;
		unsigned s_idx			: NUM_BITS_CHUNK_DATA_COUNT;

		unsigned endChunk		: NUM_BITS_CHUNK_COUNT;
		unsigned e_idx			: NUM_BITS_CHUNK_DATA_COUNT;

		unsigned isActive		: 1;  // is queue free to be used by createqueue

	}Queues[MAX_QUEUES];


	// To check the amount of memory used in bytes for queue management data structures (without padding);
	const int MEM_USED =	( (NUM_CHUNKS * (NUM_BITS_CHUNK_COUNT + 1)) / 8 ) +
							( (MAX_QUEUES * (NUM_BITS_CHUNK_COUNT * 2 + NUM_BITS_CHUNK_DATA_COUNT * 2 + 1)) / 8 );

	// Performs any necessary initialization of the QueueManager.
	void initializeQueueManager()
	{
		int i;
		//Initialzie the Chunks, Queues datastructures
		for ( i=0; i < NUM_CHUNKS; i++ )
		{
			Chunks.isFree = 0x1;
			Chunks.next = 0x0;
		}

		for ( i=0; i < MAX_QUEUES; i++ )
		{
			Queues.isActive = 0x0;
		}
	}

	// Creates a FIFO byte queue, returns the handle for the created
	// queue.
	QueueHandle createQueue()
	{
		for ( int i=0; i < MAX_QUEUES; i++ )
		{
			// Look for an inactive (free) queue
			if ( int ( Queues.isActive ) == 0x0 ) 
			{
				for ( int j=0; j < NUM_CHUNKS; j++ )
				{
					// Look for the first free chunk to allocate
					if ( int( Chunks[j].isFree ) == 0x1 )
					{
						Queues.startChunk = j;
						Queues.isActive = 0x1;
						Queues.endChunk = j;
						Queues.s_idx = 0x0;
						Queues.e_idx = 0x0;
						
						Chunks[j].isFree = 0x0;

						return ( i );  //The handle is simply the index into the Queues array
					}
				}			
				onOutOfMemory();  // f'n exit
			}
		}
		onIllegalOperation("Unable to create Queue. MAX_QUEUES limit reached"); // exit;
		return 0; // to avoid noreturn warning;
	}

	// Destroy the queue specified by queueHandle.
	void destroyQueue (QueueHandle queueHandle)
	{
		int i,j,k;

		// Check if queueHandle is valid;
		if ( !( queueHandle < MAX_QUEUES && queueHandle >= 0 ) )
		{
			onIllegalOperation("Invalid Handle : %d",(int)queueHandle);
		}
		
		i = (int)(queueHandle);
		Queues.isActive = 0x0;		

		// If queue is madeup of only 1 chunk;
		if ( int(Queues.startChunk) == int(Queues.endChunk) )
		{
			Chunks[ int(Queues.startChunk) ].isFree = 0x1;
			return;
		}


		// Free all the chunks allocated by the queue;
		j = Queues.startChunk;
		k = Queues.endChunk;

		Chunks.isFree = 0x1;
		Chunks[k].isFree = 0x1;

		while ( j != k )
		{
			Chunks[j].isFree = 0x1;
			j = Chunks[j].next;
		}
	}

	// Adds a new byte to the queue specified by queueHandle.
	void enQueue (QueueHandle queueHandle, unsigned char value)
	{
		int i,j,k;

		// Check if queueHandle is valid;
		if ( !( queueHandle < MAX_QUEUES && queueHandle >= 0 ) )
		{
			onIllegalOperation("Invalid Handle : %d",(int)queueHandle);
		}

		i = int( queueHandle );
		j = int( Queues.endChunk );
		k = int( Queues.e_idx );

		Data[ ( j * CHUNK_SIZE ) + k ] = value;
		
		// check if we have reached the chunk end;
		if ( (k+1) < CHUNK_SIZE )
		{
			Queues.e_idx = Queues.e_idx + 0x1;
		}
		else
		{
			for ( int n=0; n < NUM_CHUNKS; n++ )
			{
				// Allocate a fresh chunk for the next value to be enqued; 
				if ( int(Chunks[n].isFree) == 0x1 ) 
				{
					Chunks[n].isFree = 0x0;
					
					Chunks[j].next = n;
					Queues.endChunk = n;
					Queues.e_idx = 0x0;
					
					return;
				}
			}
			onOutOfMemory();
		}

	}

	// Pops the next byte off the queue specified by queueHandle.
	unsigned char deQueue (QueueHandle queueHandle)
	{
		int i,j,k;
		unsigned char val;

		// Check if queueHandle is valid;
		if ( !( queueHandle < MAX_QUEUES && queueHandle >= 0 ) )
		{
			onIllegalOperation("Invalid Handle : %d",(int)queueHandle);
		}

		i = int( queueHandle );
		j = int( Queues.startChunk );
		k = int( Queues.s_idx );

		// Check if Queue is empty;
		if	( (int(Queues.startChunk) == int(Queues.endChunk)) &&
					(int(Queues.s_idx) == int(Queues.e_idx))
				)
		{
			onIllegalOperation("Queue is empty");
		}

		val = Data[ j * CHUNK_SIZE + k];
		
		// check if we have reached the chunk end;
		if ( (k+1) < CHUNK_SIZE )
		{
			Queues.s_idx = Queues.s_idx + 0x1;
		}
		else
		{
			// update the start chunk to the next linked chunk

			Queues.startChunk = Chunks[j].next;
			Queues.s_idx = 0x0;

			// free current chunk
			Chunks[j].isFree = 0x1;
		}

		return val;
	}
};