#include "ch.h"
#include "hal.h"
#include "usb_usart.h"
#include "rf_task.h"
#include "motor.h"
#include <stm32l1xx.h>
#include <stdio.h>
#include "sphere_shell.h"
#include <shell.h>
#include "sensor_task.h"
#include "parameters.h"
#include "control_task.h"
#include "powersave.h"
#include "speedsensor.h"

static const ShellConfig shellconfig = {
    (BaseChannel*)&SD1,
    shell_commands
};

static WORKING_AREA(shellstack, 512);

static unsigned * const eeprom_odometer = (unsigned * const)0x08080400;

int main()
{
    SYSCFG->PMC = 0;
    halInit();
    
    if (RTC->BKP0R == 0x57400)
    {
        // This signifies that we want to go to sleep without IWDG.
        // Reset the flag
        RTC->BKP0R = 0;
        
        RCC->AHBLPENR = 0;
        RCC->APB2LPENR = 0;
        RCC->APB1LPENR = 0;
        
        RCC->APB1ENR |= RCC_APB1ENR_PWREN;
        PWR->CR |= PWR_CR_CWUF;
        PWR->CR |= PWR_CR_PDDS;
        SCB->SCR |= SCB_SCR_SLEEPDEEP;
        PWR->CSR |= PWR_CSR_EWUP1;
        
        __sync_synchronize();
        asm("wfe");
        
        // Just in case there is an interrupt immediately
        SCB->AIRCR = 0x05FA0000 | SCB_AIRCR_SYSRESETREQ;
    }
    
    DBGMCU->APB1FZ = DBGMCU_APB1_FZ_DBG_IWDG_STOP;
    IWDG->KR = 0x5555; // Enable watchdog register write
    IWDG->PR = 6;
    IWDG->RLR = 4095; // 30 seconds
    IWDG->KR = 0xCCCC; // Start watchdog
    
    chSysInit();
    
    parameters_load();
    
    speedsensor_odometer = *eeprom_odometer;
    sensor_init();
    
    sdStart(&SD1, NULL);
    
    chThdSleepMilliseconds(500);
    printf("Boot!\n");
    
    start_usb_usart();
    chThdSleepMilliseconds(500);
    
    printf("USB init\n");
    
    shellInit();
    shellCreateStatic(&shellconfig, shellstack, sizeof(shellstack), NORMALPRIO);
    
    motor_start();
    rf_start();
    sensor_start();
    control_start();
    
    while (1)
    {
       IWDG->KR = 0xAAAA; // Reset watchdog
        
       check_usb_usart();
       
       if (powersave_check())
       {
           FLASH->PEKEYR = 0x89ABCDEF;
           FLASH->PEKEYR = 0x02030405;
           *eeprom_odometer = speedsensor_odometer;
           FLASH->PECR |= FLASH_PECR_PELOCK;
           
           powersave_sleep();
       }
       
       chThdSleepMilliseconds(10);
    }
}