use defmt::*;

use embassy_stm32::{gpio::Output, mode::Blocking, spi::Spi, spi::Error};
use embassy_time::Timer;

use heapless::LinearMap;

use crate::ad5940_registers::*;

#[allow(dead_code)]
#[derive(Default)]
pub struct ClkConfig {
    adcclkdiv: Option<ADCCLKDIV>,
    sysclkdiv: Option<SYSCLKDIV>,
    clk32mhzen: Option<CLK32MHZEN>,
}

impl ClkConfig {
    pub fn adcclkdiv(mut self, adcclkdiv: ADCCLKDIV) -> Self {
        self.adcclkdiv = Some(adcclkdiv);
        self
    }

    pub fn sysclkdiv(mut self, sysclkdiv: SYSCLKDIV) -> Self {
        self.sysclkdiv = Some(sysclkdiv);
        self
    }
    pub fn clk32mhzen(mut self, clk32mhzen: CLK32MHZEN) -> Self {
        self.clk32mhzen = Some(clk32mhzen);
        self
    }
}

#[allow(dead_code)]
#[derive(Default)]
pub struct SwitchConfig {
    t9con: Option<T9CON>,
    tmuxcon: Option<TMUXCON>,
    nmuxcon: Option<NMUXCON>,
    pmuxcon: Option<PMUXCON>,
    dmuxcon: Option<DMUXCON>,
}

impl SwitchConfig {
    pub fn t9con(mut self, t9con: T9CON) -> Self {
        self.t9con = Some(t9con);
        self
    }

    pub fn tmuxcon(mut self, tmuxcon: TMUXCON) -> Self {
        self.tmuxcon = Some(tmuxcon);
        self
    }

    pub fn nmuxcon(mut self, nmuxcon: NMUXCON) -> Self {
        self.nmuxcon = Some(nmuxcon);
        self
    }

    pub fn pmuxcon(mut self, pmuxcon: PMUXCON) -> Self {
        self.pmuxcon = Some(pmuxcon);
        self
    }

    pub fn dmuxcon(mut self, dmuxcon: DMUXCON) -> Self {
        self.dmuxcon = Some(dmuxcon);
        self
    }
}

#[allow(dead_code)]
#[derive(Default)]
pub struct LpConfig {
    data_reset: Option<bool>,
    power_enable: Option<bool>,
    data_6bit: Option<u8>,
    common_mode_switch: Option<bool>,
    tia_switches_enabled: Option<u16>,
    tia_enable: Option<bool>,
    filter_resistor: Option<TIARF>,
}

impl LpConfig {
    pub fn data_reset(&mut self, rsten: bool) -> &mut Self {
        self.data_reset = Some(rsten);
        self
    }

    pub fn power_enable(&mut self, pwden: bool) -> &mut Self {
        self.power_enable = Some(pwden);
        self
    }
    
    pub fn data_6bit(&mut self, data: u8) -> &mut Self {
        self.data_6bit = Some(data & 0x3F);
        self
    }

    pub fn common_mode_enable(&mut self, enable: bool) -> &mut Self {
        self.common_mode_switch = Some(enable);
        self
    }

    pub fn tia_switches_enabled(&mut self, enable: u16) -> &mut Self {
        self.tia_switches_enabled = Some(enable);
        self
    }

    pub fn tia_enable(&mut self, enable: bool) -> &mut Self {
        self.tia_enable = Some(enable);
        self
    }

    pub fn filter_resistor(&mut self, tiarf: TIARF) -> &mut Self {
        self.filter_resistor = Some(tiarf);
        self
    }
}

#[allow(dead_code)]
#[derive(Default)]
pub struct DspConfig {
    pub gnpgain: Option<GNPGAIN>,
    muxseln: Option<MUXSELN>,
    muxselp: Option<MUXSELP>,
    ctiacon: Option<CTIACON>,
    rtiacon: Option<RTIACON>,
    sinc3osr: Option<SINC3OSR>,
    sinc2osr: Option<SINC2OSR>,
    adcsamplerate: Option<ADCSAMPLERATE>,
    dftin: Option<DFTINSEL>,
    dftnum: Option<DFTNUM>,
    hanning: Option<bool>,
    pub fsys: Option<u32>,
    pub fadc: Option<u32>,
    ratio_sys2adc_clk: Option<f32>,
}

impl DspConfig {
    pub fn gnpgain(&mut self, gnpgain: GNPGAIN) -> &mut Self {
        self.gnpgain = Some(gnpgain);
        self
    }

    pub fn adc_mux_n(&mut self, muxseln: MUXSELN) -> &mut Self {
        self.muxseln = Some(muxseln);
        self
    }

