#![no_main]
#![no_std]

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

use atomic::Atomic;
use core::sync::atomic::{AtomicBool, AtomicI32, AtomicUsize, Ordering};
use cortex_m_rt::entry;
use defmt;
use defmt::debug;
use defmt_rtt as _;
use hal::Pin;
use hal::Syscon;
use hal::drivers::{Timer, UsbBus, pins, pins::direction::Output};
use hal::prelude::*;
use hal::raw as pac;
use hal::time::{Hertz, Microseconds};
use hal::typestates::pin::state::Gpio;
use lpc55_hal as hal;
use pac::interrupt;
use static_cell::StaticCell;
use usb_device::{
    bus::{self},
    device::{StringDescriptors, UsbVidPid},
};
#[cfg(feature = "hid")]
use usbd_hid::{descriptor::SerializedDescriptor, hid_class::HIDClass};
use usbd_uac2::{
    self, AudioHandler, ClockSource, RangeEntry, TerminalConfig, UsbAudioClassConfig,
    UsbAudioClassError, UsbIsochronousFeedback, UsbSpeed,
    constants::{FunctionCode, TerminalType},
    descriptors::ClockType,
};

use crate::dac::DacImpl;
use crate::dma::DmaRing;
use crate::hw::led1;
use crate::hw::led2;
use crate::traits::Dac;
use shared::AudioState;
use shared::hid::AudioTelemetryReport;

#[cfg(feature = "ak4490")]
pub mod dac {
    mod ak4490;
    pub use self::ak4490::Ak4490Dac as DacImpl;
}
#[cfg(feature = "cs4398")]
pub mod dac {
    mod cs4398;
    pub use self::cs4398::Cs4398Dac as DacImpl;
}
#[cfg(feature = "nodac")]
pub mod dac {
    mod noop;
    pub use self::noop::NoopDac as DacImpl;
}
#[cfg(feature = "wm8904")]
pub mod dac {
    mod wm8904;
    pub use self::wm8904::Wm8904Dac as DacImpl;
}

mod dma;
mod hw;
mod traits;

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 / 4000; // run the DMA at 4khz
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 USB_FRAME_RATE: u32 = 8000; // microframe rate: 8000 for HS, 1000 for FS

// In frames
const QUEUE_RUNNING_UP: usize = ((FRAMES_PER_SLOT * N_SLOTS) * 5) / 10; // 50%
const QUEUE_RUNNING_DOWN: usize = ((FRAMES_PER_SLOT * N_SLOTS) * 2) / 10; // 20%
const NODATA_TIMEOUT_FRAMES: usize = SAMPLE_RATE as usize / 100; // ~100ms
#[cfg(not(feature = "evk"))]
const MCLK_FREQ: u32 = 24576000;
#[cfg(feature = "evk")]
const MCLK_FREQ: u32 = 24576000 / 2;

const SAMPLE_RATE: u32 = 192000;
const HID_INTERVAL_MS: u8 = 10;

struct CodecPins {
    reset: Pin<pins::Pio0_3, Gpio<Output>>,
}

struct ClockSelPins {
    sel_24m: Pin<pins::Pio0_27, Gpio<Output>>,
    sel_22m: Pin<pins::Pio0_31, Gpio<Output>>,
}

#[derive(Default)]
struct PerfCounters {
    state: Atomic<AudioState>,
    received_frames: AtomicUsize,
    played_frames: AtomicUsize,
    min_fill: AtomicUsize,
    avg_fill: AtomicUsize,
    queue_underflows: AtomicUsize,
    queue_overflows: AtomicUsize,
    audio_underflows: AtomicUsize,
    integrator: AtomicI32,
    p: AtomicI32,
    i: AtomicI32,
    fb: AtomicI32,
}

