#include <iostream>
#include <ctime>
#include <iomanip>

#include "stm32h7xx_ll_bus.h"
#include "stm32h7xx_ll_rcc.h"
#include "stm32h7xx_ll_gpio.h"
#include "stm32h7xx_ll_rtc.h"
#include "stm32h7xx_ll_utils.h"
#include "stm32h7xx_ll_exti.h"

#include "RTClock.hpp"
#include "StateMachine.hpp"

#define USE_TIMEOUT       0

#define RTC_ERROR_NONE    0
#define RTC_ERROR_TIMEOUT 1


static void MX_RTC_Init(void);
uint32_t Enter_RTC_InitMode(void);
uint32_t WaitForSynchro_RTC(void);
uint32_t Exit_RTC_InitMode(void);


constexpr uint8_t MONTH_ARR_SIZE = 13;
static const std::string monthText[MONTH_ARR_SIZE] = {
	"Jan",
	"Feb",
	"Mar",
	"Apr",
	"May",
	"Jun",
	"Jul",
	"Aug",
	"Sep",
	"Oct",
	"Nov",
	"Dec"
};

void AlarmA_Callback()
{
	std::cout << RTClock::getDateTimeStamp().data() << "   Hit AlarmA_CallbackCPF" << std::endl;
}

void RTC_Alarm_IRQHandler(void)
{
	/* USER CODE BEGIN RTC_Alarm_IRQn 0 */
	if (LL_RTC_IsEnabledIT_ALRA(RTC) != 0)
	{
		/* Get the pending status of the Alarm Interrupt */
		if (LL_RTC_IsActiveFlag_ALRA(RTC) != 0)
		{
			/* Alarm callback */
			AlarmA_Callback();

			/* Clear the Alarm interrupt pending bit */
			LL_RTC_ClearFlag_ALRA(RTC);
		}
	}
	/* Clear the EXTI's Flag for RTC Alarm */
	LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_17);
	/* USER CODE END RTC_Alarm_IRQn 0 */
	/* USER CODE BEGIN RTC_Alarm_IRQn 1 */

	/* USER CODE END RTC_Alarm_IRQn 1 */
}