    pub fn adc_mux_p(&mut self, muxselp: MUXSELP) -> &mut Self {
        self.muxselp = Some(muxselp);
        self
    }

    pub fn ctiacon(&mut self, ctiacon: CTIACON) -> &mut Self {
        self.ctiacon = Some(ctiacon);
        self
    }

    pub fn rtiacon(&mut self, rtiacon: RTIACON) -> &mut Self {
        self.rtiacon = Some(rtiacon);
        self
    }

    pub fn sinc3osr(&mut self, sinc3osr: SINC3OSR) -> &mut Self {
        self.sinc3osr = Some(sinc3osr);
        self
    }

    pub fn sinc2osr(&mut self, sinc2osr: SINC2OSR) -> &mut Self {
        self.sinc2osr = Some(sinc2osr);
        self
    }

    pub fn adcsamplerate(&mut self, adcsamplerate: ADCSAMPLERATE) -> &mut Self {
        self.adcsamplerate = Some(adcsamplerate);
        self
    }

    pub fn dftin_sel(&mut self, dftin: DFTINSEL) -> &mut Self {
        self.dftin = Some(dftin);
        self
    }

    pub fn dftnum(&mut self, dftnum: DFTNUM) -> &mut Self {
        self.dftnum = Some(dftnum);
        self
    }

    pub fn hanning(&mut self, hanning: bool) -> &mut Self {
        self.hanning = Some(hanning);
        self
    }

    pub fn set_clks(&mut self, fsys: u32, fadc: u32) -> &mut Self {
        self.fsys = Some(fsys);
        self.fadc = Some(fadc);
        self.ratio_sys2adc_clk = Some(fsys as f32 / fadc as f32);
        self
    }
}

#[allow(dead_code)]
#[derive(Default)]
pub struct SramConfig {
    datafifosrcsel: Option<DATAFIFOSRCSEL>,
    datafifoen: Option<DATAFIFOEN>,
    data_mem_size: Option<DATA_MEM_SEL>,
    cmd_mem_mode: Option<CMDMEMMDE>,
    cmd_mem_size: Option<CMD_MEM_SEL>,
}

impl SramConfig {
    pub fn datafifosrcsel(&mut self, datafifosrcsel: DATAFIFOSRCSEL) -> &mut Self {
        self.datafifosrcsel = Some(datafifosrcsel);
        self
    }

    pub fn datafifoen(&mut self, datafifoen: DATAFIFOEN) -> &mut Self {
        self.datafifoen = Some(datafifoen);
        self
    }

    pub fn data_size(&mut self, data_mem_size: DATA_MEM_SEL) -> &mut Self {
        self.data_mem_size = Some(data_mem_size);
        self
    }

    pub fn cmd_mode(&mut self, cmd_mem_mode: CMDMEMMDE) -> &mut Self {
        self.cmd_mem_mode = Some(cmd_mem_mode);
        self
    }

    pub fn cmd_size(&mut self, cmd_mem_size: CMD_MEM_SEL) -> &mut Self {
        self.cmd_mem_size = Some(cmd_mem_size);
        self
    }
}

pub struct Sequencer {

}

impl Sequencer {
    pub fn new() -> Self {
        Sequencer {}
    }

    pub async fn configure(&mut self, ad5940: &mut AD5940) -> Result<(), Error> {
        // Configure the sequencer here if needed
        // This is a placeholder for future sequencer configuration logic
        Ok(())
    }
}

pub struct AD5940 {
    spi: Spi<'static, Blocking>,
    cs: Output<'static>,
    rst: Output<'static>,
    seq_enabled: bool,
    pub seq_len: usize,
    pub seq_buffer: [u32; 500],
    pub seq_gen_db: LinearMap<Register, u32, 16>,
}

impl AD5940 {
    pub fn new(spi: Spi<'static, Blocking>, cs: Output<'static>, rst: Output<'static>) -> Self {
        AD5940 { 
            spi,
            cs,
            rst,
            seq_enabled: false,
            seq_len: 0,
            seq_buffer: [0; 500],
            seq_gen_db: LinearMap::new(),
        }
    }

    pub async fn reset(&mut self) -> Result<(), Error> {
        self.rst.set_low();
        Timer::after_millis(1).await;
        self.rst.set_high();
        Ok(())
    }

    pub async fn read_reg(&mut self, address: Register) -> Result<u32, Error> {
        if self.seq_enabled {
            self.sequencer_read_register(address).await
        } else {
            self.read_reg_raw(address as u16).await
        }
    }