impl PerfCounters {
    fn reset(&self) {
        self.received_frames.store(0, Ordering::Relaxed);
        self.played_frames.store(0, Ordering::Relaxed);
        self.min_fill
            .store(N_SLOTS * BYTES_PER_SLOT, Ordering::Relaxed);
        self.avg_fill
            .store(FILL_TARGET_BYTES as usize, Ordering::Relaxed);
        self.queue_underflows.store(0, Ordering::Relaxed);
        self.queue_overflows.store(0, Ordering::Relaxed);
        self.audio_underflows.store(0, Ordering::Relaxed);
        self.p.store(0, Ordering::Relaxed);
        self.i.store(0, Ordering::Relaxed);
        // FB loop will have to take care of the fb value
    }
    fn build_report(&self) -> AudioTelemetryReport {
        AudioTelemetryReport {
            state: self.state.load(Ordering::Relaxed) as u8,
            average_buffer_fill: self.avg_fill.load(Ordering::Relaxed) as u16,
            frame_count: self.played_frames.load(Ordering::Relaxed) as i32,
            dac_underflow_count: self.audio_underflows.load(Ordering::Relaxed) as u16,
            usb_underflow_count: self.queue_underflows.load(Ordering::Relaxed) as u16,
            dac_overflow_count: self.queue_overflows.load(Ordering::Relaxed) as u16,
            integrator: self.integrator.load(Ordering::Relaxed),
            p: self.p.load(Ordering::Relaxed),
            i: self.i.load(Ordering::Relaxed),
            fb: u32::cast_signed(self.fb.load(Ordering::Relaxed) as u32),
        }
    }
}

impl defmt::Format for PerfCounters {
    fn format(&self, fmt: defmt::Formatter) {
        defmt::write!(
            fmt,
            "frames: {}/{} min_fill: {} avg fill: {} a_underflows: {} q_underflows: {} q_overflows: {}",
            self.played_frames.load(Ordering::Relaxed),
            self.received_frames.load(Ordering::Relaxed),
            self.min_fill.load(Ordering::Relaxed),
            self.avg_fill.load(Ordering::Relaxed),
            self.audio_underflows.load(Ordering::Relaxed),
            self.queue_underflows.load(Ordering::Relaxed),
            self.queue_overflows.load(Ordering::Relaxed)
        )
    }
}

static PERF: PerfCounters = PerfCounters {
    state: Atomic::new(AudioState::Stopped),
    received_frames: AtomicUsize::new(0), // received from USB
    played_frames: AtomicUsize::new(0),   // played audio frames
    min_fill: AtomicUsize::new(0), // not recording this for now, need to figure out how to make it meaningful, since the queue starts empty
    avg_fill: AtomicUsize::new(FILL_TARGET_BYTES as usize),
    queue_underflows: AtomicUsize::new(0), // ditto here, since we underflow at startup, but we record this one as it can be trended
    queue_overflows: AtomicUsize::new(0),
    audio_underflows: AtomicUsize::new(0),
    integrator: AtomicI32::new(0),
    p: AtomicI32::new(0),
    i: AtomicI32::new(0),
    fb: AtomicI32::new(0),
};

static NODATA_FLAG: AtomicBool = AtomicBool::new(false);

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() }
}

fn cur_fill() -> usize {
    let produced_bytes = dma_ring().produced_bytes() as u32;
    let consumed_bytes = dma_ring().consumed_bytes() as u32;

    // Handle rollover properly
    produced_bytes.wrapping_sub(consumed_bytes) as usize
}

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

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

    // TODO: figure out how to track underflows properly
    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
        );
        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() {
            PERF.audio_underflows.fetch_add(1, Ordering::Relaxed);
        } else {
            led1().toggle();
            PERF.played_frames
                .fetch_add(FRAMES_PER_SLOT, Ordering::Relaxed);
        }
        if cur_fill() <= BYTES_PER_SLOT {
            led2().on();
            NODATA_FLAG.store(true, Ordering::Release);
        }
    }
}

#[interrupt]
fn FLEXCOMM7() {
    // Count I2S TX FIFO error (should be underrun, assuming we set up our DMA trigger correctly)
    PERF.audio_underflows.fetch_add(1, Ordering::Relaxed);
    unsafe { &*pac::I2S7::ptr() }
        .fifostat
        .modify(|_, w| w.txerr().set_bit())
}

