#![no_std]
#![no_main]

use arduino_hal::port::{Pin, mode::Output};
use panic_halt as _;
use avr_device::interrupt;
use core::cell::RefCell;

// Serial monitor shit
// No idea wtf this does
type Console = arduino_hal::hal::usart::Usart0<arduino_hal::DefaultClock>;
static CONSOLE: interrupt::Mutex<RefCell<Option<Console>>> =
    interrupt::Mutex::new(RefCell::new(None));

macro_rules! print {
    ($($t:tt)*) => {
        interrupt::free(
            |cs| {
                if let Some(console) = CONSOLE.borrow(cs).borrow_mut().as_mut() {
                    let _ = ufmt::uwrite!(console, $($t)*);
                }
            },
        )
    };
}

macro_rules! println {
    ($($t:tt)*) => {
        interrupt::free(
            |cs| {
                if let Some(console) = CONSOLE.borrow(cs).borrow_mut().as_mut() {
                    let _ = ufmt::uwriteln!(console, $($t)*);
                }
            },
        )
    };
}

fn put_console(console: Console) {
    interrupt::free(|cs| {
        *CONSOLE.borrow(cs).borrow_mut() = Some(console);
    })
}

// Test ratio for converting from angle to servo control pulse
// This was a guess and is kinda innacurate
// Should do more accurate testing for final version
const ANGLE_PULSE_RATIO: f32 = 10.25;

fn set_servo_degree(pin: &mut Pin<Output>, degree: f32) {
    // Using delays blocks the processor
    // Cannot work with multiple motors
    // Embassy async will fix this on pico
    let delay = (degree * ANGLE_PULSE_RATIO) as u32;
    pin.set_high();
    arduino_hal::delay_us(delay);
    pin.set_low();
    arduino_hal::delay_us(20000-delay);
}

#[arduino_hal::entry]
fn main() -> ! {
    /* Setup Pins */
    let dp = arduino_hal::Peripherals::take().unwrap();
    let pins = arduino_hal::pins!(dp);
    let serial = arduino_hal::default_serial!(dp, pins, 57600);
    put_console(serial);

    let mut servo = pins.d9.into_output().downgrade();
    let switch = pins.d10.into_pull_up_input();
    let switch2 = pins.d11.into_pull_up_input();
    println!("hewwo :3");

    // Variables for loop-persistent data
    let mut switch_count = 0.0;
    let mut endpoint = 75.0;

    loop {
        /* Get Input */
        // This debouncing sucks ass and isn't good - doesn't matter, just for testing
        switch_count = match switch.is_low() {
            true => {
                endpoint = 170.0;
                1.0
            },
            false => {
                if switch_count > 0.0 {
                    switch_count - 0.1
                } else {
                    0.0
                }
            },
        };
        switch_count = match switch2.is_low() {
            true => {
                endpoint = 5.0;
                1.0
            },
            false => {
                if switch_count > 0.0 {
                    switch_count - 0.1
                } else {
                    0.0
                }
            },
        };

        /* Move Servos */
        let deg = lerp(75.0, endpoint, switch_count);
        set_servo_degree(&mut servo, deg);
    }
}

fn lerp(v0: f32, v1: f32, t: f32) -> f32 {
    return (1.0 - t) * v0 + t * v1;
}