#![no_std]
#![no_main]

use bsp::hal::{self, delay::Delay, gpio::v2::PA22};
use core::fmt::{self, Write};
use itsybitsy_m4 as bsp;
#[cfg(not(feature = "use_semihosting"))]
use panic_halt as _;
#[cfg(feature = "use_semihosting")]
use panic_semihosting as _;

use bsp::entry;
use core::cell::RefCell;
use cortex_m::{interrupt::Mutex, peripheral::NVIC};
use hal::{
    clock::GenericClockController,
    ehal::blocking::delay::DelayMs,
    ehal::digital::v2::ToggleableOutputPin,
    pac::{interrupt, CorePeripherals, Peripherals},
    time::Hertz,
    usb::{
        usb_device::{bus::UsbBusAllocator, prelude::*},
        UsbBus,
    },
};
use mks979b::{
    cmd::{Message, Setting, Value},
    fsm::{self, Mks979b, ResponseData},
};
use usbd_serial::{SerialPort, USB_CLASS_CDC};

static MAILBOX: Mutex<RefCell<Option<ResponseData>>> =
    Mutex::new(RefCell::new(None));

static TRANSDUCER: Mutex<RefCell<Option<Mks979b<bsp::Uart>>>> =
    Mutex::new(RefCell::new(None));

static mut LED: Option<
    hal::gpio::v2::Pin<PA22, hal::gpio::v2::PushPullOutput>,
> = None;
static mut USB_SERIAL: Option<USBSerial> = None;
static mut USB_BUS_ALLOCATOR: Option<UsbBusAllocator<UsbBus>> = None;

#[entry]
fn main() -> ! {
    let mut dp = Peripherals::take().unwrap();
    let mut core = CorePeripherals::take().unwrap();
    let mut clocks = GenericClockController::with_internal_32kosc(
        dp.GCLK,
        &mut dp.MCLK,
        &mut dp.OSC32KCTRL,
        &mut dp.OSCCTRL,
        &mut dp.NVMCTRL,
    );

    let pins = bsp::Pins::new(dp.PORT);
    let mut delay = Delay::new(core.SYST, &mut clocks);

    let bus_allocator = unsafe {
        USB_BUS_ALLOCATOR = Some(bsp::usb_allocator(
            dp.USB,
            &mut clocks,
            &mut dp.MCLK,
            pins.usb_dm,
            pins.usb_dp,
        ));
        USB_BUS_ALLOCATOR.as_ref().unwrap()
    };

    unsafe {
        USB_SERIAL = Some(USBSerial::new(bus_allocator));
    }

    let mut serialx = bsp::uart(
        &mut clocks,
        Hertz(9600),
        dp.SERCOM3,
        &mut dp.MCLK,
        pins.d0_rx,
        pins.d1_tx,
    );

    let mut index = 1;
    let mut raw_response: Option<ResponseData> = None;
    let sequence: [Message; 4] = [
        Setting::TestLedON(true).into(),
        Value::TestLedON.into(),
        Setting::TestLedON(false).into(),
        Value::TestLedON.into(),
    ];
    cortex_m::interrupt::free(|cs| {
        unsafe {
            LED = Some(pins.d13.into_push_pull_output());
        }
        serialx.enable_interrupts(hal::sercom::v2::uart::Flags::RXC);
        let mut transducer = fsm::Mks979b::new(serialx, 253);

        let _ = nb::block!(transducer.schedule_message(sequence[0].clone()))
            .unwrap();
        let _ = nb::block!(transducer.send_message());

        TRANSDUCER.borrow(cs).replace(Some(transducer));
    });
    unsafe {
        core.NVIC.set_priority(interrupt::USB_OTHER, 1);
        core.NVIC.set_priority(interrupt::USB_TRCPT0, 1);
        core.NVIC.set_priority(interrupt::USB_TRCPT1, 1);
        core.NVIC.set_priority(interrupt::SERCOM3_2, 1);

        NVIC::unmask(interrupt::USB_OTHER);
        NVIC::unmask(interrupt::USB_TRCPT0);
        NVIC::unmask(interrupt::USB_TRCPT1);
        NVIC::unmask(interrupt::SERCOM3_2);
    }

    loop {
        delay.delay_ms(400u16);
        //toggle();
        cortex_m::interrupt::free(|cs| {
            if let Some(raw) = MAILBOX.borrow(cs).replace(None) {
                raw_response = Some(raw);
            }
        });
        if let Some(raw) = &raw_response {
            if let Ok(resp) = fsm::parse_response(raw) {
                unsafe {
                    if let Some(usb) = USB_SERIAL.as_mut() {
                        let _ = write!(usb, "{}", resp);
                    }
                    raw_response = None;
                }
                index = if index == 3 { 0 } else { index + 1 };
                cortex_m::interrupt::free(|cs| {
                    if let Some(mks) =
                        TRANSDUCER.borrow(cs).borrow_mut().as_mut()
                    {
                        let _ = nb::block!(
                            mks.schedule_message(sequence[index].clone())
                        );
                        let _ = nb::block!(mks.send_message());
                    }
                });
            } else {
                raw_response = None;
                cortex_m::interrupt::free(|cs| {
                    if let Some(mks) =
                        TRANSDUCER.borrow(cs).borrow_mut().as_mut()
                    {
                        //Communication failed for x o y reason
                        let _ = nb::block!(mks.reschedule_last_message());
                        let _ = nb::block!(mks.send_message());
                    }
                });
            }
        }
    }
}