bool RTClock::setAlarm(RTClock::AlarmNames alarmNum, time_t alarmTime)
{
	LL_RTC_AlarmTypeDef rtc_alarm_initstruct;
	tm* alarmHMS = { };
	
	alarmHMS = localtime(&alarmTime);
	
	uint32_t alarmHour = alarmHMS->tm_hour;
	uint32_t alarmMinute = alarmHMS->tm_min;
	uint32_t alarmSecond = alarmHMS->tm_sec;
	uint32_t alarmDay = alarmHMS->tm_mday;

	std::cout << "Set state: " << StateMachine::getStateLabel() << "  alarm to dd:HH:MM:SS = " << std::setw(2) << std::setfill('0') << 
		alarmDay << ":" << 
		alarmHour << ":" << 
		alarmMinute << ":" << 
		alarmSecond << std::endl;
	
	LL_RTC_AlarmTypeDef alarmStruct = {};
	alarmStruct.AlarmDateWeekDay = alarmDay;
	alarmStruct.AlarmDateWeekDaySel = LL_RTC_ALMA_DATEWEEKDAYSEL_DATE;
	alarmStruct.AlarmMask = LL_RTC_ALMA_MASK_NONE;
	
	alarmStruct.AlarmTime.Hours = alarmHour;
	alarmStruct.AlarmTime.Minutes = alarmMinute;
	alarmStruct.AlarmTime.Seconds = alarmSecond;
			
	/*##-1- Disable RTC registers write protection ############################*/
	LL_RTC_DisableWriteProtection(RTC);

	/*##-2- Enter in initialization mode ######################################*/
	if (Enter_RTC_InitMode() != RTC_ERROR_NONE)
	{
		/* Initialization Error */
		std::cout << "ERROR** Enter_RTC_InitMode()" << std::endl;
	}
	
	if (alarmNum == alarmA)
	{
		LL_RTC_ALMA_Disable(RTC);
		//	auto returnValue = LL_RTC_ALMA_Init(RTC, LL_RTC_FORMAT_BIN, &alarmStruct);
		LL_RTC_ALMA_ConfigTime(RTC,
			LL_RTC_ALMA_TIME_FORMAT_AM, 
			__LL_RTC_CONVERT_BIN2BCD(alarmHour), 
			__LL_RTC_CONVERT_BIN2BCD(alarmMinute), 
			__LL_RTC_CONVERT_BIN2BCD(alarmSecond)); 
		LL_RTC_ALMA_SetDay(RTC, __LL_RTC_CONVERT_BIN2BCD(alarmDay));
		LL_RTC_ALMA_SetMask(RTC, LL_RTC_ALMA_MASK_NONE);

		/* Note: following interfaces may be used but not needed as default values are used.*/
		//LL_RTC_ALMA_SetSubSecond(RTC, 0x00);
		//LL_RTC_ALMA_SetSubSecondMask(RTC, 0);
		//LL_RTC_ALMA_DisableWeekday(RTC);
		//LL_RTC_ALMA_SetDay(RTC, 0x01);

		/* Enable Alarm*/
		LL_RTC_ALMA_Enable(RTC);

		/* Clear the Alarm interrupt pending bit */
		LL_RTC_ClearFlag_ALRA(RTC);

		/* Enable IT Alarm */
		LL_RTC_EnableIT_ALRA(RTC);
	}
	else
	{
		LL_RTC_ALMB_Disable(RTC);
		LL_RTC_ALMB_ConfigTime(RTC,
			LL_RTC_ALMB_TIME_FORMAT_AM, 
			__LL_RTC_CONVERT_BIN2BCD(alarmHour), 
			__LL_RTC_CONVERT_BIN2BCD(alarmMinute), 
			__LL_RTC_CONVERT_BIN2BCD(alarmSecond)); 
		LL_RTC_ALMB_SetDay(RTC, __LL_RTC_CONVERT_BIN2BCD(alarmDay));
		LL_RTC_ALMB_SetMask(RTC, LL_RTC_ALMB_MASK_NONE);

		/* Note: following interfaces may be used but not needed as default values are used.*/
		//LL_RTC_ALMB_SetSubSecond(RTC, 0x00);
		//LL_RTC_ALMB_SetSubSecondMask(RTC, 0);
		//LL_RTC_ALMB_DisableWeekday(RTC);
		//LL_RTC_ALMB_SetDay(RTC, 0x01);

		/* Enable Alarm*/
		LL_RTC_ALMB_Enable(RTC);

		/* Clear the Alarm interrupt pending bit */
		LL_RTC_ClearFlag_ALRB(RTC);

		/* Enable IT Alarm */
		LL_RTC_EnableIT_ALRB(RTC);
	}
	/* RTC Alarm Interrupt Configuration: EXTI configuration */
	LL_EXTI_EnableIT_0_31(LL_EXTI_LINE_17);
	LL_EXTI_EnableRisingTrig_0_31(LL_EXTI_LINE_17);

	/*##-6- Configure the NVIC for RTC Alarm ###############################*/
	NVIC_SetPriority(RTC_Alarm_IRQn, 0x0F);
	NVIC_EnableIRQ(RTC_Alarm_IRQn);

	/*##-7- Exit of initialization mode #######################################*/
	if (Exit_RTC_InitMode() != RTC_ERROR_NONE)
	{
		/* Initialization Error */
		std::cout << "ERROR** Exit_RTC_InitMode()" << std::endl;
	}

	/*##-8- Enable RTC registers write protection #############################*/
	LL_RTC_EnableWriteProtection(RTC);
}

void RTClock::clearAlarm(AlarmNames alarmNum)
{
	LL_RTC_DisableWriteProtection(RTC);
	if (alarmNum == alarmA)
	{
		LL_RTC_ALMA_Disable(RTC);
		LL_RTC_ClearFlag_ALRA(RTC);
		LL_RTC_DisableIT_ALRA(RTC);
	}
	else
	{
		LL_RTC_ALMB_Disable(RTC);
		LL_RTC_ClearFlag_ALRB(RTC);
		LL_RTC_DisableIT_ALRB(RTC);
	}
	LL_RTC_DisableWriteProtection(RTC);
}

tm RTClock::getDateTimeStruct()
{
	tm dtStruct = { 0 };
	
	//NOTE: STM RTC requires reading the time register before the data register in order to synch date and time read
	dtStruct.tm_hour = __LL_RTC_CONVERT_BCD2BIN(LL_RTC_TIME_GetHour(RTC));
	dtStruct.tm_min = __LL_RTC_CONVERT_BCD2BIN(LL_RTC_TIME_GetMinute(RTC));
	dtStruct.tm_sec = __LL_RTC_CONVERT_BCD2BIN(LL_RTC_TIME_GetSecond(RTC));
	dtStruct.tm_year = __LL_RTC_CONVERT_BCD2BIN(LL_RTC_DATE_GetYear(RTC)) + 100;
	dtStruct.tm_mon = __LL_RTC_CONVERT_BCD2BIN(LL_RTC_DATE_GetMonth(RTC)) - 1;
	dtStruct.tm_mday = __LL_RTC_CONVERT_BCD2BIN(LL_RTC_DATE_GetDay(RTC));
	
	return dtStruct;
}

