#include <iostream>
#include <cstdint>

#include "FIFOQueue.hpp"

//From https://www.techiedelight.com/queue-implementation-cpp/
//See also http://www.simplyembedded.org/tutorials/interrupt-free-ring-buffer/


void FIFOQueue::clearFIFO()
{
	head = 0;
	tail = -1;
	count = 0;
}

// Utility function to remove front element from the queue
int32_t FIFOQueue::dequeue(RTClock::CPFTimestamp timestamp, uint8_t *deQMsg)
{
	// check for queue underflow
	if (isEmpty())
	{
		std::cout << qName << " FIFO empty" << std::endl;
		return (-1);
	}

	timestamp = fifoRecords[head].timestamp;
	//recordID = fifoRecords[head].recordID;
	memcpy(deQMsg, fifoRecords[head].message, qWidth);
	//deQString = fifoRecords[head].message;

	std::cout << "FIFO count = " << count << " fifoRecord Dequeued timetick / timestamp:  " 
		<< timestamp.data() << "  and msg: " << deQMsg << std::endl; 
	
	head = (head + 1) % qDepth;
	count--;
	return (count);
}

int32_t FIFOQueue::dequeue(FIFORecord &deQRecord)
{
	// check for queue underflow
	if(isEmpty())
	{
		std::cout << qName << " FIFO empty" << std::endl;
		return (-1);
	}
	else
	{
		deQRecord = fifoRecords[head];
	
		head = (head + 1) % qDepth;
		count--;
		return (count);
	}
}

int32_t FIFOQueue::enqueue(RTClock::CPFTimestamp timestamp, uint8_t inMsg[])
{
	// check for queue overflow
	if (isFull())
	{
		std::cout << "Q OverFlow, nothing queued\r\n";
		return -1;
	}
	else
	{
		tail = (tail + 1) % qDepth;

		fifoRecords[tail].timestamp = timestamp;
		memcpy(fifoRecords[tail].message, inMsg, 256);
		count++;
	}
	return count;
}

int32_t FIFOQueue::enqueue(FIFORecord enQRecord)
{
	// check for queue overflow
	if(isFull())
	{
		std::cout << "OverFlow\nQueue Overflow, nothing queued\n";
		return -1;
	}
	else
	{
		tail = (tail + 1) % qDepth;

		fifoRecords[tail] = enQRecord;
		count++;
		return count;
	}
}

// Utility function to return front element in the queue
int FIFOQueue::peek()
{
	if (isEmpty())
	{
		std::cout << "UnderFlow\nProgram Terminated\n";
	}
	return 0;
}

// Utility function to return the size of the queue
int FIFOQueue::getCount()
{
	return count;
}

// Utility function to check if the queue is empty or not
bool FIFOQueue::isEmpty()
{
	return (getCount() <= 0);
}

// Utility function to check if the queue is full or not
bool FIFOQueue::isFull()
{
	return (getCount() == qDepth);
}

void FIFOQueue::clearRecord(FIFORecord &inRecord)
{
	inRecord.timestamp.fill('0');
	//inRecord.recordID = undefined;
	memset(inRecord.message, 0, sizeof(inRecord.message));
}