#include "ch.h"
#include "hal.h"
#include "usb_usart.h"
#include "rf_task.h"
#include "motor.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"

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

static WORKING_AREA(shellstack, 512);


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