    pub async fn read_reg_raw(&mut self, address: u16) -> Result<u32, Error> {
        // Write address command
        self.cs.set_low();
        self.spi.blocking_write(&[Command::SPICMD_SETADDR as u8])?;
        self.spi.blocking_write(&[address as u16])?;
        self.cs.set_high();

        // Wait after cs is high
        Timer::after_nanos(80).await;

        // Read command
        self.cs.set_low();
        self.spi.blocking_write(&[Command::SPICMD_READREG as u8])?;
        self.spi.blocking_write(&[0 as u8])?; // Dummy byte
        let result: u32;
        if (address as u32) >= 0x1000 && (address as u32) <= 0x3014 {
            let mut data = [0u8; 4];
            self.spi.blocking_read(&mut data)?;
            result = u32::from_be_bytes(data);
        } else {            
            let mut data = [0u8; 2];
            self.spi.blocking_read(&mut data)?;
            result = u16::from_be_bytes(data) as u32;
        }

        // let mut data = [0u8; 5]; // First byte dummy, then four data bytes
        // self.spi.blocking_read(&mut data)?;
        self.cs.set_high();

        Ok(result)
    }

    pub async fn write_reg(&mut self, address: Register, value: u32) -> Result<(), Error> {
        if self.seq_enabled {
            self.sequencer_write_register(address, value).await
        } else {
            self.write_reg_raw(address as u16, value).await
        }
    }

    pub async fn write_reg_raw(&mut self, address: u16, value: u32) -> Result<(), Error> {
        // Write address command
        self.cs.set_low();
        self.spi.blocking_write(&[Command::SPICMD_SETADDR as u8])?;
        self.spi.blocking_write(&[address as u16])?;
        self.cs.set_high();

        // Wait after cs is high
        Timer::after_nanos(80).await;

        // Write value command
        self.cs.set_low();
        self.spi.blocking_write(&[Command::SPICMD_WRITEREG as u8])?;
        if (address>=0x1000) && (address<=0x3014) {
            self.spi.blocking_write(&[value])?;
        } else {
            self.spi.blocking_write(&[value as u16])?;
        }
        self.cs.set_high();
        
            
        Ok(())
    }

    pub async fn read_fifo(&mut self, buf: &mut [u32]) -> Result<(), Error> {
        if buf.len() < 3 {
            self.cs.set_low();
            self.spi.blocking_write(&[Command::SPICMD_SETADDR as u8])?;
            self.spi.blocking_write(&[Register::DATAFIFORD as u16])?;
            self.cs.set_high();

            // Wait after cs is high
            Timer::after_nanos(80).await;

            for i in 0..buf.len() {
                self.cs.set_low();
                self.spi.blocking_write(&[Command::SPICMD_READREG as u8])?;//Write Host status/Don't care
                self.spi.blocking_write(&[0 as u8])?;//Write Host status/Don't care
                let mut byte = [0u32];
                self.spi.blocking_read(&mut byte)?;
                buf[i] = byte[0];
                self.cs.set_high();

                // Wait after cs is high
                Timer::after_nanos(80).await;
            }
        } else  {
            self.cs.set_low();
            self.spi.blocking_write(&[Command::SPICMD_READFIFO as u8])?;
            
            // 6 dummy writes
            for _ in 0..6 {
                self.spi.blocking_write(&[0u8])?;
            }

            // Read all but last 2 elements
            for i in 0..buf.len()-2 {
                let mut byte = [0u32];
                self.spi.blocking_read(&mut byte)?;
                buf[i] = byte[0];                
            }

            // Read last 2 elements with transfer
            let mut bytes = [0u32; 2];
            self.spi.blocking_transfer(&mut bytes, &mut [0x4444_4444, 0x4444_4444])?;
            buf[buf.len()-2] = bytes[0];
            buf[buf.len()-1] = bytes[1];
            self.cs.set_high();
        }
        Ok(())
    }

    pub async fn system_init(&mut self) -> Result<(), Error> {
        // See table 14
        self.write_reg_raw(0x0908, 0x02C9).await?;
        self.write_reg_raw(0x0C08, 0x206C).await?;
        self.write_reg_raw(0x21F0, 0x0010).await?; // REPEATADCCNV
        self.write_reg_raw(0x0410, 0x02C9).await?; // CMDDATACON
        self.write_reg_raw(0x0A28, 0x0009).await?; // EI2CON
        self.write_reg_raw(0x238C, 0x0104).await?;
        self.write_reg_raw(0x0A04, 0x4859).await?; // PWRKEY
        self.write_reg_raw(0x0A04, 0xF27B).await?; // PWRKEY
        self.write_reg_raw(0x0A00, 0x8009).await?; // PWRMOD
        self.write_reg_raw(0x22F0, 0x0000).await?; // PMBW

        // According to library
        self.write_reg_raw(0x2230, 0xDE87A5AF).await?;
        self.write_reg_raw(0x2250, 0x103F).await?;
        self.write_reg_raw(0x2230, 0xDE87A5A0).await?;
        self.write_reg_raw(0x22B0, 0x203C).await?;
        Ok(())
    }

