#include <../source/TAL/objbase.h>

#define	FIFO_START_LEN		1024

void	V_GLOBAL
u8fInit(u8_fifo_p f)
{
	V_ASSERT(f);
	rbfInit(f,FIFO_START_LEN);
	/*
	 * Or, if you frequently create FIFOs, but don't use them:
	rbfInit(f,0);
	 * This works _absolutely_ similar, but conserves memory :)
	 */
}

void	V_GLOBAL
rbfInit(u8_fifo_p f,vsize_t fifo_size)
{
	V_ASSERT(f);
	f->len = fifo_size;
	if(fifo_size)
		f->start = (u8_p)memAlloc(f->len);
	else
		f->start = NULL;
	f->end = f->start + f->len;
	f->head = f->tail = f->start;
	f->space_avail = f->len;
}

void	V_GLOBAL
u8fPurge(u8_fifo_p f)
{
	V_ASSERT(f);
	f->head = f->tail = f->start;
	f->space_avail = f->len;
}

void	V_GLOBAL
u8fClear(u8_fifo_p f)
{
	V_ASSERT(f);
	memFree(f->start);
	f->space_avail = f->len = 0;
	f->end = f->head = f->tail = f->start = NULL;
}

V_LOCAL	void
u8fGrowBuffer(u8_fifo_p f,vsize_t nextlen)
{
	vsize_t l1, l2;
	u8_p ptr, newbuff;
	newbuff = (u8_p)memAlloc(nextlen);
	ptr = newbuff;
	RBF_CHAIN_LENS(f,l1,l2);
	if(l1)
	{
		memCopy(ptr,RBF_CHAIN1(f),l1);
		ptr += l1;
	}
	if(l2)
		memCopy(ptr,RBF_CHAIN2(f),l2);
	l1 += l2;
	f->len = nextlen;
	memFree(f->start);
	f->tail = f->start = newbuff;
	f->end = f->start + f->len;
	f->head = f->tail + l1;
	f->space_avail = f->len - l1;
}

void	V_GLOBAL
u8fPush(u8_fifo_p f,u8_t elem)
{
	vsize_t newlen;
	V_ASSERT(f);
	if(!f->space_avail)
	{
		NOT_LESS_POWER_TWO(newlen,f->len + 1,vsize_t);
		u8fGrowBuffer(f,newlen);
	}
	RBF_PUSH_NOCHECK(f,elem);
}

vboolean	V_GLOBAL
rbfPush(u8_fifo_p f,u8_t elem)
{
	V_ASSERT(f);
	if(!f->space_avail)
		return false;
	RBF_PUSH_NOCHECK(f,elem);
	return true;
}

void	V_GLOBAL
u8fPushBuffer(u8_fifo_p f,u8_t *buf,vsize_t nb)
{
	vsize_t l1, l2;
	vsize_t newlen;
	V_ASSERT(f && buf);
	while(f->space_avail < nb)
	{
		NOT_LESS_POWER_TWO(newlen,f->len + 1,vsize_t);
		u8fGrowBuffer(f,newlen);
	}
	RBF_FREE_CHAIN_LENS(f,l1,l2);
	if(nb < l1) /* fit in 1st chain */
	{
		l1 = nb;
		l2 = 0;
	}
	else
	{
		vsize_t nb2 = nb-l1;
		if(nb2 < l2)
			l2 = nb2;
	}
	if(l1)
	{
		memCopy(RBF_FREE_CHAIN1(f),buf,l1);
		buf += l1;
	}
	if(l2)
		memCopy(RBF_FREE_CHAIN2(f),buf,l2);
	RBF_MOVE_HEAD(f,nb);
}

vsize_t	V_GLOBAL
u8fPopBuffer(u8_fifo_p f,u8_t *buf,vsize_t nb)
{
	u8_p ptr = buf;
	vsize_t l1, l2, stored = f->len - f->space_avail;
	V_ASSERT(f && buf);
	if(nb > stored)
		nb = stored;
	RBF_CHAIN_LENS(f,l1,l2);
	if(l1 && nb)
	{
		if(l1 > nb)
			l1 = nb;
		memCopy(ptr,RBF_CHAIN1(f),l1);
		ptr += l1;
		nb -= l1;
	}
	if(l2 && nb)
	{
		if(l2 > nb)
			l2 = nb;
		memCopy(ptr,RBF_CHAIN2(f),l2);
		ptr += l2;
	}
	nb = ptr - buf;
	RBF_MOVE_TAIL(f,nb);
	return nb;
}

