#include "ch.h"
#include "hal.h"
#include "usb_usart.h"
#include "rf_task.h"
#include "lsm303.h"
#include <stm32l1xx.h>
#include <stdio.h>
#include "sphere_shell.h"
#include <shell.h>
#include "sensor_task.h"
#include "ahrs_task.h"
#include "parameters.h"
#include "powersave.h"

PowersaveStream PSD1;

static const ShellConfig shellconfig = {
    (BaseSequentialStream*)&PSD1,
    shell_commands
};

static WORKING_AREA(shellstack, 512);


int main()
{
    SYSCFG->PMC = 0;
    halInit();
    chSysInit();
    
    parameters_load();
    
    sensor_init();
    
    sdStart(&SD1, NULL);
    PowersaveStreamInit(&PSD1, (BaseChannel*)&SD1);
    
    printf("Boot!\n");
    
    start_usb_usart();
    chThdSleepMilliseconds(500);
    
    printf("USB init\n");
    
    shellInit();
    shellCreateStatic(&shellconfig, shellstack, sizeof(shellstack), NORMALPRIO);
    
    rf_start();
    sensor_start();
    ahrs_start();
    
    while (1)
    { 
       check_usb_usart();
       
       if (powersave_check())
           powersave_sleep();
       
       chThdSleepMilliseconds(10);
    }
}