/*
 * fifo.c
 *
 * Created: 20.02.2019 20:33:06
 *  Author: jens
 */ 

#include <stdint.h>
#include "fifo.h"

static volatile size_t criticalCount = 0;

void enterCritical()
{
	cli();
	criticalCount++;
}

void exitCritial()
{
	if (criticalCount)
	{
		criticalCount--;
		if (!criticalCount)
		{
			sei();
		}
	}
}

void fifo_init(Fifo *pObj, size_t capacity, size_t itemSize, const char *pName)
{
	pObj->wi = 0;
	pObj->ri = 0;
	pObj->fill = 0;
	pObj->capacity = capacity;
	pObj->itemSize = itemSize;
	pObj->ppData = (uint8_t**)malloc(capacity*sizeof(uint8_t*));
	
	for (int i=0; i < capacity; i++)
	{
		pObj->ppData[i] = (uint8_t*)malloc(itemSize);
	}
	pObj->pName = pName;
}

void fifo_free(Fifo *pObj)
{
	for (int i=0; i < pObj->capacity; i++)
	{
		free(pObj->ppData[i]);
	}
	free (pObj->ppData);
	pObj->itemSize = 0;
	pObj->capacity = 0;
}

int fifo_push(Fifo *pObj, void *pItem)
{
	int success = 0;
	enterCritical();
	do 
	{
		if (pObj->fill >= pObj->capacity)
		{
			break;
		}
		for (size_t i=0; i < pObj->itemSize; i++)
		{
			pObj->ppData[pObj->wi][i] = ((uint8_t*)pItem)[i];
		}
		pObj->wi++;
		pObj->fill++;
		if (pObj->wi == pObj->capacity)
		{
			pObj->wi = 0;
		}
		success = 1;
	} while (0);
	exitCritial();
	return success;
}

int fifo_pop(Fifo *pObj, void *pItem)
{
	int success = 0;
	enterCritical();
	do
	{
		if (!pObj->fill)
		{
			break;
		}
		for (size_t i=0; i < pObj->itemSize; i++)
		{
			((uint8_t*)pItem)[i] = pObj->ppData[pObj->ri][i];
		}
		pObj->ri++;
		pObj->fill--;
		if (pObj->ri == pObj->capacity)
		{
			pObj->ri = 0;
		}
		success = 1;
	} while(0);
	exitCritial();
	return success;
}

int fifo_isEmpty(Fifo *pObj)
{
	enterCritical();
	int isEmpty = (pObj->fill == 0);
	exitCritial();

	return isEmpty;
}