#![no_std]
#![no_main]
#![feature(
    type_alias_impl_trait,
    future_join,
    abi_avr_interrupt,
    impl_trait_in_assoc_type
)]

use arduino_hal::{delay_ms, delay_us, DefaultClock};
use atmega_hal::port::mode::Output;
use atmega_hal::port::{Dynamic, Pin};
use avr_hal_generic::avr_device;
use servo::Servo;
use core::{fmt::Write, panic::PanicInfo};
use embassy_executor::Spawner;
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, mutex::Mutex};
use ufmt::{uWrite, uwriteln};

static SERIAL: Mutex<CriticalSectionRawMutex, Option<atmega_hal::usart::Usart0<DefaultClock>>> =
    Mutex::new(None);

#[embassy_executor::task]
async fn try_wait_for_pwm_delay(mut input: Pin<Output, Dynamic>) {
    // TODO
}

#[embassy_executor::main]
async fn main(spawner: Spawner) {
    let mut dp = arduino_hal::Peripherals::take().unwrap();
    let pins = arduino_hal::pins!(dp);
    *SERIAL.lock().await = Some(arduino_hal::default_serial!(dp, pins, 57600));
    let d11 = pins.d11.into_output().downgrade();

    uwriteln!(
        &mut *SERIAL.lock().await.as_mut().unwrap(),
        "Initializing Servo"
    )
    .unwrap();
    unsafe { avr_servo::initialize_isr(dp.TC1).unwrap() };
    let mut servo = Servo::new(d11, 180).unwrap();
    uwriteln!(
        &mut *SERIAL.lock().await.as_mut().unwrap(),
        "Servo Initialized"
    )
    .unwrap();
    loop {
        ufmt::uwriteln!(&mut *SERIAL.lock().await.as_mut().unwrap(), "Servo ticks",).unwrap();
        servo.write(0).unwrap();

        delay_ms(1000);
    }
    // spawner.spawn(serial_print_a_every1sec()).unwrap();
    // spawner.spawn(try_wait_for_pwm_delay(d11)).unwrap();
}

#[panic_handler]
fn panic(info: &PanicInfo) -> ! {
    // disable interrupts - firmware has panicked so no ISRs should continue running
    avr_device::interrupt::disable();

    // get the peripherals so we can access serial and the LED.
    //
    // SAFETY: Because main() already has references to the peripherals this is an unsafe
    // operation - but because no other code can run after the panic handler was called,
    // we know it is okay.
    let dp = unsafe { arduino_hal::Peripherals::steal() };
    let pins = arduino_hal::pins!(dp);
    let serial = arduino_hal::default_serial!(dp, pins, 57600);
    let mut writer = Writer { u_write: serial };
    writeln!(&mut writer, "{}", info).ok();
    loop {}
}

struct Writer<T> {
    u_write: T,
}

impl<T: uWrite> Write for Writer<T> {
    fn write_str(&mut self, s: &str) -> core::fmt::Result {
        self.u_write.write_str(s).map_err(|x| match x {
            _ => core::fmt::Error,
        })
    }

    fn write_char(&mut self, c: char) -> core::fmt::Result {
        self.u_write.write_char(c).map_err(|x| match x {
            _ => core::fmt::Error,
        })
    }
}