//! Standard library support crate for an Raspberry Pi Pico with an RP2040.
//!
//! The blinky support example comes from the rp-hal-boards example, available
//! <https://github.com/rp-rs/rp-hal-boards/blob/5135e3dafe3e69b112e6d2d72cfb1856a7679b82/boards/rp-pico/examples/pico_blinky.rs#L1>
//! and licensed MIT or APACHE-2.0.

#![no_std]
#![feature(rustc_private)]
extern crate fusible;

use core::pin::Pin;

use rust_threadx_cortex_m as _;
use rust_threadx_sync_single_core as _;

use embedded_hal::digital::OutputPin;
use rp_pico::hal;
use rp_pico::hal::pac;
use usb_device::{class_prelude::*, prelude::*};
use usbd_serial::SerialPort;

extern crate alloc;
use alloc::vec::Vec;

#[doc(hidden)]
#[unsafe(no_mangle)]
pub unsafe extern "C" fn _tx_initialize_low_level() {
    use cortex_m::{
        Peripherals,
        peripheral::{scb::SystemHandler, syst::SystClkSource},
    };

    cortex_m::interrupt::disable();

    unsafe {
        let Peripherals {
            mut SYST, mut SCB, ..
        } = Peripherals::steal();

        SCB.set_priority(SystemHandler::SysTick, 0x40);
        SCB.set_priority(SystemHandler::PendSV, 0xff);
        SCB.set_priority(SystemHandler::SVCall, 0xff);

        SYST.set_clock_source(SystClkSource::Core);
        SYST.set_reload(125_000_000 / 100 - 1);
        SYST.clear_current();
        SYST.enable_interrupt();
        SYST.enable_counter();
    }
}

use fusible::queue::{Queue, QueueContext, StaticQueueSlots};
use fusible::semaphore::{Semaphore, SemaphoreContext};
use fusible::thread::{StaticStack, Thread, ThreadContext};

static BLINK_STACK: StaticStack<512> = StaticStack::new();
static BLINK_THREAD: ThreadContext = Thread::context();

static USB_STACK: StaticStack<8192> = StaticStack::new();
static USB_THREAD: ThreadContext = Thread::context();
static USB_EVENT: SemaphoreContext = Semaphore::context();

static STDOUT_MSGS: StaticQueueSlots<Vec<u8>, 4> = StaticQueueSlots::new();
static STDOUT_QUEUE: QueueContext<Vec<u8>> = Queue::context();

#[unsafe(no_mangle)]
#[doc(hidden)]
pub extern "C" fn __rust_threadx_app_define() {
    let mut pac = pac::Peripherals::take().unwrap();

    let mut watchdog = hal::Watchdog::new(pac.WATCHDOG);

    let clocks = hal::clocks::init_clocks_and_plls(
        rp_pico::XOSC_CRYSTAL_FREQ,
        pac.XOSC,
        pac.CLOCKS,
        pac.PLL_SYS,
        pac.PLL_USB,
        &mut pac.RESETS,
        &mut watchdog,
    )
    .ok()
    .unwrap();

    let sio = hal::Sio::new(pac.SIO);

    let pins = rp_pico::Pins::new(
        pac.IO_BANK0,
        pac.PADS_BANK0,
        sio.gpio_bank0,
        &mut pac.RESETS,
    );

    let mut led_pin = pins.led.into_push_pull_output();

    Thread::create(
        Pin::static_ref(&BLINK_THREAD),
        BLINK_STACK.take().unwrap(),
        &Default::default(),
        move || loop {
            led_pin.set_high().unwrap();
            fusible::thread::sleep(50);
            led_pin.set_low().unwrap();
            fusible::thread::sleep(50);
        },
    )
    .unwrap();

    let usb_event = Semaphore::create(Pin::static_ref(&USB_EVENT), &Default::default()).unwrap();
    let stdout_queue = Queue::create(
        Pin::static_ref(&STDOUT_QUEUE),
        STDOUT_MSGS.take().unwrap(),
        &Default::default(),
    )
    .unwrap();

    Thread::create(
        Pin::static_ref(&USB_THREAD),
        USB_STACK.take().unwrap(),
        &Default::default(),
        move || {
            let usb_bus = UsbBusAllocator::new(hal::usb::UsbBus::new(
                pac.USBCTRL_REGS,
                pac.USBCTRL_DPRAM,
                clocks.usb_clock,
                true,
                &mut pac.RESETS,
            ));

            let mut serial = SerialPort::new(&usb_bus);
            let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x16c0, 0x27dd))
                .strings(&[StringDescriptors::default()
                    .manufacturer("Fake company")
                    .product("Serial port")
                    .serial_number("TEST")])
                .unwrap()
                .device_class(2)
                .build();

            loop {
                unsafe { cortex_m::peripheral::NVIC::unmask(hal::pac::Interrupt::USBCTRL_IRQ) };
                usb_event.get().unwrap();
                usb_dev.poll(&mut [&mut serial]);

                serial.read(&mut []).ok();

                if let Some(msg) = stdout_queue.try_receive() {
                    let _ = serial.write(&msg);
                }
            }
        },
    )
    .unwrap();
}

use hal::pac::interrupt;

#[interrupt]
fn USBCTRL_IRQ() {
    cortex_m::peripheral::NVIC::mask(hal::pac::interrupt::USBCTRL_IRQ);

    // Safety: If the interrupt is unmasked, then we created
    // the semaphore.
    unsafe { Pin::static_ref(&USB_EVENT).assume_created().put() };
}

fn stdout_write(buf: &[u8]) -> usize {
    let Some(queue) = Pin::static_ref(&STDOUT_QUEUE).try_created() else {
        return 0;
    };

    let msg = Vec::from(buf);
    queue.send(msg).unwrap();
    // Wake up the USB thread so it can see our message.
    if let Some(usb_event) = Pin::static_ref(&USB_EVENT).try_created() {
        usb_event.put();
    }
    buf.len()
}

#[unsafe(no_mangle)]
fn __rust_threadx_stdout_write(buf: &[u8]) -> usize {
    stdout_write(buf)
}

#[unsafe(no_mangle)]
fn __rust_threadx_stderr_write(buf: &[u8]) -> usize {
    stdout_write(buf)
}

#[unsafe(no_mangle)]
fn __rust_threadx_stdin_read(_: &mut [u8]) -> usize {
    0 // Haven't tried it.
}