void RTClock::initRTC()
{
	MX_RTC_Init();
}

void RTClock::synchToGPS(std::string time, std::string date)
{
	LL_RTC_TimeTypeDef timeToSet = { 0 };
	LL_RTC_DateTypeDef dateToSet = { 0 };
	
	ErrorStatus status = ERROR;
	
	CPFTimestamp timeBeforeSet = RTClock::getDateTimeStamp();
	std::cout << "Time before set: " << timeBeforeSet.data() << std::endl;
	
	uint32_t hours = stoi(time.substr(0, 2));
	uint32_t minutes = stoi(time.substr(2, 2));
	uint32_t seconds = stoi(time.substr(4, 2));
	
	uint32_t day = stoi(date.substr(0, 2));
	uint32_t month = stoi(date.substr(2, 2));
	uint32_t year = stoi(date.substr(4, 2));
		
	LL_RTC_DisableWriteProtection(RTC);

	if (LL_RTC_EnterInitMode(RTC) != ERROR)
	{
		LL_RTC_TIME_Config(RTC,
			LL_RTC_FORMAT_BCD, 
			__LL_RTC_CONVERT_BIN2BCD(hours), 
			__LL_RTC_CONVERT_BIN2BCD(minutes), 
			__LL_RTC_CONVERT_BIN2BCD(seconds));

		LL_RTC_DATE_Config(RTC,
			LL_RTC_WEEKDAY_SUNDAY,
			__LL_RTC_CONVERT_BIN2BCD(day), 
			__LL_RTC_CONVERT_BIN2BCD(month), 
			__LL_RTC_CONVERT_BIN2BCD(year));

		/* Exit Initialization mode */
		LL_RTC_DisableInitMode(RTC);

		/* If  RTC_CR_BYPSHAD bit = 0, wait for synchro else this check is not needed */
		if (LL_RTC_IsShadowRegBypassEnabled(RTC) == 0U)
		{
			status = LL_RTC_WaitForSynchro(RTC);
		}
		else
		{
			status = SUCCESS;
		}
	}
	/* Enable the write protection for RTC registers */
	LL_RTC_EnableWriteProtection(RTC);
	
	CPFTimestamp timeAfterSet = RTClock::getDateTimeStamp();
	std::cout << "Time after set: " << timeAfterSet.data() << std::endl;
	
	CPFTimestamp fastStamp= RTClock::getDateTimeStampFast();
	std::cout << "Fast time stamp: " << fastStamp.data() << std::endl;
}



