#include <stm32f4xx_hal.h>

#include <iostream>
#include <string>
#include <cstring>
using namespace std;

#include "CPFcpp.hpp"
#include "ubloxGPS.hpp"

//TODO should this be inherited? Can it be inherited?
#include "FIFO.hpp"
FIFO msgQ(10, 256, "gpsMsgQ");

#include "mcuRealTimeClock.hpp"
mcuRealTimeClock mcuRTC_GPS = mcuRealTimeClock::getInstance();

void RXIdleCallback(UART_HandleTypeDef *huart);

const uint8_t getPosCmdText[] = "$PUBX,00*33\r\n";
const uint8_t getTimeCmdText[] = "$PUBX,04*37\r\n";
bool commandIP = false;
int8_t ubloxGPS::sendCommand(ubloxGPS::GPSCommands cmd, string &deQTimestamp, string &deQString)
{
	/* 1) Send a ubloxGPS command
	 * 2) Wait for a repsonse
	 * 3) If we get any response, check it.  Since the resonse might be incorrect, also check timeout
	 * 4) If no reponse, check timeout.
	 * 5) Done if respone is good
	 * 6) Error if no good response in timeout period
	 */
	 
	int8_t returnValue = 0;            	//<0 is an error, =0 is in process, >0 is success

	CommandParams cmdParams = CmdParam[cmd];
	
	string dateTimeString = "";
	char engrMsg[256] = "";
	
	uint8_t checkRandTResp = 0;
		
	if (!commandIP)	//Send the command
		{
			//TODO need to deal with stuff still in FIFO if any
			memset(uart7RXBuffer, 0, sizeof(uart7RXBuffer));
			deQString.clear();
			numTimeouts = 0;
			HAL_UART_Receive_DMA(&hUART7, uart7RXBuffer, cmdParams.responseSize);
			HAL_UART_Transmit_DMA(&hUART7, cmdParams.cmdPtr, cmdParams.cmdSize);
			commandIP = true;
			
			memset(engrMsg, 0, sizeof(engrMsg));
			snprintf(engrMsg, sizeof(engrMsg), "%s\t%s\tSending command: %s", IDTag, mcuRTC_GPS.getCPFTimestamp().c_str(), cmdParams.cmdLabel);
			sdLogger.logEngrMsg(engrMsg, sizeof(engrMsg));
	
			//TODO Set start time for checkTimeout()
			returnValue = 0;
		}
	else	//Wait for response or timeout
		{
			cout << "\t\tubloxGPS " << cmdParams.cmdLabel << ": checking for response" << endl;
			deQTimestamp = "";
			deQString = "";
			msgQ.dequeue(deQTimestamp, deQString);

			memset(engrMsg, 0, sizeof(engrMsg));
			snprintf(engrMsg, sizeof(engrMsg), "%s\t%s\tDequeued: %s  %s", IDTag, mcuRTC_GPS.getCPFTimestamp().c_str(), deQTimestamp.c_str(), deQString.c_str());
			sdLogger.logEngrMsg(engrMsg, sizeof(engrMsg));

			//TODO log the response
			checkRandTResp = checkRespAndTimeout(deQString, cmdParams.correctResponse);
			if (checkRandTResp > 0)
			{
				//Good response, we're done
				HAL_UART_DMAStop(&hUART7);
				cout << "\t\tubloxGPS " << cmdParams.cmdLabel << ": Got good response" << endl;
				commandIP = false;
				returnValue = 1;
			}
			else if (checkRandTResp == 0)
			{
				
				//cout << "\t\tubloxGPS " << cmdLabel << ": still waiting" << endl;
				returnValue = 0;
			}
			else if (checkRandTResp < 0)
			{
				if (checkRandTResp == (uint8_t) - 1)
				{
					cout << "\t\tubloxGPS " << cmdParams.cmdLabel << ": got wrong response" << endl;
					returnValue = 0;
				}
				else if (checkRandTResp == (uint8_t) - 2)
				{
					HAL_UART_DMAStop(&hUART7);
					cout << "\t\tubloxGPS " << cmdParams.cmdLabel << ": timed out" << endl;
					commandIP = false;
					returnValue = -1;
				}
				else
				{
					cout << "\t\tCheck response and timeout returned unknown negative value" << endl;
				}
			}
		}

	return returnValue;
}