struct FeedbackState {
    correction_enabled: AtomicBool,
    integrator: AtomicI32,
    filtered_fill: AtomicI32,
}
impl FeedbackState {
    fn start(&mut self) {
        self.correction_enabled.store(true, Ordering::Relaxed);
    }
    fn reset(&mut self) {
        self.correction_enabled.store(false, Ordering::Relaxed);
        self.integrator.store(0, Ordering::Relaxed);
        self.filtered_fill
            .store(FILL_TARGET_BYTES, Ordering::Relaxed);
    }
}
impl Default for FeedbackState {
    fn default() -> Self {
        Self {
            correction_enabled: AtomicBool::new(false),
            integrator: AtomicI32::new(0),
            filtered_fill: AtomicI32::new(FILL_TARGET_BYTES),
        }
    }
}

struct Audio<'a, D: Dac<I>, I> {
    state: Atomic<AudioState>,
    alt_setting: u8,
    i2s: I2sTx,
    dac: D,
    dma: &'a DmaRing<N_SLOTS, BYTES_PER_SLOT>,
    fb: FeedbackState,
    nodata_timeout_frame: AtomicUsize,
    cur_rate: u32,
    clock_pins: ClockSelPins,
    _marker: core::marker::PhantomData<I>,
}
impl<D: Dac<I>, I> Audio<'_, D, I> {
    const RATES: [RangeEntry<u32>; 1] = [RangeEntry::new_fixed(SAMPLE_RATE)];
    /// Perform a state transition to `state`
    fn transition(&mut self, state: AudioState) {
        defmt::info!(
            "AudioState {} -> {}",
            self.state.load(Ordering::Relaxed),
            state
        );
        match state {
            AudioState::Stopped => self.stop(),
            AudioState::Armed => self.arm(),
            AudioState::Prefill => self.prefill(),
            AudioState::Running => self.run(),
            AudioState::LowData => {}
            AudioState::NoData => self.nodata(),
            AudioState::Stopping => self.stopping(),
        }
        self.state.store(state, Ordering::SeqCst);
        PERF.state.store(state, Ordering::Relaxed);
    }

    fn init(&mut self) {
        let regs = &self.i2s.i2s;
        // Enable TX FIFO only
        regs.fifocfg.modify(|_, w| {
            w.enabletx()
                .enabled()
                .enablerx()
                .disabled()
                .dmatx()
                .disabled()
                .txi2se0()
                .zero()
        });

        // Flush
        regs.fifocfg.modify(|_, w| w.emptytx().set_bit());

        regs.cfg2
            .modify(|_, w| unsafe { w.position().bits(0).framelen().bits(63) }); // framelen = 64

        let bclk_div = (MCLK_FREQ / SAMPLE_RATE / 64) as u16;
        regs.div
            .modify(|_, w| unsafe { w.div().bits(bclk_div - 1) }); // Clock source is MCLK (12.288MHz) / 4 = 3MHz

        // Config
        regs.cfg1.modify(|_, w| unsafe {
            w.mstslvcfg()
                .normal_master()
                .onechannel()
                .dual_channel()
                .datalen()
                .bits(31)
                .mainenable()
                .disabled()
                .mode()
                .classic_mode()
                .datapause()
                .normal()
        });

        unsafe { pac::NVIC::unmask(pac::Interrupt::FLEXCOMM7) };
        self.dac.init();
    }
    ///Transition -> Stopped:
    ///clear queue, mute DAC, mask I2S ISR, stop I2S peripheral, disable & reset feedback and performance queues
    fn stop(&mut self) {
        // Disable FIFO error interrupt
        self.i2s.i2s.fifointenclr.write(|w| w.txerr().set_bit());
        dma_ring().stop();
        pac::NVIC::mask(pac::Interrupt::DMA0);
        self.dac.mute();
        // Clear any samples in the FIFO
        self.i2s.i2s.fifocfg.modify(|_, w| w.emptytx().set_bit());
        // Disable I2S
        self.i2s.i2s.cfg1.modify(|_, w| w.mainenable().disabled());
        // Reset feedback state
        self.fb.reset();
        // reset performance counters
        PERF.reset();
        // Stop the clocks
        self.clock_pins.sel_22m.set_low().ok();
        self.clock_pins.sel_24m.set_low().ok();
    }
    ///Transition -> Armed
    /// Start I2S peripheral and MCLK. Since we assume we have interrupts disabled at
    /// this point (as we came from Stopped), and the FIFO is empty, this will
    /// play out 0s.
    fn arm(&mut self) {
        dma_ring().init();
        self.set_sample_rate(self.cur_rate).ok();
        self.i2s.i2s.cfg1.modify(|_, w| w.mainenable().enabled());
    }
    ///Transition -> Prefill
    /// Unmute DAC
    fn prefill(&mut self) {
        self.dac.unmute();
    }
    ///Transition -> Running
    ///Unmask I2S ISR, start feedback
    fn run(&mut self) {
        self.fb.start();
        // FIFO threshold trigger enable
        self.i2s
            .i2s
            .fifotrig
            .modify(|_, w| unsafe { w.txlvl().bits(6).txlvlena().enabled() });
        self.i2s
            .i2s
            .fifocfg
            .modify(|_, w| w.enabletx().enabled().dmatx().enabled());
        dma_ring().run();
        // clear tx error status
        self.i2s.i2s.fifostat.write(|w| w.txerr().set_bit());
        // enable tx error interrupt
        self.i2s.i2s.fifointenset.write(|w| w.txerr().enabled());
        unsafe {
            pac::NVIC::unmask(pac::Interrupt::DMA0);
        }
    }
    ///Transition->NoData
    ///store framecount at transition so we can time out recovery
    fn nodata(&mut self) {
        self.nodata_timeout_frame.store(
            PERF.queue_underflows.load(Ordering::Relaxed) + NODATA_TIMEOUT_FRAMES, // we underflow every frame, use it as a timeout counter
            Ordering::Relaxed,
        );
    }
    /// Transition -> Stopping
    /// just a marker that upcoming nodata is expected, do nothing
    fn stopping(&mut self) {}
}
impl<D: Dac<I>, I> ClockSource for Audio<'_, D, I> {
    const CLOCK_TYPE: usbd_uac2::descriptors::ClockType = ClockType::InternalFixed;
    const SOF_SYNC: bool = false;

    fn sample_rate(&self) -> u32 {
        self.cur_rate
    }
    fn set_sample_rate(
        &mut self,
        sample_rate: u32,
    ) -> core::result::Result<(), usbd_uac2::UsbAudioClassError> {
        if 24_576_000u32.is_multiple_of(sample_rate) {
            defmt::info!("[clock] 24M clock selected");
            self.clock_pins.sel_22m.set_low().ok();
            // hal::wait_at_least(1);
            self.clock_pins.sel_24m.set_high().ok();
        } else {
            defmt::info!("[clock] 22M clock selected");
            self.clock_pins.sel_24m.set_low().ok();
            // hal::wait_at_least(1);
            self.clock_pins.sel_22m.set_high().ok();
        };
        self.dac.change_rate(sample_rate);
        self.cur_rate = sample_rate;
        Ok(())
    }
    fn sample_rates(
        &self,
    ) -> core::result::Result<&[usbd_uac2::RangeEntry<u32>], usbd_uac2::UsbAudioClassError> {
        Ok(&Self::RATES)
    }
    fn clock_validity(&self) -> Result<bool, UsbAudioClassError> {
        Ok(true)
    }
}
impl<D: Dac<I>, I, B: bus::UsbBus> AudioHandler<'_, B> for Audio<'_, D, I> {
    fn alternate_setting_changed(&mut self, _terminal: usb_device::UsbDirection, alt_setting: u8) {
        let state = self.state.load(Ordering::Relaxed);
        match (alt_setting, state) {
            (0, AudioState::Armed | AudioState::Prefill | AudioState::NoData) => {
                self.transition(AudioState::Stopped)
            }
            (0, AudioState::Running) => self.transition(AudioState::Stopping),
            (0, AudioState::Stopped | AudioState::Stopping) => {} // already stopped/ing
            (1, AudioState::Stopped) => self.transition(AudioState::Armed),
            (1, _) => {} // altSetting 1 in any other state is a no-op
            (_, _) => {
                defmt::error!("Invalid alt setting {}", alt_setting)
            }
        }
        self.alt_setting = alt_setting;
    }
    fn audio_data_rx(
        &mut self,
        ep: &usb_device::endpoint::Endpoint<'_, B, usb_device::endpoint::Out>,
    ) {
        let state = self.state.load(Ordering::Relaxed);
        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.
            defmt::error!(
                "overflowed dma ring, asked {}, wrote {}, dropped {}",
                buf.len(),
                res.written,
                res.dropped
            );
            PERF.queue_overflows
                .fetch_add(res.dropped / BYTES_PER_FRAME, Ordering::Relaxed);
        }