    pub async fn sequencer_enable(&mut self, enabled: bool) {
        self.seq_enabled = enabled;
        if enabled {
            self.seq_len = 0;
        }
    }

    async fn sequencer_read_register(&mut self, address: Register) -> Result<u32, Error> {
        if address as u32 > 0x21FF {
            error!("Sequencer read address out of range: 0x{:04X}", address as u32);
        }

        // Read from sequencer database, if not present read default (often default) from hardware
        if let Some(value) = self.seq_gen_db.get(&address) {
            Ok(*value)
        } else {
            self.read_reg_raw(address as u16).await
        }
    }

    async fn sequencer_write_register(&mut self, address: Register, value: u32) -> Result<(), Error> {
        if address as u32 > 0x21FF {
            error!("Sequencer write address out of range: 0x{:04X}", address as u32);
        }

        // Update or put in sequencer database
        if self.seq_gen_db.contains_key(&address) {
            *self.seq_gen_db.get_mut(&address).unwrap() = value;
        } else {
            self.seq_gen_db.insert(address, value).unwrap();
        }

        // Place into buffer
        let cmd = 0b1 << 31 | (((address as u32) >> 2) & 0x7F) << 24 | (value & 0xFF_FFFF);
        self.sequencer_insert(cmd).await;

        return Ok(())
    }

    pub async fn sequencer_wait(&mut self, cycles: u32) {
        if cycles > 0x3FFF_FFFF {
            error!("Sequencer wait cycles out of range: {}", cycles);
            return;
        }
        let cmd = 0b00 << 30 | 0x0000_0000 | cycles & 0x3FFF_FFFF;
        self.sequencer_insert(cmd).await;
    }

    pub async fn sequencer_trigger_interrupt(&mut self, int: AFEGENINTSTA) {
        let cmd = 0x10 | int.bits();
        self.sequencer_write_register(Register::AFEGENINTSTA, cmd).await.unwrap();
    }

    async fn sequencer_insert(&mut self, cmd: u32) {
        if self.seq_len >= self.seq_buffer.len() {
            error!("Sequencer buffer full, cannot insert command: 0x{:08X}", cmd);
            return;
        }

        self.seq_buffer[self.seq_len] = cmd;
        self.seq_len += 1;
    }

    pub async fn sequencer_cmd_write(&mut self, mut start_address: u32) {
        // let start_address = 3000 >> 2;
        let mut counter = 0;

        start_address &= 0x7FF; // Ensure start address is within bounds

        while counter < self.seq_len {
            self.write_reg(Register::CMDFIFOWADDR, start_address + counter as u32).await.unwrap();
            self.write_reg(Register::CMDFIFOWRITE, self.seq_buffer[counter as usize]).await.unwrap();
            counter += 1;
        }
    }

    pub async fn sequencer_info_configure(&mut self, seq: u8, length: usize, start_address: u32) {
        let cmd = (length as u32 & 0x7FF) << 16 | (start_address & 0x7FF);

        match seq {
            0 => self.write_reg(Register::SEQ0INFO, cmd).await.unwrap(),
            1 => self.write_reg(Register::SEQ1INFO, cmd).await.unwrap(),
            2 => self.write_reg(Register::SEQ2INFO, cmd).await.unwrap(),
            3 => self.write_reg(Register::SEQ3INFO, cmd).await.unwrap(),
            _ => {
                error!("Sequencers from 0 till 3 are allows,  now: seq={}", seq);
            },
        }
    }

    pub async fn sequencer_trigger(&mut self, seq: u8) {
        // Trigger the sequencer
        match seq {
            0 => self.write_reg(Register::TRIGSEQ, 1 << 0).await.unwrap(),
            1 => self.write_reg(Register::TRIGSEQ, 1 << 1).await.unwrap(),
            2 => self.write_reg(Register::TRIGSEQ, 1 << 2).await.unwrap(),
            3 => self.write_reg(Register::TRIGSEQ, 1 << 3).await.unwrap(),
            _ => {
                error!("Sequencers from 0 till 3 are allows, now: seq={}", seq);
            },
        }
    }

