//! Interrupt driven example for the LPCXpresso55S28 demo board
//!
//! Uses the onboard WM8904 DAC at 48KHz. Clock is generated by PLL0. Simple PI feedback
//! is implemented.
//!
//! Packets from USB are placed a `heapless::spsc::Queue`. They are consumed
//! by the I2S FIFO in the FLEXCOMM7 interrupt.
#![no_main]
#![no_std]

#[cfg(all(feature = "usbfs", feature = "usbhs"))]
compile_error!("Choose one USB peripheral, usbfs and usbhs cannot be used together");

extern crate panic_probe;
#[defmt::panic_handler]
fn panic() -> ! {
    panic_probe::hard_fault()
}

use core::sync::atomic::{AtomicBool, Ordering};
use cortex_m_rt::entry;
use defmt::debug;
use defmt_rtt as _;
use hal::raw as pac;
use hal::{
    Syscon,
    drivers::{Timer, UsbBus, pins},
    prelude::*,
    time::Hertz,
};
use lpc55_hal as hal;
use pac::interrupt;
use static_cell::StaticCell;
use usb_device::{
    bus::{self},
    device::{StringDescriptors, UsbDeviceBuilder, UsbVidPid},
    endpoint::IsochronousSynchronizationType,
};
use usbd_uac2::UsbIsochronousFeedback;
use usbd_uac2::{
    self, AudioClassConfig, RangeEntry, TerminalConfig, UsbAudioClass, UsbAudioClockImpl, UsbSpeed,
    constants::{FunctionCode, TerminalType},
    descriptors::{ChannelConfig, ClockType, FormatType1, LockDelay},
};

use crate::dma::DmaRing;
use crate::hw::{I2sTx, blue_led, green_led, red_led};

mod dma;
mod hw;
mod wm8904;

const CODEC_I2C_ADDR: u8 = 0b0011010;
const MCLK_FREQ: u32 = 12288000;
const SAMPLE_RATE: u32 = 96000;
//latency ≈ (current_fill × FRAMES_PER_SLOT)
//              + FRAMES_PER_SLOT/2 - average DMA transfer position
//              + 8 - FIFO depth @ 32-bit samples
const BYTES_PER_SAMPLE: usize = 4; // 32 bit samples
const BYTES_PER_FRAME: usize = BYTES_PER_SAMPLE * 2; // 2 channels
const FRAMES_PER_SLOT: usize = SAMPLE_RATE as usize / 2000; // run the DMA at 2khz
const SLOT_SIZE_BYTES: usize = FRAMES_PER_SLOT * BYTES_PER_FRAME; // run the DMA at 2khz
const N_SLOTS: usize = 32;
const FILL_TARGET: i32 = (FRAMES_PER_SLOT * N_SLOTS) as i32 / 2;

struct Clock {}
impl Clock {
    const RATES: [RangeEntry<u32>; 1] = [RangeEntry::new_fixed(SAMPLE_RATE)];
}
impl UsbAudioClockImpl for Clock {
    const CLOCK_TYPE: usbd_uac2::descriptors::ClockType = ClockType::InternalFixed;
    const SOF_SYNC: bool = false;
    fn get_sample_rate(&self) -> u32 {
        Clock::RATES[0].min
    }
    fn get_rates(
        &self,
    ) -> core::result::Result<&[usbd_uac2::RangeEntry<u32>], usbd_uac2::UsbAudioClassError> {
        Ok(&Clock::RATES)
    }
    fn get_clock_validity(&self) -> core::result::Result<bool, usbd_uac2::UsbAudioClassError> {
        Ok(true)
    }
}

static DMA_RING: StaticCell<DmaRing<N_SLOTS, SLOT_SIZE_BYTES>> = StaticCell::new();
static mut DMA_RING_REF: Option<&'static DmaRing<N_SLOTS, SLOT_SIZE_BYTES>> = None;
#[inline]
fn dma_ring() -> &'static DmaRing<N_SLOTS, SLOT_SIZE_BYTES> {
    unsafe { DMA_RING_REF.unwrap() }
}

#[interrupt]
fn DMA0() {
    let dma = unsafe { &*pac::DMA0::ptr() };

    let inta = dma.inta0.read().bits();
    let err = dma.errint0.read().bits();

    if (err & (1 << 19)) != 0 {
        let live = dma.channel19.xfercfg.read().bits();

        let desc = unsafe { &*dma_ring().channel_desc.get() };
        let mem = desc.d[19];
        defmt::error!(
            "DMA error ch19: live={=u32:08x} INTA={=u32:x} ERR={=u32:x}\n         desc: {}",
            live,
            inta,
            err,
            mem
        );
        red_led().on();
        dma.errint0.write(|w| unsafe { w.bits(1 << 19) });
    }

    if (inta & (1 << 19)) != 0 {
        dma.inta0.write(|w| unsafe { w.bits(1 << 19) });
        if dma_ring().advance_consumed(1).is_err() {
            red_led().on();
        }
    }
}

