#include <stdio.h>
#include <math.h>
#include <stm32l432xx.h>
#include "ee14lib.h"

void delay_us(unsigned int us)
{
    // 4MHz clock, while loop is 4 cycles?
    volatile int i = us;
    while(i--){}
}


// This function is called by printf() to handle the text string
// We want it to be sent over the serial terminal, so we just delegate to that function
int _write(int file, char *data, int len) {
    serial_write(USART2, data, len);
    return len;
}

void clear_rtc_flags(void)
{
    RTC->WPR = 0xCA; // Unlock the RTC registers
    RTC->WPR = 0x53;
    RTC->ISR &= ~RTC_ISR_WUTF;   // Clear RTC wakeup flag
    RTC->WPR = 0x00;
    EXTI->PR1 = EXTI_PR1_PIF20;  // Clear EXTI pending bit 20 (RTC wakeup)
}

void RTC_WKUP_IRQHandler(void) {
    clear_rtc_flags();
}

int main()
{
    uint32_t csr = RCC->CSR;
    RCC->CSR |= RCC_CSR_RMVF; // Clear the reset cause flags

    host_serial_init(9600);

    printf("Starting up...\n");
    for(int i = 10; i > 0; i--){
        delay_us(100000);
        printf("Going to sleep in %d...\n", i);
    }
    printf("Good night!\n");

    // Set the RTC wake-up timer to wake us up

    // Enable the PWR clock (peripheral for managing low-power modes)
    RCC->APB1ENR1 |= RCC_APB1ENR1_PWREN;

    // Disable backup-domain write protection so we can configure RTC
    // Normally registers on the backup power domain are write-protected to
    // prevent runaway memset() or buffer overflows from wrecking the
    // precious information that persists across resets
    PWR->CR1 |= PWR_CR1_DBP;

    // Turn on the external low-speed oscillator (LSE, which the RTC runs on)
    RCC->BDCR |= RCC_BDCR_LSEON;

    // Wait for LSE to be stable before enabling the RTC
    while (!(RCC->BDCR & RCC_BDCR_LSERDY));

    // Now enable the RTC and set it to use the LSE clock
    RCC->BDCR |= RCC_BDCR_RTCEN | (0b01 << RCC_BDCR_RTCSEL_Pos); 

    // Clear the interrupt flag in case it was already set
    clear_rtc_flags();

    // Enter the magic key to unlock write access for the RTC registers
    // This is another protection layer against runaway writes: since the RTC is
    // the only way we're getting out of sleep, an accidental write due to a
    // software bug could leave us bricked!
    RTC->WPR = 0xCA;
    RTC->WPR = 0x53;

    RTC->CR &= ~RTC_CR_WUTE; // Disable the wake-up timer
    while(!(RTC->ISR & RTC_ISR_WUTWF)){ } // Wait until WUTWF (write flag) is 1

    // Set the RTC wakeup clock source
    RTC->CR &= ~RTC_CR_WUCKSEL;
    RTC->CR |= 0b000; // 000 = RTC/16 ~= 2kHz, see options in reference manual

    RTC->WUTR = 8000; // 16 bit counter
    RTC->CR |= (RTC_CR_WUTE | RTC_CR_WUTIE); // Re-enable the wake-up timer and interrupt

    // Re-lock the RTC registers by writing an incorrect value
    RTC->WPR = 0x00;


    // Select which low-power mode to go into

    // Set deep sleep mode in the Cortex-M4 system control register
    SCB->SCR = SCB_SCR_SLEEPDEEP_Msk;

    // Select which of the "deep sleep" options we want
    //PWR->CR1 |= 0b000 << PWR_CR1_LPMS_Pos; // 0b000 is "Stop 0"
    PWR->CR1 |= 0b100 << PWR_CR1_LPMS_Pos; // 0b1xx is shutdown mode

    // Enable the RTC wakeup interrupt in the EXTI
    EXTI->IMR1 |= EXTI_IMR1_IM20;
    EXTI->RTSR1 |= EXTI_RTSR1_RT20; // Enable trigger on rising edge
    NVIC_EnableIRQ(RTC_WKUP_IRQn);

    // Go to sleep by calling the "Wait for Interrupt" CPU instruction
    __WFI();


    while(1){

        printf("I'm still alive!\n");
        printf("CSR: %08x\n", csr);
        delay_us(1000000);
    }

    return 0;
}