    pub async fn sequencer_calculate_wait_time(&mut self, config: &DspConfig) -> Option<u32> {
        let mut wait_time = 1;

        let sinc3_table = [5, 4, 2];
        let sinc2_table = [22, 44, 89, 178, 267, 533, 640, 667, 800, 889, 1067, 1333];

        match config.dftin {
            Some(DFTINSEL::Sinc2) => {
                if let Some(sinc3osr) = config.sinc3osr {
                    wait_time *= sinc3_table[sinc3osr as usize];
                } else {
                    return None; // Sinc2 requires sinc3osr to be set
                };

                if let Some(sinc2osr) = config.sinc2osr {
                    wait_time *= sinc2_table[sinc2osr as usize];
                } else {
                    return None; // Sinc2 requires sinc2osr to be set
                };
            }
            Some(DFTINSEL::GainOffset) => {
                if let Some(sinc3osr) = config.sinc3osr {
                    wait_time *= sinc3_table[sinc3osr as usize];
                } else {
                    return None; // Sinc2 requires sinc3osr to be set
                };
            }
            Some(DFTINSEL::AdcRaw) => {
            }
            None => {
                return None;
            }
        }

        // Calculate wait time based on DFTNUM
        if let Some(dftnum) = config.dftnum {
            let samples_per_dft = 1 << (dftnum as u32 + 2);
            wait_time *= samples_per_dft;
        } else {
            return None; // DFTNUM must be set
        }

        // When ACLK = 16MHz, ADC samplerate is 800kHz
        // When ACLK = 32MHz, ADC samplerate is 1.6MHz
        // If ACLK == SYSCLK --> Always per ADC sample 20 cycles
        // If ACLK != SYSCLK --> Always per ADC sample 20*(SYSCLK/ACLK) cycles
        // When SYSCLK is lower, the wait cycles to wait are less becasuse the ADC samples at higher rate
        wait_time *= 20;

        if let Some(ratio) = config.ratio_sys2adc_clk {
            wait_time = (wait_time as f32 * ratio) as u32;
        } else {
            return None; // Ratio must be set
        }

        Some(wait_time)
    }

    pub async fn apply_switch_config(&mut self, config: SwitchConfig) -> Result<(), Error> {
        // SWCON
        let mut current = self.read_reg(Register::SWCON).await?;

        if let Some(t9con) = config.t9con {
            current = T9CON::apply(current, t9con as u32);
        }

        if let Some(tmuxcon) = config.tmuxcon {
            current = TMUXCON::apply(current, tmuxcon as u32);
        }

        if let Some(nmuxcon) = config.nmuxcon {
            current = NMUXCON::apply(current, nmuxcon as u32);
        }

        if let Some(pmuxcon) = config.pmuxcon {
            current = PMUXCON::apply(current, pmuxcon as u32);
        }

        if let Some(dmuxcon) = config.dmuxcon {
            current = DMUXCON::apply(current, dmuxcon as u32);
        }

        self.write_reg(Register::SWCON, current).await?;

        Ok(())
    }

    pub async fn apply_clk_config(&mut self, config: &ClkConfig) -> Result<(), Error> {
        // PMBW
        let mut current = self.read_reg(Register::PMBW).await?;
        current |= 0b1;
        self.write_reg(Register::PMBW, current).await?;

        // CLKCON0
        let mut current = self.read_reg(Register::CLKCON0).await?;

        if let Some(adcclkdiv) = config.adcclkdiv {
            current = ADCCLKDIV::apply(current, adcclkdiv as u32);
        }

        if let Some(sysclkdiv) = config.sysclkdiv {
            current = SYSCLKDIV::apply(current, sysclkdiv as u32);
        }
        self.write_reg(Register::CLKCON0, current).await?;

        // HSOSCCON
        let mut current = self.read_reg(Register::HSOSCCON).await?;
        if let Some(clk32mhzen) = config.clk32mhzen {
            current = CLK32MHZEN::apply(current, clk32mhzen as u32);
        }

        self.write_reg(Register::HSOSCCON, current).await?;

        Ok(())
    }

