//! DMA based audio output example for the LPCXpresso55S28 demo board
//!
//! Uses the onboard WM8904 DAC at 96KHz. Clock is generated by PLL0. Simple proportional feedback is implemented.
//!
//! USB walks around a static ring of slots, filling them as data comes in from
//! the host. DMA chases it, filling the I2S FIFO as it drains to the DAC.
//! Feedback ensures that the host doesn't overrun or underrun the ring.
//!
//! This implementation is more suitable for real use than the interrupt-based
//! example, but it is still missing many niceties and behaves worse in
//! anomalous situations since the DMA just keeps trucking over the ring
//! regardless of the data validity.
#![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, UsbVidPid},
};
use usbd_uac2::TerminalConfig;
use usbd_uac2::{
    self, AudioHandler, ClockSource, RangeEntry, UsbAudioClassConfig, UsbIsochronousFeedback,
    UsbSpeed, constants::FunctionCode, descriptors::ClockType,
};

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

mod dma;
mod hw;
mod wm8904;

// pid.codes test IDs
const USB_VID: u16 = 0x1209;
const USB_PID: u16 = 0x0001;
const USB_MANUFACTURER: &str = "usbd_uac2";
const USB_PRODUCT: &str = "DMA example device";

const CODEC_I2C_ADDR: u8 = 0b0011010;
const MCLK_FREQ: u32 = 12288000;

const SAMPLE_RATE: u32 = 96000;
const USB_FRAME_RATE: u32 = if cfg!(feature = "usbhs") { 8000 } else { 1000 };

//latency ≈ (current_fill × FRAMES_PER_SLOT)
//              + FRAMES_PER_SLOT/2 - average DMA transfer position
//              + 8 - FIFO depth @ 32-bit samples
// with example values, ~2.3ms
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 BYTES_PER_SLOT: usize = FRAMES_PER_SLOT * BYTES_PER_FRAME;
const N_SLOTS: usize = 8;
const FILL_TARGET_BYTES: i32 = (BYTES_PER_SLOT * N_SLOTS) as i32 / 2;

const LOG_PERIOD: u32 = 1000;

