use cortex_m::singleton;
use rp2040_hal::dma::{bidirectional, Channel};
use fugit::HertzU32;
use fugit::RateExtU32;

pub struct AdcContext<D, TXC, RXC, CSP>
where
    TXC: rp2040_hal::dma::ChannelIndex,
    RXC: rp2040_hal::dma::ChannelIndex,
    D: rp2040_hal::spi::SpiDevice
{
    dma_tx_chan: Channel<TXC>,
    dma_rx_chan: Channel<RXC>,
    tx_buf:      &'static mut [u8; 3],
    rx_buf:      &'static mut [u8; 3],
    spi:         rp2040_hal::spi::Spi<rp2040_hal::spi::Enabled, D, 8>,
    cs_pin:      CSP
}

impl<D, TXC, RXC, E, CSP> AdcContext<D, TXC, RXC, CSP>
where
    TXC: rp2040_hal::dma::ChannelIndex,
    RXC: rp2040_hal::dma::ChannelIndex,
    D: rp2040_hal::spi::SpiDevice,
    E: core::fmt::Debug,
    CSP: embedded_hal::digital::v2::OutputPin<Error=E>
{
    pub fn init(
        device: D,
        periph_freq: HertzU32,
        resets: &mut rp2040_pac::RESETS,
        tx_chan: Channel<TXC>,
        rx_chan: Channel<RXC>,
        mut cs_pin: CSP
        ) -> Self
    {
        let spi_if = rp2040_hal::spi::Spi::<_, _, 8>::new(device);

        // Exchange the uninitialised SPI driver for an initialised one
        let spi_if = spi_if.init(
            resets,
            periph_freq,
            1_000_000u32.Hz(),
            &embedded_hal::spi::MODE_0,
        );

        cs_pin.set_high().unwrap();

        AdcContext::<D, TXC, RXC, CSP> {
            dma_rx_chan: rx_chan,
            dma_tx_chan: tx_chan,
            tx_buf:      singleton!(: [u8; 3] = [0x06, 0x80, 0x00]).unwrap(),
            rx_buf:      singleton!(: [u8; 3] = [0; 3]).unwrap(),
            spi:         spi_if,
            cs_pin:      cs_pin
        }
    }

    pub fn sample_adc_channel(mut self, chan: u8) -> (Self, u16)
    {
        self.tx_buf[1] = chan << 6;

        // clear chip select
        self.cs_pin.set_low().unwrap();

        // Use BidirectionalConfig to simultaneously write to spi from tx_buf and read into rx_buf
        let transfer = bidirectional::Config::new(
            (self.dma_tx_chan, self.dma_rx_chan),
            self.tx_buf,
            self.spi,
            self.rx_buf).start();

        // Wait for both DMA channels to finish
        ((self.dma_tx_chan, self.dma_rx_chan), self.tx_buf, self.spi, self.rx_buf) = transfer.wait();
        self.cs_pin.set_high().unwrap();

        let result = (((self.rx_buf[1] & 0x0F) as u16) << 8) | self.rx_buf[2] as u16;
        (self, result)
    }
}
