/*
 * datafifo.c
 *
 *  Created on: 1 Jul 2018
 *      Author: nats
 */

#include "datafifo.h"
#include "radio.h"

packet *fifo[FIFOSIZE];

uint16_t readp = 0;
uint16_t writep = 0;

uint8_t full = 0;
uint8_t empty = 1;

uint8_t add_packet(packet *p) {
	check_full();
	if(full) {
		return FIFOERROR;
	} else {
		fifo[writep] = p;
		writep++;
		return FIFOOK;
	}
}

uint8_t read_packet(packet *p) {
	check_empty();
	if(empty) {
		return FIFOERROR;
	} else {
		p = fifo[readp];
		readp++;
		return FIFOOK;
	}
}

uint8_t check_full() {
	uint8_t wmask = writep & FIFOMASK;
	uint8_t rmask = readp & FIFOMASK;

	uint8_t overwbit = (writep & FIFOSIZE);
	uint8_t overrbit = (readp & FIFOSIZE);

	if((wmask == rmask-1) && (overwbit != overrbit)) {
		full = 1;
		return FULL;
	} else {
		full = 0;
		return READY;
	}
}

uint8_t check_empty() {
	uint8_t wmask = writep & FIFOMASK;
	uint8_t rmask = readp & FIFOMASK;

	uint8_t overwbit = (writep & FIFOSIZE);
	uint8_t overrbit = (readp & FIFOSIZE);

	if(overwbit == overrbit) {
		writep = writep & FIFOMASK;
		readp = readp & FIFOMASK;
	}

	if((wmask-1 == rmask) && (overwbit == overrbit)) {
		empty = 1;
		return EMPTY;
	} else {
		empty = 0;
		return READY;
	}
}

void init_fifo() {
	for(uint8_t i = 0; i < FIFOSIZE; i++) {
		uint8_t *datap = fifo[i];
		datap[0] = START_TX;
	}
}
