/*
 * fifo.c
 *
 *  Created on: Jan 17, 2022
 *      Author: radioman
 */
#include "fifo.h"
#include <stdlib.h>
#include <string.h>

void fifo_init(FIFO_HandleTypeDef *fifo, uint8_t *buffer, uint32_t size)
{
	fifo->head = fifo->tail = 0;
	fifo->state = FIFO_EMPTY;
	fifo->size = size;
	fifo->data = buffer;
}

FIFO_StateTypeDef fifo_state(FIFO_HandleTypeDef *fifo)
{
	return fifo->state;
}

int fifo_size(FIFO_HandleTypeDef *fifo)
{
	if(fifo->state == FIFO_FULL)
		return fifo->size;

	if(fifo->state == FIFO_EMPTY)
		return 0;

	if(fifo->head >= fifo->tail)
		return (fifo->head - fifo->tail);
	else
		return (fifo->size - fifo->tail + fifo->head);
}

int fifo_read(FIFO_HandleTypeDef *fifo, uint8_t *buf, uint32_t len)
{
	if(fifo->state != FIFO_EMPTY)
	{
		if(fifo->tail >= fifo->head)
		{
			uint32_t cap_avail = fifo->size - fifo->tail + fifo->head;
			if((fifo->size - fifo->tail) >= len)
			{
				memcpy(buf, &(fifo->data[fifo->tail]), len);
				fifo->tail = (fifo->tail + len) % fifo->size;
			}
			else
			{
				if(len > cap_avail)
					len = cap_avail;
				uint32_t blk1_sz = fifo->size - fifo->tail;
				memcpy(buf, &(fifo->data[fifo->tail]), blk1_sz);
				fifo->tail = 0;
				memcpy(&buf[blk1_sz], &(fifo->data[fifo->tail]), (len - blk1_sz));
				fifo->tail = (len - blk1_sz) % fifo->size;
			}
		}
		else
		{
			if(len > (fifo->head - fifo->tail))
				len = fifo->head - fifo->tail;
			memcpy(buf, &(fifo->data[fifo->tail]), len);
			fifo->tail = (fifo->tail + len) % fifo->size;
		}

		if(fifo->tail == fifo->head)
			fifo->state = FIFO_EMPTY;
		else
			fifo->state = FIFO_READY;

		return (len);
	}
	else
	{
		return (-1);
	}
}

int fifo_write(FIFO_HandleTypeDef *fifo, uint8_t *data, uint32_t len)
{
	if(fifo->state != FIFO_FULL)
	{
		if(fifo->head >= fifo->tail)
		{
			uint32_t cap_avail = fifo->size - fifo->head + fifo->tail;
			if((fifo->size - fifo->head) >= len)
			{
				memcpy(&(fifo->data[fifo->head]), data, len);
				fifo->head = (fifo->head + len) % fifo->size;
			}
			else
			{
				if(len > cap_avail)
					len = cap_avail;
				uint32_t blk1_sz = fifo->size - fifo->head;
				memcpy(&(fifo->data[fifo->head]), data, blk1_sz);
				fifo->head = 0;
				memcpy(&(fifo->data[fifo->head]), &data[blk1_sz], (len - blk1_sz));
				fifo->head = (len - blk1_sz) % fifo->size;
			}
		}
		else
		{
			if(len > (fifo->tail - fifo->head))
				len = fifo->tail - fifo->head;
			memcpy(&(fifo->data[fifo->head]), data, len);
			fifo->head = (fifo->head + len) % fifo->size;
		}

		if(fifo->head == fifo->tail)
			fifo->state = FIFO_FULL;
		else
			fifo->state = FIFO_READY;

		return (len);
	}
	else
	{
		return (-1);
	}
}