RTClock::CPFTimestamp RTClock::getDateTimeStampFast()
{
	RTClock::CPFTimestamp fastTimestamp = { 0, 0, '-', 0, 0, '-', 0, 0, 'T', 0, 0, ':', 0, 0, ':', 0, 0, '.', 0, 0, 0, 0 };

	const uint8_t asciiOffset = '0';
	uint32_t tr;
	float ssr; //TODO need to check for all 3 registers rolling over
	uint32_t dr;
	uint8_t yearTens;
	uint8_t yearOnes;
	uint8_t monthTens;
	uint8_t monthOnes;
	uint8_t dayTens;
	uint8_t dayOnes;
	uint8_t hoursTens;
	uint8_t hoursOnes;
	uint8_t minuteTens;
	uint8_t minuteOnes;
	uint8_t secondTens;
	uint8_t secondOnes;
	
	ssr = (float)(RTC->SSR & 0x0000FFFF); 
	tr = RTC->TR;
	dr = RTC->DR;
	float prediv_s = (float)LL_RTC_GetSynchPrescaler(RTC);

	float secFrac = (prediv_s - ssr) / (prediv_s + 1);

	yearTens = (dr & 0x00F00000) >> 20;
	yearOnes = (dr & 0x000F0000) >> 16;
	fastTimestamp[0] = yearTens + asciiOffset;
	fastTimestamp[1] = yearOnes + asciiOffset;
	
	monthTens = (dr & 0x00001000) >> 12;
	monthOnes = (dr & 0x00000F00) >> 8;
	fastTimestamp[3] = monthTens + asciiOffset;
	fastTimestamp[4] = monthOnes + asciiOffset;
	
	dayTens = (dr & 0x00000030) >> 4;
	dayOnes = (dr & 0x0000000F);
	fastTimestamp[6] = dayTens + asciiOffset;
	fastTimestamp[7] = dayOnes + asciiOffset;
	
	hoursTens = (tr & 0x00300000) >> 20;
	hoursOnes = (tr & 0x000F0000) >> 16;
	fastTimestamp[9] = hoursTens + asciiOffset;
	fastTimestamp[10] = hoursOnes + asciiOffset;
	
	minuteTens = (tr & 0x00007000) >> 12;
	minuteOnes = (tr & 0x00000F00) >> 8;
	fastTimestamp[12] = minuteTens + asciiOffset;
	fastTimestamp[13] = minuteOnes + asciiOffset;
	
	secondTens = (tr & 0x00000070) >> 4;
	secondOnes = (tr & 0x0000000F);
	fastTimestamp[15] = secondTens + asciiOffset;
	fastTimestamp[16] = secondOnes + asciiOffset;
	
	uint8_t secTenths = (uint8_t)(10 * secFrac);
	fastTimestamp[18] = secTenths + asciiOffset;
	secFrac = secFrac - (secTenths * 0.1);
	uint8_t secHundredths = (uint8_t)(100 * secFrac);
	fastTimestamp[19] = secHundredths + asciiOffset;
	secFrac = secFrac - (secHundredths * 0.01);
	uint8_t secThousandths = (uint8_t)(1000 * secFrac);
	fastTimestamp[20] = secThousandths + asciiOffset;
	
	return fastTimestamp;
}

RTClock::CPFTimestamp RTClock::getDateTimeStamp()
{
	RTClock::CPFTimestamp timestamp = { 0 };	//	In RTClock.hpp CPFTimestamp is typedef std::array<char, 32> CPFTimestamp;
	
	float ssr = (float)(RTC->SSR & 0x0000FFFF); 
	float subsec = (float)LL_RTC_TIME_GetSubSecond(RTC);
	float prediv_s = (float)LL_RTC_GetSynchPrescaler(RTC);

	float secFrac = (prediv_s - ssr) / (prediv_s + 1);
	float secWhole = (float)__LL_RTC_CONVERT_BCD2BIN(LL_RTC_TIME_GetSecond(RTC));
	float sec = secWhole + secFrac;
	
	//NOTE: STM RTC requires reading the time register before the data register in order to synch date and time read
	//NOTE: %6.3f won't work unless printf support is enabled in project settings
	sprintf((char*)timestamp.data(), 
		"%02d-%02d-%02dT%02d:%02d:%2d.%03d",
		__LL_RTC_CONVERT_BCD2BIN(LL_RTC_DATE_GetYear(RTC)),
		__LL_RTC_CONVERT_BCD2BIN(LL_RTC_DATE_GetMonth(RTC)),
		__LL_RTC_CONVERT_BCD2BIN(LL_RTC_DATE_GetDay(RTC)),
		__LL_RTC_CONVERT_BCD2BIN(LL_RTC_TIME_GetHour(RTC)),
		__LL_RTC_CONVERT_BCD2BIN(LL_RTC_TIME_GetMinute(RTC)), (uint8_t)secWhole, (uint8_t)(1000 * secFrac));	

	return timestamp;
}

std::string RTClock::getMonthText(uint8_t monthNum)
{
	//cpfAssert("checkMonthNum");
	return monthText[monthNum];
		
}

/**
  * @brief RTC Initialization Function
  * @param None
  * @retval None
  */
