#include "stm32l_rtc.h"
#include <hal.h>
#include <ch.h>
#include <stm32l1xx.h>
#include <stdlib.h>

static int dow(int y, int m, int d)
{
    static const uint8_t t[] = {0, 3, 2, 5, 0, 3, 5, 1, 4, 6, 2, 4};
    y -= m < 3;
    d = (y + y/4 - y/100 + y/400 + t[m-1] + d) % 7;
    
    if (d == 0) d = 7;
    return d;
}

void set_rtc(const time_t *time)
{
    // Allow RTC write
    RTC->WPR = 0xCA;
    RTC->WPR = 0x53;
    
    // Enter RTC init mode
    RTC->ISR |= RTC_ISR_INIT;
    chThdSleepMilliseconds(10);
    
    // Program clock frequency (32.768kHz crystal)
    // Reference manual says to write the prescalers in two operations..
    RTC->PRER = 0x007F0000;
    RTC->PRER = 0x007F00FF;
    
    // Program the date and time
    RTC->DR = (
        (((time->year % 100) / 10) << 20) | // Year tens
        ((time->year % 10) << 16) | // Year units
        (dow(time->year, time->month, time->day) << 13) | // Day of week
        ((time->month / 10) << 12) | // Month tens
        ((time->month % 10) << 8) | // Month units
        ((time->day / 10) << 4) | // Day tens
        ((time->day % 10) << 0) // Day units
    );
    
    RTC->TR = (
        ((time->hour / 10) << 20) | // Hour tens
        ((time->hour % 10) << 16) | // Hour units
        ((time->minute / 10) << 12) | // Minute tens
        ((time->minute % 10) << 8) | // Minute units
        ((time->second / 10) << 4) | // Second tens
        ((time->second % 10) << 0) // Second units
    );
    
    // Start the RTC
    RTC->ISR &= ~RTC_ISR_INIT;
    
    RTC->WPR = 0; // Relock registers
    chThdSleepMilliseconds(10); // Wait for RTC to start
}

void rtc_sleep(int timeout_msec)
{
    // Allow RTC write
    RTC->WPR = 0xCA;
    RTC->WPR = 0x53;
    
    if (!(RTC->ISR & RTC_ISR_INITS))
    {
        // RTC wasn't enabled yet
        RTC->ISR |= RTC_ISR_INIT;
        chThdSleepMilliseconds(10);
        RTC->PRER = 0x007F0000;
        RTC->PRER = 0x007F00FF;
        RTC->ISR &= ~RTC_ISR_INIT;
    }
    
    RTC->CR &= ~RTC_CR_WUTE;
    RTC->CR &= (~7); // Wakeup clock = RTC/16
    int delay = timeout_msec * 2048 / 1000;
    
    chDbgAssert((delay > 0 && delay < 65536), "RTC delay", "")
    
    RTC->WUTR = delay;
    
    // Block all tasks and start the wakeup timer
    chSysLock();
    PWR->CR |= PWR_CR_CWUF;
    RTC->ISR &= ~RTC_ISR_WUTF;
    RTC->CR |= RTC_CR_WUTE | RTC_CR_WUTIE;
    
    // Enable EXTI20 rising edge event
    EXTI->EMR |= EXTI_EMR_MR20;
    EXTI->RTSR |= EXTI_RTSR_TR20;
    EXTI->PR |= EXTI_PR_PR20;
    
    // Go to sleep
    SCB->SCR |= SCB_SCR_SLEEPDEEP;
    __sync_synchronize();
    asm("wfe");
    SCB->SCR &= ~SCB_SCR_SLEEPDEEP;
    
    // Recover
    RTC->CR &= ~RTC_CR_WUTE;
    chSysUnlock();
    
    RTC->WPR = 0; // Relock registers
}

bool get_rtc(time_t *time)
{
    if (!(RTC->ISR & RTC_ISR_INITS))
        return false;
    
    uint32_t tr = RTC->TR;
    uint32_t dr = RTC->DR; // Consistency guaranteed by hardware.

    time->year = 2000 + ((dr >> 20) & 0xF) * 10 + ((dr >> 16) & 0xF);
    time->day_of_week = (dr >> 13) & 0x7;
    time->month = ((dr >> 12) & 0x1) * 10 + ((dr >> 8) & 0xF);
    time->day = ((dr >> 4) & 0x3) * 10 + ((dr >> 0) & 0xF);
    
    time->hour = ((tr >> 20) & 0x3) * 10 + ((tr >> 16) & 0xF);
    time->minute = ((tr >> 12) & 0x7) * 10 + ((tr >> 8) & 0xF);
    time->second = ((tr >> 4) & 0x7) * 10 + ((tr >> 0) & 0xF);
    
    return true;
}

bool is_sun_up(const time_t *time)
{
    // Length of day in minutes, indexed by month-1.
    // Calculated for 63°N
    const int daylength[12] = {
         268,  412,  586,  782,  970, 1140,
        1174, 1034,  844,  654,  460,  298
    };
    const int monthlen[12] = {
          31,   28,   31,   30,   31,   30,
          31,   31,   30,   31,   30,   31
    };
    
    // Interpolate the day length
    int len1 = daylength[time->month - 1];
    int len2 = daylength[time->month % 12];
    int daylen = len1 + (len2 - len1) * time->day / monthlen[time->month - 1];
    
    // Calculate time from noon
    int minutes_to_noon = (12 - time->hour) * 60 + (00 - time->minute);
    
    // Check if it is still day
    return abs(minutes_to_noon) < daylen / 2;
}