static DMA_RING: StaticCell<DmaRing<N_SLOTS, BYTES_PER_SLOT>> = StaticCell::new();
static mut DMA_RING_REF: Option<&'static DmaRing<N_SLOTS, BYTES_PER_SLOT>> = None;
#[inline]
fn dma_ring() -> &'static DmaRing<N_SLOTS, BYTES_PER_SLOT> {
    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>,
    log_counter: u32,
}
impl<const N: usize, const MAX_SLOT_BYTES: usize> Audio<'_, N, MAX_SLOT_BYTES> {
    const RATES: [RangeEntry<u32>; 1] = [RangeEntry::new_fixed(SAMPLE_RATE)];
    fn start(&mut self) {
        red_led().off(); // clear any dma error
        self.running.store(false, Ordering::Relaxed);

        defmt::info!("playback armed (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.
        pac::NVIC::mask(pac::Interrupt::DMA0);
        self.running.store(false, Ordering::Relaxed);
        dma_ring().stop();
        defmt::info!("playback stopped");
        green_led().off();
        blue_led().off();
    }
}
impl<const N: usize, const MAX_SLOT_BYTES: usize, B: bus::UsbBus> AudioHandler<'_, 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).
        // Buffer must have room for one additional frame in case the host clock runs slower than the device.
        let mut buf = [0; (SAMPLE_RATE.div_ceil(USB_FRAME_RATE) + 1) 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::info!(
                "buffer warmed ({} slots) starting playback",
                self.dma.fill_slots()
            );
            self.dma.run();
            self.running.store(true, Ordering::Relaxed)
        }
    }
    fn audio_data_tx(
        &mut self,
        _ep: &usb_device::endpoint::Endpoint<'_, B, usb_device::endpoint::In>,
    ) {
    }

    /// Provide rate feedback to the host. P-only is stable and works fine, with
    /// most hosts. The host can either filter it internally or treat it
    /// instantaneously and send more data specifically when the error gets
    /// large; we will absorb reasonable clock drifts with our ring buffer.
    fn feedback(&mut self, nominal_rate: UsbIsochronousFeedback) -> Option<UsbIsochronousFeedback> {
        // Don't want to signal an absurd rate when not consuming; let the
        // buffer fill before starting feedback.
        if !self.running.load(Ordering::Acquire) {
            return Some(nominal_rate);
        }

        let produced_bytes = self.dma.produced_bytes();
        let consumed_bytes = self.dma.consumed_bytes();

        if produced_bytes < consumed_bytes || produced_bytes == 0 {
            defmt::error!("[fb] dma underrun detected!");
            red_led().on();
            return Some(nominal_rate);
        }

        let current_bytes = (produced_bytes - consumed_bytes) as i32;
        // normalize error wrt. frame size etc.
        let error_permille = ((current_bytes - FILL_TARGET_BYTES) * 1000) / FILL_TARGET_BYTES;

        let nominal_v = nominal_rate.to_u32_12_13() as i32;

        // 0.2% which is a huge clock error
        let max_allowed_deviation = nominal_v / 500;

        let p_term = -(error_permille * nominal_v) / 256000; // this works reasonably well to keep the buffer
        let i_term = 0; // placeholder

        let mut v = nominal_v + p_term + i_term;
        v = v.clamp(
            nominal_v - max_allowed_deviation,
            nominal_v + max_allowed_deviation,
        );
        self.log_counter += 1;
        if self.log_counter.is_multiple_of(LOG_PERIOD) {
            defmt::info!(
                "fill:{}% err_pm:{} p:{} i:{} fb_delta:{} fb:{=u32:x}",
                (current_bytes * 100) / (N_SLOTS * BYTES_PER_SLOT) as i32,
                error_permille,
                p_term,
                i_term,
                v - nominal_v,
                v as u32
            );
        }

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

impl<const N: usize, const MAX_SLOT_BYTES: usize> ClockSource for Audio<'_, N, MAX_SLOT_BYTES> {
    const CLOCK_TYPE: usbd_uac2::descriptors::ClockType = ClockType::InternalFixed;
    const SOF_SYNC: bool = false;
    fn sample_rate(&self) -> u32 {
        Self::RATES[0].min
    }
    fn sample_rates(
        &self,
    ) -> core::result::Result<&[usbd_uac2::RangeEntry<u32>], usbd_uac2::UsbAudioClassError> {
        Ok(&Self::RATES)
    }
    fn clock_validity(&self) -> core::result::Result<bool, usbd_uac2::UsbAudioClassError> {
        Ok(true)
    }
}

#[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 driver, 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");
    let clocks = hal::ClockRequirements::default()
        .system_frequency(96.MHz())
        .configure(&mut anactrl, &mut pmc, &mut syscon)
        .unwrap();
    let mut usb_delay_timer = Timer::new(
        hal.ctimer
            .0
            .enabled(&mut syscon, clocks.support_1mhz_fro_token().unwrap()),
    );

    // 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 usb_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::<N_SLOTS, BYTES_PER_SLOT>::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 audio = Audio {
        i2s: i2s_peripheral,
        dma: dma_ring(),
        running: AtomicBool::new(false),
        log_counter: 0,
    };
    defmt::debug!("usb init");
    let config = UsbAudioClassConfig::new(usb_speed, FunctionCode::IoBox, &mut audio)
        .with_output_config(TerminalConfig::builder().base_id(2).build());
    let mut uac2 = config.build(&usb_bus).unwrap();

    let mut usb_dev = usbd_uac2::builder(&usb_bus, UsbVidPid(USB_VID, USB_PID))
        .strings(&[StringDescriptors::default()
            .manufacturer(USB_MANUFACTURER)
            .product(USB_PRODUCT)])
        .unwrap()
        .max_packet_size_0(64) // Required to be 64 on HS, allowed on FS
        .unwrap()
        .build();

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

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