static void MX_RTC_Init(void)
{

	/* USER CODE BEGIN RTC_Init 0 */

	/* USER CODE END RTC_Init 0 */

	LL_RTC_InitTypeDef RTC_InitStruct = { 0 };
	LL_RTC_TimeTypeDef RTC_TimeStruct = { 0 };
	LL_RTC_DateTypeDef RTC_DateStruct = { 0 };

	LL_GPIO_InitTypeDef GPIO_InitStruct = { 0 };

	if (LL_RCC_GetRTCClockSource() != LL_RCC_RTC_CLKSOURCE_LSI)
	{
		LL_RCC_ForceBackupDomainReset();
		LL_RCC_ReleaseBackupDomainReset();
		LL_RCC_SetRTCClockSource(LL_RCC_RTC_CLKSOURCE_LSI);
	}

	/* Peripheral clock enable */
	LL_RCC_EnableRTC();

	LL_AHB4_GRP1_EnableClock(LL_AHB4_GRP1_PERIPH_GPIOB);
	/**RTC GPIO Configuration
	PB2   ------> RTC_OUT_CALIB
	*/
	GPIO_InitStruct.Pin = LL_GPIO_PIN_2;
	GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
	GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
	GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
	GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
	LL_GPIO_Init(GPIOB, &GPIO_InitStruct);

	/* USER CODE BEGIN RTC_Init 1 */

	/* USER CODE END RTC_Init 1 */
	RTC_InitStruct.HourFormat = LL_RTC_HOURFORMAT_24HOUR;
	RTC_InitStruct.AsynchPrescaler = 127;
	RTC_InitStruct.SynchPrescaler = 255;
	LL_RTC_Init(RTC, &RTC_InitStruct);
	/** Initialize RTC and set the Time and Date
	*/
	RTC_TimeStruct.Hours = 0x0;
	RTC_TimeStruct.Minutes = 0x0;
	RTC_TimeStruct.Seconds = 0x0;
	LL_RTC_TIME_Init(RTC, LL_RTC_FORMAT_BCD, &RTC_TimeStruct);
	
	RTC_DateStruct.WeekDay = LL_RTC_WEEKDAY_TUESDAY;
	RTC_DateStruct.Month = LL_RTC_MONTH_APRIL;
	RTC_DateStruct.Day = 0x1;
	RTC_DateStruct.Year = 0x23;
	LL_RTC_DATE_Init(RTC, LL_RTC_FORMAT_BCD, &RTC_DateStruct);
	
	/** Enable the WakeUp
 */
	LL_RTC_WAKEUP_SetClock(RTC, LL_RTC_WAKEUPCLOCK_DIV_16);
	
	/** Enable Calibration
	*/
	LL_RTC_CAL_SetOutputFreq(RTC, LL_RTC_CALIB_OUTPUT_1HZ);
	/* USER CODE BEGIN RTC_Init 2 */

	/* USER CODE END RTC_Init 2 */

}

/**
  * @brief  Enter in initialization mode
  * @note In this mode, the calendar counter is stopped and its value can be updated
  * @param  None
  * @retval RTC_ERROR_NONE if no error
  */
uint32_t Enter_RTC_InitMode(void)
{
	/* Set Initialization mode */
	LL_RTC_EnableInitMode(RTC);

#if (USE_TIMEOUT == 1)
	Timeout = RTC_TIMEOUT_VALUE;
#endif /* USE_TIMEOUT */

	/* Check if the Initialization mode is set */
	while (LL_RTC_IsActiveFlag_INIT(RTC) != 1)
	{
#if (USE_TIMEOUT == 1)
		if (LL_SYSTICK_IsActiveCounterFlag())
		{
			Timeout--;
		}
		if (Timeout == 0)
		{
			return RTC_ERROR_TIMEOUT;
		}
#endif /* USE_TIMEOUT */
	}

	return RTC_ERROR_NONE;
}

/**
  * @brief  Exit Initialization mode
  * @param  None
  * @retval RTC_ERROR_NONE if no error
  */
uint32_t Exit_RTC_InitMode(void)
{
	LL_RTC_DisableInitMode(RTC);

	/* Wait for synchro */
	/* Note: Needed only if Shadow registers is enabled           */
	/*       LL_RTC_IsShadowRegBypassEnabled function can be used */
	return (WaitForSynchro_RTC());
}

/**
  * @brief  Wait until the RTC Time and Date registers (RTC_TR and RTC_DR) are
  *         synchronized with RTC APB clock.
  * @param  None
  * @retval RTC_ERROR_NONE if no error (RTC_ERROR_TIMEOUT will occur if RTC is
  *         not synchronized)
  */
uint32_t WaitForSynchro_RTC(void)
{
	/* Clear RSF flag */
	LL_RTC_ClearFlag_RS(RTC);

#if (USE_TIMEOUT == 1)
	Timeout = RTC_TIMEOUT_VALUE;
#endif /* USE_TIMEOUT */

	/* Wait the registers to be synchronised */
	while (LL_RTC_IsActiveFlag_RS(RTC) != 1)
	{
#if (USE_TIMEOUT == 1)
		if (LL_SYSTICK_IsActiveCounterFlag())
		{
			Timeout--;
		}
		if (Timeout == 0)
		{
			return RTC_ERROR_TIMEOUT;
		}
#endif /* USE_TIMEOUT */
	}
	return RTC_ERROR_NONE;
}