vsize_t V_GLOBAL
rbfPushBuffer(u8_fifo_p f,u8_t *buf,vsize_t nb)
{
	vsize_t l1, l2;
	V_ASSERT(f && buf);
	if(nb > f->space_avail)
		nb = f->space_avail;
	RBF_FREE_CHAIN_LENS(f,l1,l2);
	if(nb < l1) /* fit in 1st chain */
	{
		l1 = nb;
		l2 = 0;
	}
	else
	{
		vsize_t nb2 = nb-l1;
		if(nb2 < l2)
			l2 = nb2;
	}
	if(l1)
	{
		memCopy(RBF_FREE_CHAIN1(f),buf,l1);
		buf += l1;
	}
	if(l2)
		memCopy(RBF_FREE_CHAIN2(f),buf,l2);
	RBF_MOVE_HEAD(f,nb);
	return nb;
}

vboolean V_GLOBAL
u8fPop(u8_fifo_p f,u8_t *elem_ret)
{
	V_ASSERT(f && elem_ret);
	if(f->space_avail == f->len)
		return false;
	RBF_POP_NOCHECK(f,elem_ret);
	return true;
}

#ifdef	FIFO_NO_MACRO

vsize_t V_GLOBAL
u8fCount(u8_fifo_t *f)
{
	V_ASSERT(f);
	return f->len - f->space_avail;
}

#endif	/* FIFO_NO_MACRO */

/*
 * Implementation of 'block' FIFO
 */

void	V_GLOBAL
fifoInit(fifo_t *fifo,vsize_t element_size)
{
	V_ASSERT(fifo && element_size);
	memZero(fifo,sizeof(fifo_t));
	fifo->element_size=element_size;
}

void	V_GLOBAL
fifoClear(fifo_t *fifo)
{
	V_ASSERT(fifo);
	if(fifo->buf)
		memFree(fifo->buf);
	fifoInit(fifo,fifo->element_size);
}

void	V_GLOBAL
fifoPush(fifo_t *fifo,void *element)
{
	V_ASSERT(fifo && element);
	if(fifo->avail == 0)
	{
		/* Allocate new storage */
		u8_t *nextbuf;
		vsize_t nextlen,len,headlen,taillen;
		len = fifo->size-fifo->avail;
		headlen = fifo->size - (fifo->tail-fifo->buf) / fifo->element_size;
		if(headlen>len)
			headlen=len;
		taillen=len-headlen;
		GREATER_POWER_TWO(nextlen,fifo->size,vsize_t);
		nextbuf=(u8_t*)memAlloc(nextlen*fifo->element_size);
		/* Copy head bytes */
		if(headlen)
			memCopy(nextbuf,fifo->tail,headlen*fifo->element_size);
		/* Copy tail bytes */
		if(taillen)
			memCopy(nextbuf+headlen*fifo->element_size,fifo->buf,
				taillen*fifo->element_size);
		if(fifo->buf)
			memFree(fifo->buf);
		fifo->buf=nextbuf;
		fifo->limit=fifo->buf + nextlen * fifo->element_size;
		fifo->head=fifo->buf + len * fifo->element_size;
		fifo->tail=fifo->buf;
		fifo->size=nextlen;
		fifo->avail=fifo->size-len;
	}
	memCopy(fifo->head,element,fifo->element_size);
	fifo->head+=fifo->element_size;
	if(fifo->head == fifo->limit)
		fifo->head = fifo->buf;
	fifo->avail--;
}

void	V_GLOBAL
fifoPurge(fifo_t *fifo)
{
	V_ASSERT(fifo);
	fifo->head=fifo->tail=fifo->buf;
	fifo->avail=fifo->size;
}

vboolean	V_GLOBAL
fifoPop(fifo_t *fifo,void *element)
{
	V_ASSERT(fifo && element);
	if(fifo->avail == fifo->size)
		return false;
	memCopy(element,fifo->tail,fifo->element_size);
	fifo->tail+=fifo->element_size;
	if(fifo->tail == fifo->limit)
		fifo->tail = fifo->buf;
	fifo->avail++;
	return true;
}

#ifdef	FIFO_NO_MACRO

u32_t	V_GLOBAL
fifoCount(fifo_t *fifo)
{
	V_ASSERT(fifo);
	return fifo->size - fifo->avail;
}

#endif /* FIFO_NO_MACRO */