struct Audio<'a, const N: usize, const MAX_SLOT_BYTES: usize> {
    running: AtomicBool,
    i2s: I2sTx,
    dma: &'a DmaRing<N, MAX_SLOT_BYTES>,
}
impl<const N: usize, const MAX_SLOT_BYTES: usize> Audio<'_, N, MAX_SLOT_BYTES> {
    fn start(&self) {
        red_led().off(); // clear any dma error
        self.running.store(false, Ordering::Relaxed);

        defmt::info!("playback starting (DMA)");

        let i2s = &self.i2s.i2s;
        i2s.fifotrig
            .modify(|_, w| unsafe { w.txlvl().bits(6).txlvlena().enabled() });

        // Enable TX FIFO
        i2s.fifocfg
            .modify(|_, w| w.enabletx().enabled().dmatx().enabled());
        dma_ring().init();
        // Enable DMA interrupt (channel 19)
        unsafe { pac::NVIC::unmask(pac::Interrupt::DMA0) };

        green_led().on();
    }
    fn stop(&self) {
        // If we don't disable interrupts while stopped, we will underflow constantly and continuously refill the fifo with 0s
        // We could actually stop the I2S here, but sometimes that makes the DAC misbehave. The peripheral is configured to send
        // 0s when the FIFO is empty, so this is fine.
        self.running.store(false, Ordering::Relaxed);
        dma_ring().stop();
        defmt::info!("playback stopped");
        pac::NVIC::mask(pac::Interrupt::DMA0);
        green_led().off();
        blue_led().off();
    }
}
impl<const N: usize, const MAX_SLOT_BYTES: usize, B: bus::UsbBus> UsbAudioClass<'_, B>
    for Audio<'_, N, MAX_SLOT_BYTES>
{
    fn alternate_setting_changed(&mut self, _terminal: usb_device::UsbDirection, alt_setting: u8) {
        // alt setting 0 means stopped
        match alt_setting {
            0 => self.stop(),
            1 => self.start(),
            _ => defmt::error!("unexpected alt setting {}", alt_setting),
        }
    }
    fn audio_data_rx(
        &mut self,
        ep: &usb_device::endpoint::Endpoint<'_, B, usb_device::endpoint::Out>,
    ) {
        // Buffer must fit 125us of audio data (based on how `usbd_uac2` sets up the descriptors).
        let mut buf = [0; SAMPLE_RATE.div_ceil(8000) as usize * BYTES_PER_FRAME];

        let len = match ep.read(&mut buf) {
            Ok(len) => len,
            Err(_) => {
                defmt::error!("usb error in rx callback");
                return;
            }
        };

        let buf = &buf[..len];
        let res = self.dma.push(buf);

        if res.dropped != 0 {
            // Overflow: some or all bytes couldn't be queued.
            blue_led().toggle();
            defmt::error!(
                "overflowed dma ring, asked {}, wrote {}, dropped {}",
                buf.len(),
                res.written,
                res.dropped
            );
        }
        // If we're not running yet, wait until we reach 50% full then enable DMA requests
        if !self.running.load(Ordering::Relaxed) && self.dma.fill_slots() >= (N_SLOTS / 2) {
            defmt::debug!(
                "buffer has {} slots, start dma transfers",
                self.dma.fill_slots()
            );
            self.dma.run();
            self.running.store(true, Ordering::Relaxed)
        }
    }

    /// Provide rate feedback to the host, so that it doesn't over- or underflow
    /// the buffer. Proportional-only control is stable with normal hosts,
    /// adding an I term with proper tuning (quite weak) would stabilize the
    /// rate reported to the host but is not necessary for basic playback.
    fn feedback(&mut self, nominal_rate: UsbIsochronousFeedback) -> Option<UsbIsochronousFeedback> {
        const MAX_CORRECTION: i32 = 1 << 10; // ~1.6%

        let produced_bytes = self.dma.produced_bytes.load(Ordering::Acquire);
        let consumed_bytes = self.dma.consumed_bytes();

        let valid = produced_bytes >= consumed_bytes; // else we are in underrun condition

        let fill_frames = if valid {
            (produced_bytes - consumed_bytes) as i32 / BYTES_PER_FRAME as i32
        } else {
            // we will emit a canonical error in the DMA ISR
            defmt::debug!("[fb] dma underrun detected");
            0
        };

        let mut error = fill_frames - FILL_TARGET;
        error = error.clamp(-32, 32); // avoid huge spikes

        let p = error * 256;
        let i = 0; // placeholder

        let correction = -(p + i);

        let nominal_v = nominal_rate.to_u32_12_13() as i32;

        let mut v = nominal_v + correction;

        v = v.clamp(nominal_v - MAX_CORRECTION, nominal_v + MAX_CORRECTION);

        defmt::debug!(
            "valid:{} fill:{} err:{} fb:{=u32:x}",
            valid,
            fill_frames,
            error,
            v as u32
        );

        Some(UsbIsochronousFeedback::new(v as u32))
    }
}