    pub async fn apply_lp_config(&mut self, config: &LpConfig) -> Result<(), Error> {
        // LPDACCON0
        let mut current = self.read_reg(Register::LPDACCON0).await?;

        if let Some(data_reset) = config.data_reset {
            current = RSTEN::apply(current, data_reset as u32);
        }

        if let Some(power_enable) = config.power_enable {
            current = PWDEN::apply(current, !power_enable as u32);
        }

        self.write_reg(Register::LPDACCON0, current).await?;

        // LPDACDAT0
        let mut current = self.read_reg(Register::LPDACDAT0).await?;

        if let Some(data) = config.data_6bit {
            current = DACIN6::apply(current, data as u32);
        }

        self.write_reg(Register::LPDACDAT0, current).await?;

        // SWMUX
        let mut current = self.read_reg(Register::SWMUX).await?;

        if let Some(common_mode_switch) = config.common_mode_switch {
            current = SWMUX::apply(current, common_mode_switch as u32);
        }

        self.write_reg(Register::SWMUX, current).await?;

        // LPTIASW0 
        if let Some(tia_switches_enabled) = config.tia_switches_enabled {
            self.write_reg(Register::LPTIASW0, tia_switches_enabled as u32).await?;
        }

        // LPTIACON0
        let mut current = self.read_reg(Register::LPTIACON0).await?;

        if let Some(tia_enable) = config.tia_enable {
            current = TIAPDEN::apply(current, !tia_enable as u32);
        }

        if let Some(tiarf) = config.filter_resistor {
            current = TIARF::apply(current, tiarf as u32);
        }

        self.write_reg(Register::LPTIACON0, current).await?;

        Ok(())
    }

    pub async fn apply_dsp_config(&mut self, config: &DspConfig) -> Result<(), Error> {
        // ADCCON
        let mut current = self.read_reg(Register::ADCCON).await?;

        if let Some(gnpgain) = config.gnpgain {
            current = GNPGAIN::apply(current, gnpgain as u32);
        }
        if let Some(muxseln) = config.muxseln {
            current = MUXSELN::apply(current, muxseln as u32);
        }
        if let Some(muxselp) = config.muxselp {
            current = MUXSELP::apply(current, muxselp as u32);
        }

        self.write_reg(Register::ADCCON, current).await?;

        // HSRTIACON
        let mut current = self.read_reg(Register::HSRTIACON).await?;

        if let Some(ctiacon) = config.ctiacon {
            current = CTIACON::apply(current, ctiacon as u32);
        }

        if let Some(rtiacon) = config.rtiacon {
            current = RTIACON::apply(current, rtiacon as u32);
        }

        self.write_reg(Register::HSRTIACON, current).await?;

        // ADCFILTERCON
        let mut current = self.read_reg(Register::ADCFILTERCON).await?;

        if let Some(sinc3osr) = config.sinc3osr{
            current = SINC3OSR::apply(current, sinc3osr as u32);
        }

        if let Some(sinc2osr) = config.sinc2osr {
            current = SINC2OSR::apply(current, sinc2osr as u32);
        }

        if let Some(adcsamplerate) = config.adcsamplerate {
            current = ADCSAMPLERATE::apply(current, adcsamplerate as u32);
        }

        self.write_reg(Register::ADCFILTERCON, current).await?;

        // DFTCON
        let mut current =  self.read_reg(Register::DFTCON).await?;

        if let Some(dftin) = config.dftin {
            current = DFTINSEL::apply(current, dftin as u32);
        }
        if let Some(dftnum) = config.dftnum {
            current = DFTNUM::apply(current, dftnum as u32);
        }
        if let Some(hanning) = config.hanning {
            current = HANNING::apply(current, hanning as u32);
        }
        self.write_reg(Register::DFTCON, current).await?;

        Ok(())
    }

    pub async fn afecon(&mut self, ctr: AFECON, state: bool) {
        let reg = self.read_reg(Register::AFECON).await.unwrap();
        let mut reg = AFECON::from_bits_truncate(reg);

        let flags = [
            AFECON::DACBUFEN,
            AFECON::DACREFEN,
            AFECON::SINC2EN,
            AFECON::DFTEN,
            AFECON::WAVEGENEN,
            AFECON::TEMPCONVEN,
            AFECON::TEMPSENSEN,
            AFECON::TIAEN,
            AFECON::INAMPEN,
            AFECON::EXBUFEN,
            AFECON::ADCCONVEN,
            AFECON::ADCEN,
            AFECON::DACEN,
            AFECON::HSREFDIS,
        ];

        for &flag in flags.iter() {
            if ctr.contains(flag) {
                reg = if state {
                    reg | flag
                } else {
                    reg & !flag
                };
            }
        }

        self.write_reg(Register::AFECON, reg.bits()).await.unwrap();
    }

    pub async fn wgfcw(&mut self, frequency: u32) {
        let sinefcw = (frequency as f64) * (1_073_741_824.0 / 16_000_000.0);
        let sinefcw = 0x00FFFFFF & sinefcw as u32;
        self.write_reg(Register::WGFCW, sinefcw).await.unwrap();
    }