int8_t ubloxGPS::checkRespAndTimeout(string &actualResponse, string &desiredResponse)
{
	//return 0 if response is good, return -1 if response is bad, return -2 if no response in appropriate time
	int8_t returnValue = 0;
	
	numTimeouts++;
	if (actualResponse.empty())
	{
		//TODO replace this simple counter with actual time check when the RTC is working
		if(numTimeouts > 15)
		{
			cout << "\t\tcheckResponse timed out" << endl;
			numTimeouts = 0;
			returnValue = -1;
		}
		else
		{
			//			cout << "\t\tcheckResponse still checking" << endl;
						returnValue = 0;
		}
	}
	else
	{
		if (actualResponse.compare(0, desiredResponse.length(), desiredResponse) == 0)
		{
			cout << "\t\tcheckResponse got good response" << endl;
			numTimeouts = 0;
			returnValue = 1;
		}
		else
		{
			cout << "\t\tcheckResponse got wrong response" << endl;
			returnValue = -1;
		}
	}
	return returnValue;
}

ubloxGPS::CommandParams ubloxGPS::setCmdParams(ubloxGPS::GPSCommands cmd)
{
	CommandParams cmdParams;
	
	if (cmd == getPos)
	{
		cmdParams.cmdPtr = (uint8_t *)getPosCmdText;
		cmdParams.cmdSize = sizeof(getPosCmdText);
		cmdParams.responseSize = 108;
		cmdParams.cmdTimeout = 1000;
		strcpy(cmdParams.cmdLabel, "Get position");
		cmdParams.correctResponse = "$PUBX,00";
	}
	else
	{
		cmdParams.cmdPtr = (uint8_t *)getTimeCmdText;
		cmdParams.cmdSize = sizeof(getTimeCmdText);
		cmdParams.responseSize = 66;
		cmdParams.cmdTimeout = 1000;
		strcpy(cmdParams.cmdLabel, "Get time");
		cmdParams.correctResponse = "$PUBX,04";
	}
	return cmdParams;
}

int8_t ubloxGPS::checkResponse()
{
	//return 0 if response is good, return -1 if response is bad, return -2 if no response in appropriate time
	cout << "\t\tuBlox checking response" << endl;
	return (0);
}

extern "C" void UART7_IRQHandler()
{
	//cout << "hit LPUART1_IRQHandler" << endl;

	HAL_UART_IRQHandler(&hUART7);

	if (__HAL_UART_GET_FLAG(&hUART7, UART_FLAG_IDLE))
	{
		RXIdleCallback(&hUART7);
		__HAL_UART_CLEAR_IDLEFLAG(&hUART7); 
	}
}

void RXIdleCallback(UART_HandleTypeDef *huart)
{
	HAL_UART_DMAStop(&hUART7);  //TODO this isn't actually stopping the Xfer, RXCpltCallback still fires.
	//TODO need to create a GPS message structure with timestamp, message, qCount?, message length?, etc.
	msgQ.enqueue(mcuRTC_GPS.getCPFTimestamp(), uart7RXBuffer);
	
	cout << endl << "\t\thit HAL_UART_RXIdleCallback" << "   uart7RXBuffer: " << uart7RXBuffer;
}

extern "C" void DMA1_Stream1_IRQHandler()
{
	cout << "\t\thit DMA1_Stream1_IRQHandler (TX)" << endl;
	HAL_DMA_IRQHandler(&hDMA_TX);
}

extern "C" void DMA1_Stream3_IRQHandler()
{
	cout << "\t\thit DMA1_Stream3_IRQHandler (RX)" << endl;
	HAL_DMA_IRQHandler(&hDMA_RX);
}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	HAL_UART_DMAStop(&hUART7);
	//TODO need to create a GPS message structure with timestamp, message, qCount?, message length?, etc.
//	msgQ.enqueue(mcuRTC.getCPFTimestamp(), uart7RXBuffer);
	
	cout << endl << "\t\tUART RX Complete Callback RX GPS Message: " << uart7RXBuffer;
}

