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

use core::cell::RefCell;
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 avr_device::interrupt::{self, Mutex};
use panic_halt as _;
use serde::Deserialize;
use serde_json_core::from_str;

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();

static INPUT_LINE: Mutex<RefCell<[u8; 20]>> = Mutex::new(RefCell::new([0;20]));

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

    static mut BUF: [u8; 20] = [0;20];
    static mut N: usize = 0; // index into line

    if let Ok(val) = rx.read() {
        if val == b'\n' {

            interrupt::free(|cs| {
                let mut line = INPUT_LINE.borrow(cs).borrow_mut();
                *line = BUF;
                line[N] = b'\0';
            });

            N = 0;
        } else {
            BUF[N] = val;
            N += 1;
        }
    }
}


#[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();

        let command = decode_command();

        if command.left > 0. {
            shift_register |= LEFT_FWD;
        }
        if command.left < 0. {
            shift_register |= LEFT_REV;
        }

        if command.right > 0. {
            shift_register |= RIGHT_FWD;
        }
        if command.right < 0. {
            shift_register |= RIGHT_REV;
        }

        fn to_pwm(val: f32) -> u8 {
            (val.abs() * 255.0) as u8
        }

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

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

        arduino_hal::delay_ms(1000);
    }
}

#[derive(Deserialize, Default)]
struct Command {
    left: f32,
    right: f32,
}

fn decode_command() -> Command {
    let input = interrupt::free(|cs| {
        INPUT_LINE.borrow(cs).borrow().clone()
    });

    let Some(length) = input.iter().position(|n| *n == 0) else { return Default::default() };

    let Ok(input) = str::from_utf8(&input[..length]) else { return Default::default() };

    from_str(input.trim()).unwrap_or((Default::default(),0)).0
}