#include <ch.h>
#include <hal.h>
#include <shell.h>
#include "shell_commands.h"
#include "usb_usart.h"
#include <chprintf.h>
#include "motor.h"

static const ADCConversionGroup convgrp = {
    0,
    1,
    NULL,
    NULL,
    
    // CR
    0,
    ADC_CR2_SWSTART,
    
    // SMPR
    0,
    0,
    ADC_SMPR3_SMP_AN0(ADC_SAMPLE_4),
    
    // SQR
    ADC_SQR1_NUM_CH(1),
    0,
    0,
    0,
    ADC_SQR5_SQ1_N(ADC_CHANNEL_IN0)
};

int main()
{
    SYSCFG->PMC = 0;
    halInit();
    chSysInit();
    
    motor_init();
    
    adcsample_t buf = 10;
    adcStart(&ADCD1, NULL);
    
    int angle = 0;
    while (1)
    {
        check_usb_usart();
        
        if (!palReadPad(GPIOB, GPIOB_VBUS))
        {
            adcConvert(&ADCD1, &convgrp, &buf, 1);
            chThdSleepMilliseconds(1);
            
            int delay = 10000 - buf * 6000 / 4096;
            
            angle++;
            
            int p1 = (angle % 3 == 0) ? 1 : 0;
            int p2 = (angle % 3 == 1) ? 1 : 0;
            int p3 = (angle % 3 == 2) ? 1 : 0;
            
            palSetPad(GPIOB, GPIOB_LED);
            
            for (int i = 0; i <= 100; i++)
            {
                motor_set_phases(p1 * i, p2 * i, p3 * i);
                chThdSleepMilliseconds(20);
            }
            
            chThdSleepMilliseconds(1000);
            
            for (int i = 100; i >= 0; i -= 10)
            {
                motor_set_phases(p1 * i, p2 * i, p3 * i);
                chThdSleepMilliseconds(10);
            }
            
            palClearPad(GPIOB, GPIOB_LED);
            
            motor_poweroff();
            
            chThdSleepMilliseconds(delay - 3101);
        }
    }
}