fn poll_usb() {
    unsafe {
        if let Some(usb) = USB_SERIAL.as_mut() {
            usb.poll();
            usb.flush();

            let mut buf = [0u8; 64];
            if let Ok(count) = usb.read(&mut buf) {
                for (i, c) in buf.iter().enumerate() {
                    if i >= count {
                        break;
                    }
                    let _ = write!(usb, "{}", c);
                }
            }
            toggle();
        }
    }
}

#[interrupt]
fn USB_OTHER() {
    poll_usb();
}

#[interrupt]
fn USB_TRCPT0() {
    poll_usb();
}

#[interrupt]
fn USB_TRCPT1() {
    poll_usb();
}

#[interrupt]
fn SERCOM3_2() {
    cortex_m::interrupt::free(|cs| {
        if let Some(transducer) = TRANSDUCER.borrow(cs).borrow_mut().as_mut() {
            if let Ok(Some(response)) = transducer.poll() {
                MAILBOX.borrow(cs).replace(Some(response));
            }
        } else {
            toggle();
            cortex_m::interrupt::disable();
            panic!("interrupt can't be cleared");
        }
    })
}

fn toggle() {
    unsafe {
        if let Some(x) = LED.as_mut() {
            x.toggle().unwrap();
        }
    }
}

pub struct USBSerial<'a> {
    serial: SerialPort<'a, UsbBus>,
    bus: UsbDevice<'a, UsbBus>,
}
impl<'a> USBSerial<'a> {
    pub fn new(bus_allocator: &'a UsbBusAllocator<UsbBus>) -> Self {
        USBSerial {
            serial: SerialPort::new(bus_allocator),
            bus: UsbDeviceBuilder::new(
                bus_allocator,
                UsbVidPid(0x16c0, 0x27dd),
            )
            .manufacturer("Fake company")
            .product("Serial port")
            .serial_number("TEST")
            .device_class(USB_CLASS_CDC)
            .build(),
        }
    }
    pub fn poll(&mut self) {
        self.bus.poll(&mut [&mut self.serial]);
    }
    pub fn flush(&mut self) {
        let _ = self.serial.flush();
    }
    pub fn read(&mut self, buf: &mut [u8]) -> Result<usize, UsbError> {
        self.serial.read(buf)
    }
}

impl fmt::Write for USBSerial<'_> {
    fn write_str(&mut self, string: &str) -> core::fmt::Result {
        let mut string = string.as_bytes();

        while !string.is_empty() {
            match self.serial.write(string) {
                Ok(count) => string = &string[count..],
                Err(UsbError::WouldBlock) => continue,
                Err(_) => return Err(fmt::Error),
            }
        }

        Ok(())
    }
}