#include <array>

#include "mcuRealTimeClock.hpp"

const string mcuRealTimeClock::getCPFTimestamp(void)
{
	RTC_DateTypeDef dateNow;
	RTC_TimeTypeDef timeNow;
	char cTimestamp[32] = "";
	string sTimestamp = "";
	uint16_t milliSeconds = 0;
	
	HAL_RTC_GetTime(&RtcHandle, &timeNow, RTC_FORMAT_BIN);
	HAL_RTC_GetDate(&RtcHandle, &dateNow, RTC_FORMAT_BIN);
	milliSeconds = (int)(1000.0 * ((float)timeNow.SubSeconds / ((float)timeNow.SecondFraction + 1.0)));

	snprintf(cTimestamp, sizeof(cTimestamp), "%02d-%02d-%02dT%02d:%02d:%02d.%03d", dateNow.Year + 2000, dateNow.Month, dateNow.Date,
		timeNow.Hours, timeNow.Minutes, timeNow.Seconds, milliSeconds);
	sTimestamp = cTimestamp;
	return sTimestamp;
}

void mcuRealTimeClock::SynchToPUBX04(string timeMsg)
{
	RTC_DateTypeDef RTCDateSet;
	RTC_DateTypeDef RTCDateGet;
	RTC_TimeTypeDef RTCTimeSet;
	RTC_TimeTypeDef RTCTimeGet;
	
	uint8_t formattedTime[50] = { 0 };
	uint8_t formattedDate[50] = { 0 };
	
	//TODO might need to set day of week properly for some future reason
	if(timeMsg.find("PUBX,04") != string::npos)
	{
		HAL_RTC_GetTime(&RtcHandle, &RTCTimeGet, RTC_FORMAT_BIN);
		HAL_RTC_GetDate(&RtcHandle, &RTCDateGet, RTC_FORMAT_BIN);
		snprintf((char *)formattedTime, sizeof(formattedTime), "%2d:%2d:%2d", RTCTimeGet.Hours, RTCTimeGet.Minutes, RTCTimeGet.Seconds);
		snprintf((char *)formattedDate, sizeof(formattedDate), "%2d-%2d-%2d", RTCDateGet.Month, RTCDateGet.Date, 2000 + RTCDateGet.Year);
		cout << "\t\tBefore synch: " << formattedDate << "  " << formattedTime << endl; 

		RTCDateSet.Year = (timeMsg[23] - 48) * 10 + (timeMsg[24] - 48);
		if (RTCDateSet.Year > 18)
		{
			RTCTimeSet.Hours = (timeMsg[9] - 48) * 10 + (timeMsg[10] - 48);
			RTCTimeSet.Minutes = (timeMsg[11] - 48) * 10 + (timeMsg[12] - 48);
			RTCTimeSet.Seconds = (timeMsg[13] - 48) * 10 + (timeMsg[14] - 48);
			RTCTimeSet.DayLightSaving = RTC_DAYLIGHTSAVING_NONE;
			RTCTimeSet.TimeFormat = RTC_HOURFORMAT_24;
		
			RTCDateSet.Date = (timeMsg[19] - 48) * 10 + (timeMsg[20] - 48);
			RTCDateSet.Month = (timeMsg[21] - 48) * 10 + (timeMsg[22] - 48);
			RTCDateSet.WeekDay = 0;

			snprintf((char *)formattedTime, sizeof(formattedTime), "%2d:%2d:%2d", RTCTimeSet.Hours, RTCTimeSet.Minutes, RTCTimeSet.Seconds);
			snprintf((char *)formattedDate, sizeof(formattedDate), "%2d-%2d-%2d", RTCDateSet.Month, RTCDateSet.Date, 2000 + RTCDateSet.Year);
			cout << "\t\tAbout to set: " << formattedDate << "  " << formattedTime << endl; 
			if (HAL_RTC_SetDate(&RtcHandle, &RTCDateSet, RTC_FORMAT_BIN) != HAL_OK)
			{
				cout << "Error in HAL_RTC_SetDate" << endl;
			}
			if (HAL_RTC_SetTime(&RtcHandle, &RTCTimeSet, RTC_FORMAT_BIN) != HAL_OK)
			{
				cout << "Error_Handler();" << endl;
			}
			
			HAL_RTC_GetTime(&RtcHandle, &RTCTimeGet, RTC_FORMAT_BIN);
			HAL_RTC_GetDate(&RtcHandle, &RTCDateGet, RTC_FORMAT_BIN);
			snprintf((char *)formattedTime, sizeof(formattedTime), "%2d:%2d:%2d", RTCTimeGet.Hours, RTCTimeGet.Minutes, RTCTimeGet.Seconds);
			snprintf((char *)formattedDate, sizeof(formattedDate), "%2d-%2d-%2d", RTCDateGet.Month, RTCDateGet.Date, 2000 + RTCDateGet.Year);
			cout << "\t\tAfter synch: " << formattedDate << "  " << formattedTime << endl; 
			cout << "\t\tCPF Time Stamp: " << getCPFTimestamp() << endl;
		}
	}
}