    pub async fn apply_sram_config(&mut self, config: SramConfig) -> Result<(), Error> {
         // Disable sequencer
        self.write_reg(Register::SEQCON, 0x0000_0002).await?;

        // Reset SEQCON
        self.write_reg(Register::SEQCNT, 0x0000_0001).await?;

        // FIFICON
        let mut current_fifocon = self.read_reg(Register::FIFOCON).await?;
        if let Some(datafifosrcsel) = config.datafifosrcsel {
            current_fifocon = DATAFIFOSRCSEL::apply(current_fifocon, datafifosrcsel as u32);
        }
        current_fifocon &= !(0b1 << 11); // Disable FIFO
        self.write_reg(Register::FIFOCON, current_fifocon).await?;

        // CMDDATACON
        let mut current = self.read_reg(Register::CMDDATACON).await?;
        if let Some(size) = config.data_mem_size {
            current = DATA_MEM_SEL::apply(current, size as u32);
        }
        if let Some(mode) = config.cmd_mem_mode {
            current = CMDMEMMDE::apply(current, mode as u32);
        }
        if let Some(size) = config.cmd_mem_size {
            current = CMD_MEM_SEL::apply(current, size as u32);
        }
        self.write_reg(Register::CMDDATACON, current).await?;

        // Enable FIFO
        current_fifocon |= 0b1 << 11; // Enable FIFO
        self.write_reg(Register::FIFOCON, current_fifocon).await?;

        // Enable sequencer
        self.write_reg(Register::SEQCON, 0x0000_0003).await?;

        Ok(())
    }

    pub async fn get_chipid(&mut self) -> u16 {
        self.read_reg(Register::CHIPID).await.unwrap() as u16
    }

    pub async fn get_adiid(&mut self) -> 
    u16 {
        self.read_reg(Register::ADIID).await.unwrap() as u16
    }

    pub async fn init_temperature(&mut self) -> Result<(), Error> {
        // AFECON:
        self.afecon(AFECON::TEMPSENSEN | AFECON::ADCEN, true).await;

        // ADCCON
        let config_adccon = ADCCON::GNPGA_1_5.bits() | ADCCON::MUXSELN_TEMP.bits() | ADCCON::MUXSELP_TEMP.bits();
        self.write_reg(Register::ADCCON, config_adccon).await?;

        // AFECON - start conversion
        self.afecon(AFECON::TEMPCONVEN | AFECON::ADCCONVEN, true).await;

        Ok(())
    }

    pub async fn get_temperature(&mut self) -> Result<f32, Error> {
        // See page 57 of the datasheet for temperature calculation
        let pga_gain = 1.5;
        let k = 8.13;

        let mut tempsensdat0: u32 = self.read_reg(Register::TEMPSENSDAT).await?;
        tempsensdat0 &= 0x0000FFFF;

        Ok((tempsensdat0 as f32/(pga_gain * k)) - 273.15)
    }

    pub async fn get_fifo_count(&mut self) -> Result<u32, Error> {
        let data = self.read_reg(Register::FIFOCNTSTA).await?;
        Ok((data >> 16) & 0x7FF)  
    }

    pub async fn clear_and_enable_fifo(&mut self) -> Result<(), Error> {
        let mut fifocon = self.read_reg(Register::FIFOCON).await?;
        fifocon = DATAFIFOEN::apply(fifocon, DATAFIFOEN::FIFOisReset as u32); // Disable FIFO
        self.write_reg(Register::FIFOCON, fifocon).await?;

        fifocon = DATAFIFOEN::apply(fifocon, DATAFIFOEN::Normal as u32); // Enable FIFO
        self.write_reg(Register::FIFOCON, fifocon).await?;

        Ok(())
    }

    pub async fn init_waveform(&mut self) -> Result<(), Error> {
        // Set frequency
        let freq: u32 = 1000;
        self.wgfcw(freq).await;

        // Init amplitude
        let wg_amplitude = 2047; // 2047 is the maximum amplitude for a 12-bit DAC --> 1.62V peak-to-peak
        self.write_reg(Register::WGAMPLITUDE, wg_amplitude).await?;

        // WGCON, enable waveform generator and set to sinusoidal, NO DACCAL
        let config_wgcon = WGCON::TYPESEL_SIN.bits();
        self.write_reg(Register::WGCON, config_wgcon).await?;

        // SWCON - set up switch matri
        let switch_config = SwitchConfig::default()
            .nmuxcon(NMUXCON::N2Closed)
            .pmuxcon(PMUXCON::P11Closed)
            .dmuxcon(DMUXCON::D5Closed);
        self.apply_switch_config(switch_config).await?;
        
        // AFECON:
        self.afecon(
            AFECON::DACREFEN
            | AFECON::EXBUFEN
            | AFECON::INAMPEN
            | AFECON::DACEN
            | AFECON::WAVEGENEN,
            true,
        )
        .await;

        Ok(())
    }
}

