#include <PowerManager_Interface.hpp>
#include <bsp.hpp>
#include <stm32h7a3xxq.h>
#include <stm32h7xx_ll_rcc.h>
#include <stm32h7xx_ll_pwr.h>
#include <_bsp_private.hpp>
#include <iostream>

/// If you don't want to assert on unexpected failures to go to suspend,
// comment this out
#define ASSERT_ON_UNEXPECTED_EARLY_WAKEUP

#ifdef USING_VISUAL_GDB
extern "C" void SuspendFastSemihostingPolling();
extern "C" void ResumeFastSemihostingPolling();
#endif // USING_VISUAL_GDB

/** STM32 Power States and System Power States
 *
 *
 * STM32H7 provides a number of low-power states, some of which
 * are used within this implementation. Here is the mapping between
 * STM32H7 power states and system power states:
 *
 * - sleep() corresponds to the CSleep State (wfi and SLEEPDEEP=0)
 * - Suspend state corresponds to the CStop State (wfi and SLEEPDEEP=1)
 * 	- All memory and peripheral register data is retained, but high-speed clocks are stopped
 *  - LSE (32.768k external oscilator and LSI (32k internal oscillator) can be enabled
 * 	- System will resume execution following the wfi call
 *  - In the future, you could consider whether this should be Stop
 *  - CStop: CPU clock stopped, CPU sleep-enabled peripheral clock(s) active
 *  - Stop: system clock is stopped (When CPU is in CStop mode and at least one PDDS_Dn bit selects Stop)
 * - Deep Sleep state corresponds to Standby state (sleep deep)
 * 	- System is powered down, but the backup domain may be kept active (e.g., RTC)
 *  - By default, no RAM or register retention. 128-byte backup registers always retained.
 *  - Possible to retain 4KB of backup RAM ==backup regulator needs to be enabled in software==
 *  - Standby when CPUs are n CStop mode and all PDDS_Dn bits select Standby
 *  - When waking up from Stop modes, the clock system is reset
 *	- system will restart the program
 *  - critical information should be saved to the RTC scratch registers
 *  - RTC should be configured to provide a periodic wakeup event
 *
 * The low-power mode is entered by configuring:
 * - SLEEPDEEP=1
 * - The PWR Domain PDDS bits (to either STOP or Standby)
 * 	- When entering CStop mode, PDDS_D1, PDDS_D2, and PDDS_D3 select which state the
 *  - Domains may enter in.
 *
 * To tell that you've woken up from CStop mode, you can use flags:
 * - SLEEPDEEP bit (sleep vs stop)
 * - PWR control
 * 	- SBF_Dn (SBF_D1, SBF_D2) - CPU domain has woken up from DStandby Mode (CPU starts from Reset)
 * 	- STOPF - system has woken up from STOP mode
 * 	- SBF - system has woekn up from standby mode (CPU starts from reset)
 *
 * You'd need to reinitialize system clocks when STOPF == 1 and DEEPSLEEP = 1. If STOPF==0, then
 * you stayed in run mode and there is no need to re-initialize the clock system.
 * If the SBF flag is also set, the System was in standby and you need to reinitialize everything.
 *
 * For more information, please consult the reference manual.
 *
 * - Chapter 6, power control (PWR), pg 254
 *	- 6.7, Low-Power Modes, pg 291
 * - Chapter 7, Low-power D3 Domain Applciation Example
 *
 * These slides also provide a useful summary:
 * - https://www.st.com/content/ccc/resource/training/technical/product_training/group0/97/f1/9d/3a/c4/c2/4c/ff/STM32H7-System-Power_control_PWR/files/STM32H7-System-Power_control_PWR.pdf/_jcr_content/translations/en.STM32H7-System-Power_control_PWR.pdf
 */

static inline void prepareProcessorSLEEPDEEP()
{
	PWR->CR1 |= PWR_CR1_LPDS;

	// set the ARM DEEPSLEEP bit before calling WFI in order to suspend the system
	// RTC wakeup should be configured *before* doing this!
	SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk;

#ifdef USING_VISUAL_GDB
	SuspendFastSemihostingPolling();
#endif
}

static void recoverFromStopMode()
{
#ifdef USING_VISUAL_GDB
	ResumeFastSemihostingPolling();
#endif

	// This means we came out of stop mode, and we need to reset the clocks
	// to our target settings
	if(PWR->CPUCR & PWR_CPUCR_STOPF)
	{
		bsp_private::initializeSystemClocks();

		// This has to be done to clear the STOP flag
		LL_PWR_ClearFlag_CPU();
	}

	SCB->SCR &= !SCB_SCR_SLEEPDEEP_Msk;
}

static void ProcessorStopMode()
{
	prepareProcessorSLEEPDEEP();

	/* Keep DSTOP mode when Cortex-M7 enter in DEEP-SLEEP by ensuring standby is not set */
	PWR->CPUCR &= ~(PWR_CPUCR_PDDS_D1 | PWR_CPUCR_PDDS_D3);

  /* Ensure that all instructions are done before entering STOP mode */
	__DSB ();
	__ISB ();

	PowerManager::sleep();

	recoverFromStopMode();
}