#[entry]
fn main() -> ! {
    let hal = hal::new();

    let mut anactrl = hal.anactrl;
    let mut pmc = hal.pmc;
    let mut syscon = hal.syscon;

    let mut gpio = hal.gpio.enabled(&mut syscon);
    let mut iocon = hal.iocon.enabled(&mut syscon);

    debug!("start");

    hw::init_leds(&mut iocon, &mut gpio);

    debug!("iocon");
    let usb0_vbus_pin = pins::Pio0_22::take()
        .unwrap()
        .into_usb0_vbus_pin(&mut iocon);
    let codec_i2c_pins = (
        pins::Pio1_20::take().unwrap().into_i2c4_scl_pin(&mut iocon),
        pins::Pio1_21::take().unwrap().into_i2c4_sda_pin(&mut iocon),
    );
    // We can initialize and iocon these, but there is no peripheral, so they do not get used
    let _codec_i2s_pins = (
        pins::Pio0_21::take().unwrap().into_spi7_sck_pin(&mut iocon),
        pins::Pio0_20::take().unwrap().into_i2s7_sda_pin(&mut iocon),
        pins::Pio0_19::take().unwrap().into_i2s7_ws_pin(&mut iocon),
        pins::Pio1_31::take().unwrap(), // MCLK
    );

    debug!("clocks");
    // Run the system clock at 96MHz. The lpc55-hal will run it from the FRO. But we won't actually use these clocks, we just need the guards...
    let clocks = hal::ClockRequirements::default()
        .system_frequency(96.MHz())
        .configure(&mut anactrl, &mut pmc, &mut syscon)
        .unwrap();
    let mut _delay_timer = Timer::new(
        hal.ctimer
            .0
            .enabled(&mut syscon, clocks.support_1mhz_fro_token().unwrap()),
    );
    // Start PLL1 at 150MHz as main system clock
    hw::init_sys_pll1();
    // Start PLL0 at 24.576MHz as the audio clock. The FRO cannot evenly divide
    // any common audio frequencies and is not particularly stable anyway.
    hw::init_audio_pll();

    debug!("peripherals");

    let i2c_peripheral = hal
        .flexcomm
        .4
        .enabled_as_i2c(&mut syscon, &clocks.support_flexcomm_token().unwrap());
    let mut i2c_bus = I2cMaster::new(
        i2c_peripheral,
        codec_i2c_pins,
        Hertz::try_from(400.kHz()).unwrap(),
    );

    let i2s_peripheral = {
        let fc7 = hal.flexcomm.7.release();
        hw::init_i2s(fc7.0, fc7.2, &mut syscon)
    };

    #[cfg(feature = "usbhs")]
    let (usb_speed, usb_peripheral) = (
        UsbSpeed::High,
        hal.usbhs.enabled_as_device(
            &mut anactrl,
            &mut pmc,
            &mut syscon,
            &mut _delay_timer,
            clocks.support_usbhs_token().unwrap(),
        ),
    );
    #[cfg(feature = "usbfs")]
    let (usb_speed, usb_peripheral) = (
        UsbSpeed::Full,
        hal.usbfs.enabled_as_device(
            &mut anactrl,
            &mut pmc,
            &mut syscon,
            clocks.support_usbfs_token().unwrap(),
        ),
    );

    let usb_bus = UsbBus::new(usb_peripheral, usb0_vbus_pin);

    defmt::debug!("codec init");
    wm8904::init_codec(&mut i2c_bus);

    defmt::debug!("dma init");
    let i2s_dma_addr = &i2s_peripheral.i2s.fifowr as *const _ as *mut u32;
    let dma = DmaRing::<32, SLOT_SIZE_BYTES>::new(hal.dma.release(), &mut syscon, i2s_dma_addr, 4)
        .unwrap();
    let dma_ref = DMA_RING.init(dma);
    unsafe { DMA_RING_REF = Some(dma_ref) };

    let mut clock = Clock {};
    let mut audio = Audio {
        i2s: i2s_peripheral,
        dma: dma_ring(),
        running: AtomicBool::new(false),
    };
    defmt::debug!("usb init");
    let config = AudioClassConfig::new(usb_speed, FunctionCode::Other, &mut clock, &mut audio)
        .with_output_config(TerminalConfig::new(
            4,
            1,
            2,
            FormatType1 {
                bit_resolution: 32,
                bytes_per_sample: 4,
            },
            TerminalType::ExtLineConnector,
            ChannelConfig::default_chans(2),
            IsochronousSynchronizationType::Asynchronous,
            LockDelay::Milliseconds(10),
            None,
        ));

    let mut uac2 = config.build(&usb_bus).unwrap();

    let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x1209, 0xcc1d))
        .composite_with_iads()
        .strings(&[StringDescriptors::default()
            .manufacturer("Generic")
            .product("usbd_uac2 device")
            .serial_number("123456789")])
        .unwrap()
        .max_packet_size_0(64)
        .unwrap()
        .device_class(0xef)
        .device_sub_class(0x02)
        .device_protocol(0x01)
        .build();

    defmt::info!("main loop");

    loop {
        usb_dev.poll(&mut [&mut uac2]);
    }
}