#![no_std]
#![no_main]
#![feature(abi_avr_interrupt)]

use core::mem::MaybeUninit;

use arduino_hal::clock::MHz16;
use arduino_hal::hal::port::*;
use arduino_hal::hal::usart::UsartReader;
use arduino_hal::pac::USART0;
use arduino_hal::port::mode::*;
use arduino_hal::prelude::*;
use panic_halt as _;

fn shift_out(data_pin: &mut Pin<Output, PB0>, clock_pin: &mut Pin<Output, PD4>, data: &u8) {
    for i in 0..8 {
        let n = data & (1 << i);

        if n == 0 {
            data_pin.set_low();
        } else {
            data_pin.set_high();
        }

        clock_pin.set_high();
        clock_pin.set_low();
    }
}

fn update_shift_register(
    data_pin: &mut Pin<Output, PB0>,
    latch_pin: &mut Pin<Output, PB4>,
    clock_pin: &mut Pin<Output, PD4>,
    data: &u8,
) {
    latch_pin.set_low();

    shift_out(data_pin, clock_pin, data);

    latch_pin.set_high();
}

static mut UART_RX: MaybeUninit<UsartReader<USART0, Pin<Input, PD0>, Pin<Output, PD1>, MHz16>> = MaybeUninit::uninit();

#[avr_device::interrupt(atmega328p)]
fn USART_RX() {
    let rx = unsafe{ &mut *UART_RX.as_mut_ptr() };

    rx.read();
}


#[arduino_hal::entry]
fn main() -> ! {
    let dp = arduino_hal::Peripherals::take().unwrap();
    let pins = arduino_hal::pins!(dp);

    // shift register
    let mut data_pin = pins.d8.into_output();
    let mut clock_pin = pins.d4.into_output();
    let mut enable_pin = pins.d7.into_output();
    let mut latch_pin = pins.d12.into_output();

    enable_pin.set_low(); // enable outputs

    pins.d3.into_output(); // right
    pins.d11.into_output(); // left

    // TCCR2A |= _BV(COM2A1) | _BV(WGM20) | _BV(WGM21); // fast PWM, turn on oc2a
    // TCCR2B = freq & 0x7;
    // OCR2A = 0;
    let tc2 = dp.TC2;
    tc2.tccr2a.write(|w| { w
            .wgm2().pwm_fast()
            .com2a().match_clear()
            .com2b().match_clear()
    });
    tc2.tccr2b.write(|w| w.cs2().prescale_8().wgm22().clear_bit());
    tc2.ocr2a.write(|w| w.bits(0));
    tc2.ocr2b.write(|w| w.bits(0));

    let mut led = pins.d13.into_output();

    let mut serial = arduino_hal::default_serial!(dp, pins, 115200);

    avr_device::interrupt::disable();
    serial.listen(arduino_hal::hal::usart::Event::RxComplete);

    let (rx,tx) = serial.split();

    unsafe { 
        UART_RX = MaybeUninit::new(rx);
        avr_device::interrupt::enable();
    };

    loop {
        let mut shift_register = 0;
        const LEFT_FWD: u8 = 1 << 5; // 1a
        const LEFT_REV: u8 = 1 << 4; // 1b
        const RIGHT_FWD: u8 = 1 << 3; // 2b
        const RIGHT_REV: u8 = 1 << 6; // 2a

        led.toggle();

        // fwd left
        shift_register |= LEFT_FWD | RIGHT_FWD;

        // 16/255
        tc2.ocr2a.write(|w| w.bits(000)); // left
        tc2.ocr2b.write(|w| w.bits(000)); // right

        update_shift_register(&mut data_pin, &mut latch_pin, &mut clock_pin, &shift_register);

        arduino_hal::delay_ms(1000);
    }
}