#![no_std]
#![no_main]

mod pid;
mod touch;
mod usb;

use cortex_m_rt::entry;

use panic_halt as _;

use pid::{FixedI32, Pid, PidConfig};

use stm32f0xx_hal as hal;

use hal::adc::Adc;
use hal::delay::Delay;
use hal::pac::{CorePeripherals, Peripherals};
use hal::prelude::*;
use hal::pwm;
use hal::usb::{Peripheral, UsbBus};

use touch::{SysCfg, Touch};

use usb::Usb;

#[entry]
fn main() -> ! {
    let mut p = Peripherals::take().unwrap();
    let cp = CorePeripherals::take().unwrap();

    let sys_cfg = SysCfg::new(p.SYSCFG, &mut p.RCC);

    let mut rcc = p
        .RCC
        .configure()
        .hsi48()
        .enable_crs(p.CRS)
        .sysclk(48.mhz())
        .pclk(24.mhz())
        .freeze(&mut p.FLASH);

    let gpioa = p.GPIOA.split(&mut rcc);
    let gpiob = p.GPIOB.split(&mut rcc);
    let gpioc = p.GPIOC.split(&mut rcc);
    let gpiof = p.GPIOF.split(&mut rcc);

    #[allow(unused)]
    let (
        mut adc3,   // ADC_IN0
        mut adc2,   // ADC_IN1
        mut adc1,   // ADC_IN2
        mut adc8,   // ADC_IN3
        mut adc7,   // ADC_IN4
        mut adc6,   // ADC_IN5
        mut adc5,   // ADC_IN6
        mut adc4,   // ADC_IN7
        pwm6_pin,   // TIM1_CH1
        tx,         // USART1_TX
        rx,         // USART1_RX
        mut dir5,   // PA13
        mut dir4,   // PA14
        mut dir3,   // PA15
        mut touch4, // PB0
        mut touch5, // PB1
        mut touch6, // PB2
        pwm5_pin,   // TIM2_CH2
        pwm4_pin,   // TIM3_CH1
        pwm3_pin,   // TIM3_CH2
        mut dir2,   // PB6
        mut dir1,   // PB7
        pwm1_pin,   // TIM16_CH1
        pwm2_pin,   // TIM17_CH1
        mut touch7, // PB10
        mut touch8, // PB11
        mut dir8,   // PB12
        mut dir7,   // PB13
        pwm8_pin,   // TIM15_CH1
        pwm7_pin,   // TIM15_CH2
        mut touch1, // PC13
        mut touch2, // PC14
        mut touch3, // PC15
        mut nsleep, // PF0
        mut dir6,   // PF1
    ) = cortex_m::interrupt::free(|cs| {
        (
            gpioa.pa0.into_analog(cs),
            gpioa.pa1.into_analog(cs),
            gpioa.pa2.into_analog(cs),
            gpioa.pa3.into_analog(cs),
            gpioa.pa4.into_analog(cs),
            gpioa.pa5.into_analog(cs),
            gpioa.pa6.into_analog(cs),
            gpioa.pa7.into_analog(cs),
            gpioa.pa8.into_alternate_af2(cs),
            gpioa.pa9.into_alternate_af1(cs),
            gpioa.pa10.into_alternate_af1(cs),
            gpioa.pa13.into_push_pull_output(cs),
            gpioa.pa14.into_push_pull_output(cs),
            gpioa.pa15.into_push_pull_output(cs),
            gpiob.pb0.into_open_drain_output(cs),
            gpiob.pb1.into_open_drain_output(cs),
            gpiob.pb2.into_open_drain_output(cs),
            gpiob.pb3.into_alternate_af2(cs),
            gpiob.pb4.into_alternate_af1(cs),
            gpiob.pb5.into_alternate_af1(cs),
            gpiob.pb6.into_push_pull_output(cs),
            gpiob.pb7.into_push_pull_output(cs),
            gpiob.pb8.into_alternate_af2(cs),
            gpiob.pb9.into_alternate_af2(cs),
            gpiob.pb10.into_open_drain_output(cs),
            gpiob.pb11.into_open_drain_output(cs),
            gpiob.pb12.into_push_pull_output(cs),
            gpiob.pb13.into_push_pull_output(cs),
            gpiob.pb14.into_alternate_af1(cs),
            gpiob.pb15.into_alternate_af1(cs),
            gpioc.pc13.into_open_drain_output(cs),
            gpioc.pc14.into_open_drain_output(cs),
            gpioc.pc15.into_open_drain_output(cs),
            gpiof.pf0.into_push_pull_output(cs),
            gpiof.pf1.into_push_pull_output(cs),
        )
    });

    let usb_bus = UsbBus::new(Peripheral {
        usb: p.USB,
        pin_dm: gpioa.pa11,
        pin_dp: gpioa.pa12,
    });
    let mut usb = Usb::new(&usb_bus);

    let mut _adc = Adc::new(p.ADC, &mut rcc);

    let mut _pwm1 = pwm::tim16(p.TIM16, pwm1_pin, &mut rcc, 250.khz());
    let mut _pwm2 = pwm::tim17(p.TIM17, pwm2_pin, &mut rcc, 250.khz());
    let (mut _pwm4, mut _pwm3) = pwm::tim3(p.TIM3, (pwm4_pin, pwm3_pin), &mut rcc, 250.khz());
    let mut _pwm5 = pwm::tim2(p.TIM2, pwm5_pin, &mut rcc, 250.khz());
    let mut _pwm6 = pwm::tim1(p.TIM1, pwm6_pin, &mut rcc, 250.khz());
    let (mut _pwm8, mut _pwm7) = pwm::tim15(p.TIM15, (pwm8_pin, pwm7_pin), &mut rcc, 250.khz());

    let delay = Delay::new(cp.SYST, &mut rcc);

    let touch = Touch::new(sys_cfg, p.EXTI, delay.clone());
    touch.enable();

    let pid_config = PidConfig::<false>::new()
        .d_gain(FixedI32!(0))
        .i_gain(FixedI32!(0))
        .i_max(FixedI32!(0))
        .i_min(FixedI32!(0))
        .p_gain(FixedI32!(0.5));
    let mut pid = Pid::new(&pid_config, 2048);

    loop {
        usb.poll();

        let (_duty, _dir) = pid.update(0, 2048);
    }
}