        PERF.received_frames
            .fetch_add(res.written / BYTES_PER_FRAME, Ordering::Relaxed);

        // Valid states here are Armed, Prefill, Running, Draining and NoData
        match state {
            AudioState::Stopped | AudioState::Stopping => {
                defmt::error!("Received audio data when stopped/stopping")
            }
            // When armed, data rx goes to prefill
            AudioState::Armed => self.transition(AudioState::Prefill),
            // When prefilling, if we have received frames over the up threshold, move to running
            AudioState::Prefill => {
                if PERF.received_frames.load(Ordering::Relaxed) >= QUEUE_RUNNING_UP {
                    self.transition(AudioState::Running);
                }
            }
            // When running, USB RX is a no-op
            AudioState::Running => {}
            // If draining, check cur_fill, if it rises above QUEUE_RUNNING_UP, move back to running. If it drops to 0, move to NoData or Stopped
            AudioState::LowData => {
                let fill = cur_fill() as usize;
                // Do we check alt setting here? We shouldn't be receiving data at all if we are not in altSetting 1
                if fill >= QUEUE_RUNNING_UP {
                    self.transition(AudioState::Running);
                } else if fill == 0 && self.alt_setting == 0 {
                    self.transition(AudioState::Stopped);
                } else if fill == 0 {
                    self.transition(AudioState::NoData);
                }
            }
            // Any data in NoData moves us into LowData. But maybe it should be more like prefill?
            AudioState::NoData => self.transition(AudioState::LowData),
        }
    }
    fn audio_data_tx(
        &mut self,
        _ep: &usb_device::endpoint::Endpoint<'_, B, usb_device::endpoint::In>,
    ) {
    }
    fn feedback(&mut self, nominal_rate: UsbIsochronousFeedback) -> Option<UsbIsochronousFeedback> {
        if !self.fb.correction_enabled.load(Ordering::Relaxed) {
            return Some(nominal_rate);
        }

        let current_bytes = cur_fill() as i32;

        if current_bytes == 0 {
            defmt::error!("[fb] dma underrun detected!");
            PERF.queue_underflows.fetch_add(1, Ordering::Relaxed);
            return Some(nominal_rate);
        }

        PERF.avg_fill
            .fetch_update(Ordering::SeqCst, Ordering::SeqCst, |v| {
                Some(((v << 6) - v + current_bytes as usize) >> 6)
            })
            .ok();

        let raw_error = current_bytes - FILL_TARGET_BYTES;
        let i_error = if raw_error.abs() <= 4 { 0 } else { raw_error }; // deadband
        let current_i = self.fb.integrator.load(Ordering::Relaxed);
        let leak = current_i >> 7;
        let new_i = current_i
            .saturating_sub(leak)
            .saturating_add(i_error)
            .clamp(-5000, 5000);
        self.fb.integrator.store(new_i, Ordering::Relaxed);
        PERF.integrator.store(new_i, Ordering::Relaxed);

        let nominal_v = nominal_rate.to_u32_12_13() as i32;
        let max_allowed_deviation = nominal_v / 500; // 0.2%

        // 3. SEPARATE GAINS FOR P AND I
        // For P: Keep your working math (converting raw error to a permille equivalent scale)
        let error_permille = (raw_error * 1000) / FILL_TARGET_BYTES;
        let p_term = (-((error_permille as i64) * (nominal_v as i64)) / (10 * 256000)) as i32;
        let i_term = (-((new_i as i64) * (nominal_v as i64)) / (256000 * 1000)) as i32;
        let i_term = 0;

        PERF.p.store(p_term, Ordering::Relaxed);
        PERF.i.store(i_term, Ordering::Relaxed);

        let mut v = nominal_v + p_term + i_term;
        v = v.clamp(
            nominal_v - max_allowed_deviation,
            nominal_v + max_allowed_deviation,
        );
        PERF.fb.store(v, Ordering::Relaxed);

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

pub struct I2sTx {
    pub i2s: pac::I2S7,
}

pub fn init_i2s(mut fc7: pac::FLEXCOMM7, i2s7: pac::I2S7, syscon: &mut Syscon) -> I2sTx {
    defmt::debug!("init i2s");
    // Enable BOTH
    syscon.reset(&mut fc7);
    syscon.enable_clock(&mut fc7);

    unsafe {
        pac::IOCON::ptr().as_ref().unwrap().pio0_23.modify(|_, w| {
            w.func()
                .alt1() // MCLK
                .mode()
                .inactive()
                .slew()
                .fast()
                .invert()
                .disabled()
                .digimode()
                .digital()
                .od()
                .normal()
        });
        pac::SYSCON::ptr()
            .as_ref()
            .unwrap()
            .fcclksel7()
            .modify(|_, w| w.sel().enum_0x5()); // MCLK
    };

    #[cfg(not(feature = "evk"))]
    unsafe {
        pac::SYSCON::ptr()
            .as_ref()
            .unwrap()
            .mclkio
            .modify(|_, w| w.mclkio().input());
    }
    #[cfg(feature = "evk")]
    unsafe {
        pac::SYSCON::ptr()
            .as_ref()
            .unwrap()
            .mclkclksel
            .modify(|_, w| w.sel().enum_0x1()); // PLL0
        pac::SYSCON::ptr()
            .as_ref()
            .unwrap()
            .mclkdiv
            .modify(|_, w| w.div().bits(1).halt().run().reset().released()); // div by 2 = PLL0 fout / 2 = 12.288MHz, max for WM8904 @ 96k
        pac::SYSCON::ptr()
            .as_ref()
            .unwrap()
            .mclkio
            .modify(|_, w| w.mclkio().output());
    }

    // Select I2S TX function
    fc7.pselid.write(|w| w.persel().i2s_transmit());

    let regs = i2s7;
    I2sTx { i2s: regs }
}

#[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");

    debug!("iocon");
    let usb0_vbus_pin = pins::Pio0_22::take()
        .unwrap()
        .into_usb0_vbus_pin(&mut iocon);

    #[cfg(not(feature = "evk"))]
    let codec_i2c_pins = (
        pins::Pio0_16::take().unwrap().into_i2c4_scl_pin(&mut iocon),
        pins::Pio0_5::take().unwrap().into_i2c4_sda_pin(&mut iocon),
    );
    #[cfg(feature = "evk")]
    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),
    );
    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::Pio0_23::take().unwrap(),
    );
    let codec_gpio_pins = CodecPins {
        reset: pins::Pio0_3::take()
            .unwrap()
            .into_gpio_pin(&mut iocon, &mut gpio)
            .into_output_low(),
    };
    let clock_sel_pins = ClockSelPins {
        sel_24m: pins::Pio0_27::take()
            .unwrap()
            .into_gpio_pin(&mut iocon, &mut gpio)
            .into_output_low(),
        sel_22m: pins::Pio0_31::take()
            .unwrap()
            .into_gpio_pin(&mut iocon, &mut gpio)
            .into_output_low(),
    };

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

    // iocon.disabled(&mut syscon).release(); // save the environment :)

    debug!("clocks");
    let clocks = hal::ClockRequirements::default()
        .system_frequency(96.MHz())
        .configure(&mut anactrl, &mut pmc, &mut syscon)
        .unwrap();
    hw::init_sys_pll1();
    #[cfg(feature = "evk")]
    hw::init_audio_pll();

    let mut delay_timer = Timer::new(
        hal.ctimer
            .0
            .enabled(&mut syscon, clocks.support_1mhz_fro_token().unwrap()),
    );

    debug!("peripherals");

    let i2c_peripheral = hal
        .flexcomm
        .4
        .enabled_as_i2c(&mut syscon, &clocks.support_flexcomm_token().unwrap());

    let i2c_bus = I2cMaster::new(
        i2c_peripheral,
        codec_i2c_pins,
        Hertz::try_from(400.kHz()).unwrap(),
    );
    let dac_impl = DacImpl::new(i2c_bus, codec_gpio_pins);

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

    let usb_peripheral = hal.usbhs.enabled_as_device(
        &mut anactrl,
        &mut pmc,
        &mut syscon,
        &mut delay_timer,
        clocks.support_usbhs_token().unwrap(),
    );

    defmt::info!("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) };

    defmt::info!("audio init");
    let mut audio = Audio {
        state: Atomic::new(AudioState::Stopped),
        i2s: i2s_peripheral,
        dac: dac_impl,
        dma: dma_ring(),
        fb: FeedbackState::default(),
        alt_setting: 0,
        nodata_timeout_frame: AtomicUsize::new(0),
        cur_rate: SAMPLE_RATE,
        clock_pins: clock_sel_pins,
        _marker: core::marker::PhantomData,
    };
    audio.init();

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

    let config = UsbAudioClassConfig::new(UsbSpeed::High, FunctionCode::Other, &mut audio)
        .with_output_config(
            TerminalConfig::builder()
                .base_id(2)
                .terminal_type(TerminalType::UsbUndefined)
                .build(),
        );
    let mut uac2 = config.build(&usb_bus).unwrap();
    #[cfg(feature = "hid")]
    let mut hid = HIDClass::new_ep_in(&usb_bus, AudioTelemetryReport::desc(), HID_INTERVAL_MS);

    let mut usb_dev = usbd_uac2::builder(&usb_bus, UsbVidPid(0x1209, 0xcc1d))
        .strings(&[StringDescriptors::default()
            .manufacturer("VE7XEN")
            .product("Guac Tortilla")])
        .unwrap()
        .max_packet_size_0(64)
        .unwrap()
        .build();

    #[cfg(feature = "hid")]
    let mut poll_all = {
        let mut hid_update_timer = Timer::new(
            hal.ctimer
                .1
                .enabled(&mut syscon, clocks.support_1mhz_fro_token().unwrap()),
        );
        hid_update_timer.start(Microseconds::new(HID_INTERVAL_MS as u32 * 1000));

        move || {
            usb_dev.poll(&mut [&mut uac2, &mut hid]);
            // DMA ring is empty; if altsetting = 0 then -> stopped else NoData
            // TODO: handle NoData state
            if NODATA_FLAG.swap(false, Ordering::Acquire) {
                match uac2.handler().state.load(Ordering::Acquire) {
                    AudioState::Stopping => uac2.handler().transition(AudioState::Stopped),
                    _ => uac2.handler().transition(AudioState::Stopped),
                }
            }
            if hid_update_timer.wait().is_ok() {
                let report = PERF.build_report();
                match hid.push_input(&report) {
                    Ok(_) => {}
                    Err(UsbError::WouldBlock) => {}
                    Err(e) => defmt::error!("Failed to send HID report: {:?}", e),
                }
                // lpc55 ctimer is not Periodic, so restart it
                hid_update_timer.start(Microseconds::new(HID_INTERVAL_MS as u32 * 1000));
            }
        }
    };
    #[cfg(not(feature = "hid"))]
    let poll_all = || {
        usb_dev.poll(&mut [&mut uac2]);
    };

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

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