void mcuRealTimeClock::configRTC()
{
	RtcHandle.Instance = RTC; 
	RtcHandle.Init.HourFormat = RTC_HOURFORMAT_24;
	RtcHandle.Init.AsynchPrediv = RTC_ASYNCH_PREDIV; 
	RtcHandle.Init.SynchPrediv = RTC_SYNCH_PREDIV; 
	RtcHandle.Init.OutPut = RTC_OUTPUT_DISABLE;
	RtcHandle.Init.OutPutPolarity = RTC_OUTPUT_POLARITY_HIGH;
	RtcHandle.Init.OutPutType = RTC_OUTPUT_TYPE_OPENDRAIN;
	__HAL_RTC_RESET_HANDLE_STATE(&RtcHandle);
	if (HAL_RTC_Init(&RtcHandle) != HAL_OK)
	{
		cout << "Error initializing RTC" << endl;
	}
	//__HAL_RCC_CLEAR_RESET_FLAGS();
}

//TODO This subroutine is actually part of stm32f4xx_hal_msp.c but if I include 
//the whole header file it causes errors that I can't fix.  It would be nice to figure out why.
void HAL_RTC_MspInit(RTC_HandleTypeDef *hrtc)
{
	RCC_OscInitTypeDef        RCC_OscInitStruct;
	RCC_PeriphCLKInitTypeDef  PeriphClkInitStruct;

	/*##-1- Enables the PWR Clock and Enables access to the backup domain ###################################*/
	/* To change the source clock of the RTC feature (LSE, LSI), You have to:
	   - Enable the power clock using __HAL_RCC_PWR_CLK_ENABLE()
	   - Enable write access using HAL_PWR_EnableBkUpAccess() function before to 
	     configure the RTC clock source (to be done once after reset).
	- Reset the Back up Domain using __HAL_RCC_BACKUPRESET_FORCE() and 
	  __HAL_RCC_BACKUPRESET_RELEASE().
	- Configure the needed RTc clock source */
	__HAL_RCC_PWR_CLK_ENABLE();
	HAL_PWR_EnableBkUpAccess();
  
	/*##-2- Configure LSE as RTC clock source ###################################*/
	RCC_OscInitStruct.OscillatorType =  RCC_OSCILLATORTYPE_LSE;
	RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
	RCC_OscInitStruct.LSEState = RCC_LSE_ON;
	if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
	{ 
		cout << "HAL_RCC_OscConfig Error_Handler()" << endl;
	}
  
	PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_RTC;
	PeriphClkInitStruct.RTCClockSelection = RCC_RTCCLKSOURCE_LSE;
	if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
	{ 
		//		Error_Handler();
				cout << "HAL_RCCEx_PeriphCLK_Config Error_Handler()" << endl;
	}
  
	/*##-3- Enable RTC peripheral Clocks #######################################*/
	/* Enable RTC Clock */
	__HAL_RCC_RTC_ENABLE();
}