#[allow(dead_code)]
#[allow(non_camel_case_types)]
#[repr(u8)]
enum Command {
    SPICMD_SETADDR  = 0x20,
    SPICMD_READREG  = 0x6D,
    SPICMD_WRITEREG = 0x2D,
    SPICMD_READFIFO = 0x5F,
}

#[allow(dead_code)]
#[allow(non_camel_case_types)]
#[repr(u16)]
#[derive(Clone, Copy, Eq, PartialEq, Debug, defmt::Format)]
pub enum Register {
    ADIID       = 0x0000_0400, // Analog Devices Inc., identification register
    CHIPID      = 0x0000_0404, // Chip identification register
    TRIGSEQ     = 0x0000_0430, // Trigger Sequencer Register
    AFECON      = 0x0000_2000, // Configuration Register
    SEQCON      = 0x0000_2004, // Sequencer Configuration Register
    FIFOCON     = 0x0000_2008, // FIFO Configuration Register
    SWCON       = 0x0000_200C, // Switch Matrix Configuration Register
    WGCON       = 0x0000_2014, // Waveform Generator Configuration Register
    WGFCW       = 0x0000_2030, // Waveform Generator, Sinusoid Frequency Control Word Register
    WGAMPLITUDE = 0x0000_203C, // Waveform Generator, Sinusoid Amplitude Register
    SEQCNT      = 0x0000_2064, // Sequencer Command Count Register
    CMDFIFOWRITE = 0x0000_2070, // Command FIFO Write Register
    TEMPSENSDAT = 0x0000_2084, // Temperature Sensor Result Register
    ADCDAT      = 0x0000_2074, // ADC Raw Result Register
    LPTIASW0    = 0x0000_20E4, // Low power TIA switch configuration
    LPTIACON0   = 0x0000_20EC, // Low power TIA control bits, Channel 0
    LPDACDAT0   = 0x0000_2120, // Low power DAC data output Register 
    LPDACCON0   = 0x0000_2128, // Low power DAC configuration register 
    TEMPSENS    = 0x0000_2174, // Temperature Sensor Configuration Register
    ADCCON      = 0x0000_21A8, // ADC Configuration Register
    SEQ0INFO    = 0x0000_21CC, // Sequence 0 Information Register
    SEQ1INFO    = 0x0000_21E8, // Sequence 1 Information Register
    SEQ2INFO    = 0x0000_21D0, // Sequence 2 Information Register
    CMDDATACON  = 0x0000_21D8, // Command Data Control Register
    SEQ3INFO    = 0x0000_21E4, // Sequence 3 Information Register
    AFEGENINTSTA = 0x0000_209C, // Analog Generation Interrupt Register
    CMDFIFOWADDR = 0x0000_21D4, // Command FIFO Write Address Register
    PMBW        = 0x0000_22F0, // Power Mode Configuration Register
    SWMUX       = 0x0000_235C, // Common-mode switch mux select register 
    INTCFLAG0   = 0x0000_3010, // Interrupt Control Flag 0 Register
    INTCFLAG1   = 0x0000_3014, // Interrupt Control Flag 1 Register
    OSCCON      = 0x0000_0A10, // Oscillator Control Register
    SEQTIMEOUT  = 0x0000_2068, // Sequencer Timeout Counter Register
    SEQCRC      = 0x0000_2060, // Sequencer CRC Value Register
    DFTREAL     = 0x0000_2078, // DFT Result, Real Device Register
    DFTIMAG     = 0x0000_207C, // DFT Result, Imaginary Device Register
    DATAFIFOTHRES = 0x0000_21E0, // Data FIFO Threshold Register
    SYNCEXTDEVICE = 0x0000_2054, // Sync External Device Register
    GP0CON      = 0x0000_0000, // GPIO Port 0 Configuration Register
    DATAFIFORD  = 0x0000_206C, // Data FIFO Read Register
    DFTCON      = 0x0000_20D0, // DFT Configuration Register
    HSDACCON    = 0x0000_2010, // High Speed DAC Configuration Register
    HSRTIACON   = 0x0000_20F0, // High Speed RTIA Configuration Register
    BUFSENCON   = 0x0000_2180, // HIGH POWER AND LOW POWER BUFFER CONTROL REGISTER
    FIFOCNTSTA  = 0x0000_2200, // Command and data FIFO internal data count register 
    ADCFILTERCON = 0x0000_2044, // ADC Output Filters Configuration Register
    CLKCON0     = 0x0000_0408, // Clock Divider Configuration Register
    HSOSCCON    = 0x0000_20BC, // High Power Oscillator Configuration Register
}