void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart)
{
	cout << "\t\tUART RX Half Complete" << endl;
}

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
	cout << "\t\tUART TX Complete" << endl;
}

void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart)
{
	cout << "\t\tUART TX Half Complete" << endl;
}


void ubloxGPS::configUART7()
{
	/**UART7 GPIO Configuration    
	PF6     ------> UART7_RX
	PF7     ------> UART7_TX 
	*/
	__USART7_CLK_ENABLE();
	__GPIOF_CLK_ENABLE();
    
	GPIO_InitTypeDef GPIO_InitStructure;
 
	GPIO_InitStructure.Pin = GPIO_PIN_7;
	GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
	GPIO_InitStructure.Alternate = GPIO_AF8_UART7;
	GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
	GPIO_InitStructure.Pull = GPIO_NOPULL;
	HAL_GPIO_Init(GPIOF, &GPIO_InitStructure);
    
	GPIO_InitStructure.Pin = GPIO_PIN_6;
	GPIO_InitStructure.Mode = GPIO_MODE_AF_OD;
	HAL_GPIO_Init(GPIOF, &GPIO_InitStructure);
 
	hUART7.Instance        = UART7;
	hUART7.Init.BaudRate   = 9600;
	hUART7.Init.WordLength = UART_WORDLENGTH_8B;
	hUART7.Init.StopBits   = UART_STOPBITS_1;
	hUART7.Init.Parity     = UART_PARITY_NONE;
	hUART7.Init.HwFlowCtl  = UART_HWCONTROL_NONE;
	hUART7.Init.Mode       = UART_MODE_TX_RX;
    
	if (HAL_UART_Init(&hUART7) != HAL_OK)
	{
		cout << "Error in UART_Init" << endl;
		while (1) {}
	}
	
	NVIC_EnableIRQ(UART7_IRQn);
	__HAL_UART_ENABLE_IT(&hUART7, UART_IT_IDLE);
			
	//Setup UART TX DMA
	__DMA1_CLK_ENABLE();
	hDMA_TX.Instance = DMA1_Stream1;
	hDMA_TX.Init.Channel = DMA_CHANNEL_5;
	hDMA_TX.Init.Direction = DMA_MEMORY_TO_PERIPH;
	hDMA_TX.Init.PeriphInc = DMA_PINC_DISABLE;
	hDMA_TX.Init.MemInc = DMA_MINC_ENABLE;
	//hDMA.Init.Mode = DMA_CIRCULAR;
    hDMA_TX.Init.Mode = DMA_NORMAL;
	//hDMA.Init.Priority = DMA_PRIORITY_VERY_HIGH;
	hDMA_TX.Init.Priority = DMA_PRIORITY_LOW;
	hDMA_TX.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
	hDMA_TX.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
 
	if (HAL_DMA_Init(&hDMA_TX) != HAL_OK)
		asm("bkpt 255");
    
	__HAL_LINKDMA(&hUART7, hdmatx, hDMA_TX);
	NVIC_EnableIRQ(DMA1_Stream1_IRQn);

	//Setup UART RX DMA
	hDMA_RX.Instance = DMA1_Stream3;
	hDMA_RX.Init.Channel = DMA_CHANNEL_5;
	hDMA_RX.Init.Direction = DMA_PERIPH_TO_MEMORY;
	hDMA_RX.Init.PeriphInc = DMA_PINC_DISABLE;
	hDMA_RX.Init.MemInc = DMA_MINC_ENABLE;
	//hDMA.Init.Mode = DMA_CIRCULAR;
    hDMA_RX.Init.Mode = DMA_NORMAL;
	//hDMA.Init.Priority = DMA_PRIORITY_VERY_HIGH;
	hDMA_RX.Init.Priority = DMA_PRIORITY_HIGH;
	hDMA_RX.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
	hDMA_RX.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
 
	if (HAL_DMA_Init(&hDMA_RX) != HAL_OK)
		asm("bkpt 255");
    
	__HAL_LINKDMA(&hUART7, hdmarx, hDMA_RX);
	NVIC_EnableIRQ(DMA1_Stream3_IRQn);
}