#![no_std]
#![no_main]

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

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

const ANGLE_PULSE_RATIO: f32 = 10.25;

fn set_servo_degree(pin: &mut Pin<Output>, degree: f32) {
    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() -> ! {
    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();
    println!("hewwo :3");

    let mut switchSmoothed: f32 = 0.0;
    let mut switchPrev: f32 = 0.0;

    loop {
        let val = match switch.is_low() {
            true => 1.0,
            false => 0.0,
        };
        switchSmoothed = (val * 0.05) + (switchPrev * 0.95);
        switchPrev = switchSmoothed;
        let deg = (190.0 * switchSmoothed) + 25.0;

        set_servo_degree(&mut servo, deg);
    }
}