static void ProcessorSuspendMode()
{
	prepareProcessorSLEEPDEEP();

	/* Enter DSTANDBY mode when Cortex-M7 enters DEEP-SLEEP */
	PWR->CPUCR |= (PWR_CPUCR_PDDS_D1 | PWR_CPUCR_PDDS_D3);

  /* Ensure that all instructions are done before entering standby mode */
	__DSB ();
	__ISB ();

	// TODO: if the cache is ever used, then you should also flush the data cache before sleeping.
	// Otherwise, writes to backup RAM may not work!
	PowerManager::sleep();

	// This normally will not return - the system should reset - unless there is something
	// preventing the system from going to sleep.
	// We'll assert, because this probably isn't your intention.
	// To make this work, however, you would
#ifdef ASSERT_ON_UNEXPECTED_EARLY_WAKEUP
	assert(0);
#else
	recoverFromStopMode();
#endif // ASSERT_ON_UNEXPECTED_EARLY_WAKEUP
}

void PowerManager::setState(PowerManager::States state)
{
	switch(state) {
	case PowerManager::States::Standby:
		// TODO, for example, .stop() all non-critical drivers
		break;
	case PowerManager::States::Active:
		// TODO, for example, .start() all drivers
		break;
	case PowerManager::States::Suspend:
		// Ensure the low-power regulator remains on
		ProcessorStopMode();
		break;
	case PowerManager::States::DeepSleep:
		ProcessorSuspendMode();
		break;
	case PowerManager::States::Shutdown:
		// TODO: turn off voltage rails, whatever else is necessary to
		// completely power down the device
		break;
	}
}

// Note well: for this to actually sleep, there must be no pending
// interrupts. If there are pending interrupts, it will return immediately
void PowerManager::sleep()
{
	SysTick->CTRL &= ~SysTick_CTRL_ENABLE_Msk;
	asm("wfi");
	SysTick->CTRL |= ~SysTick_CTRL_ENABLE_Msk;
}


void PowerManager::sleep(std::chrono::milliseconds max_time)
{
	static auto PowerManager_Timer = allocateSoftwareTimer();
	PowerManager_Timer.period(max_time);
	PowerManager_Timer.start();
	PowerManager::sleep();
	PowerManager_Timer.stop();
}

static std::string_view getPowerOnReasonString(PowerManager::PowerOnReason reason)
{
	switch(reason)
	{
		case PowerManager::PowerOnReason::PowerOn:
			return "PowerOn";
		case PowerManager::PowerOnReason::SoftwareReset:
			return "SoftwareReset";
		case PowerManager::PowerOnReason::HardwareReset:
			return "HardwareReset";
		case PowerManager::PowerOnReason::WatchdogReset:
			return "WatchdogReset";
		case PowerManager::PowerOnReason::Brownout:
			return "Brownout";
		case PowerManager::PowerOnReason::Wakeup:
			return "Wakeup";
		default:
			return "Unknown";
	}
}

/// There are a couple of thigns we need to do to determine power on source
/// For one, we can check the RCC->RSR register to determine if various events have
/// occurred. But we also need to check PWR->CPUCR to determine if we have
/// woken from standby. If none of those occurred, we will assume it is a pure "power-on".
///
/// There are other flags that can be checked (CPURST, Domain resets, etc.), but we aren't
/// using them right now so I have opted to not address them. Just be aware that you can detect
/// more than what is captured here.
///
/// Note that this assumes there is not _contention_ here, and that there is only one wakeup source.
/// It is completely possible that the system has woken from standby (PWR->CPUCR & PWR_CPUCR_SBF),
/// while the wakeup was triggered by some other source.
///
/// If that case _really_ needs to be handled (rather than indicative of another problem),
/// this function could be adjusted to return a std::pair<PowerOnReason, bool[woke-from-standby]>
PowerManager::PowerOnReason PowerManager::checkPowerOnReason()
{
	PowerManager::PowerOnReason reason = PowerManager::PowerOnReason::PowerOn;

	if(LL_RCC_IsActiveFlag_WWDG1RST())
	{
		reason = PowerManager::PowerOnReason::WatchdogReset;
	}
	else if(LL_RCC_IsActiveFlag_IWDG1RST())
	{
		reason = PowerManager::PowerOnReason::WatchdogReset;
	}
	else if(LL_RCC_IsActiveFlag_SFTRST())
	{
		reason = PowerManager::PowerOnReason::SoftwareReset;
	}
	else if(LL_RCC_IsActiveFlag_PINRST())
	{
		reason = PowerManager::PowerOnReason::HardwareReset;
	}
	else if(LL_RCC_IsActiveFlag_BORRST())
	{
		reason = PowerManager::PowerOnReason::Brownout;
	}

	if(PWR->CPUCR & PWR_CPUCR_SBF)
	{
		reason = PowerManager::PowerOnReason::Wakeup;
	}

	// This has to be done, because the reset causes are _cumulative_. If you don't
	// clear them, you will detect causes from previous reset events.
	LL_RCC_ClearResetFlags();

	// This has to be done to clear the SUSPEND flag
	LL_PWR_ClearFlag_CPU();

	// Ideally, this would be logged, but the current logging strategy does not allow for it.
	// In the future, if early logging happens to a buffer that is later flushed, then it
	// would be wise to convert this to use the Logger.
	std::cout << "Reset Reason: " << getPowerOnReasonString(reason) << std::endl;

	return reason;
}
