use defmt::{info, error};

use embassy_stm32::spi::Error;
use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_sync::channel::Channel;
use embassy_sync::mutex::Mutex;

use static_cell::StaticCell;

use crate::ad5940::*;
use crate::ad5940_registers::*;

use bioz_icd_rs::ImpedanceOutput;

pub static IMPEDANCE_CHANNEL: Channel<ThreadModeRawMutex, ImpedanceOutput, 2000> = Channel::new();

pub type ImpedanceSetupType = Mutex<ThreadModeRawMutex, ImpedanceSetup>;
pub static IMPEDANCE_SETUP: StaticCell<ImpedanceSetupType> = StaticCell::new();

pub struct ImpedanceSetup {
    ad5940: AD5940,
    dsp_config: Option<DspConfig>,
    pub running: bool,
}

impl ImpedanceSetup {
    pub fn new(ad5940: AD5940) -> Self {
        ImpedanceSetup { ad5940, dsp_config: None, running: false }
    }
        
    pub async fn init(&mut self) -> Result<(), Error> {
        // AFECON:
        self.ad5940.afecon(
            AFECON::DACBUFEN
            | AFECON::DACREFEN
            | AFECON::SINC2EN
            | AFECON::TIAEN
            | AFECON::INAMPEN
            | AFECON::EXBUFEN
            | AFECON::DACEN,
            true,
        )
        .await;

        // Set DSP configuration
        let dsp_config = DspConfig::default()
            .adc_mux_n(MUXSELN::HsTiaNeg)
            .adc_mux_p(MUXSELP::HsTiaPos)
            .ctiacon(CTIACON::C32)
            .rtiacon(RTIACON::R1k)
            .sinc3osr(SINC3OSR::R5)
            .sinc2osr(SINC2OSR::R178)
            .adcsamplerate(ADCSAMPLERATE::R800kHz)
            .dftin_sel(DFTINSEL::GainOffset)
            .dftnum(DFTNUM::Num2048)
            .hanning(true);

        self.ad5940.apply_dsp_config(&dsp_config).await.unwrap();
        self.dsp_config = Some(dsp_config);

        // Set SRAM configuration (cmd and data sram)
        let sram_config = SramConfig::default()
            .datafifosrcsel(DATAFIFOSRCSEL::DFT)
            .datafifoen(DATAFIFOEN::Normal)
            .data_size(DATA_MEM_SEL::Size2kB)
            .cmd_mode(CMDMEMMDE::MemoryMode)
            .cmd_size(CMD_MEM_SEL::Size2kB);
        self.ad5940.apply_sram_config(sram_config).await.unwrap();

        // WGCON: set sinus output
        let config_wgcon = WGCON::TYPESEL_SIN.bits();
        self.ad5940.write_reg(Register::WGCON, config_wgcon).await.unwrap();

        Ok(())
    }

    pub async fn init_single_frequency_measurement(&mut self, frequency: u32) {
        // Configure GPIOs
        self.ad5940.write_reg(Register::GP0CON, 0b10 << 4 | 0b10 << 2 | 0b10).await.unwrap();
        self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap();

        // Calculate wait time between measurement start and stop
        let mut wait_time = 0;
        if let Some(dsp_config) = &self.dsp_config {
            wait_time = self.ad5940.sequencer_calculate_wait_time(dsp_config).await.unwrap();
            info!("Sinus periods per DFT: {}", wait_time as f32 / 16e6 * frequency as f32);
        } else {
            error!("DSP configuration not set, cannot calculate wait time");
        }

        // Configure sequencer
        self.ad5940.sequencer_enable(true).await;

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

        // Rcal
        let switch_config = SwitchConfig::default()
            .t9con(T9CON::T9Closed)
            .tmuxcon(TMUXCON::TR1Closed)
            .nmuxcon(NMUXCON::NR1Closed)
            .pmuxcon(PMUXCON::PR0Closed)
            .dmuxcon(DMUXCON::DR0Closed);
        self.ad5940.apply_switch_config(switch_config).await.unwrap();
        self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await;
        self.ad5940.sequencer_wait(16*10).await; // 10 us
        self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await;
        self.ad5940.sequencer_wait(wait_time).await; // Determined above
        self.ad5940.sequencer_wait(16*20).await; // 10 us
        self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await;

        // Rz
        let switch_config = SwitchConfig::default()
            .t9con(T9CON::T9Closed)
            .tmuxcon(TMUXCON::T2Closed)
            .nmuxcon(NMUXCON::N2Closed)
            .pmuxcon(PMUXCON::P11Closed)
            .dmuxcon(DMUXCON::D5Closed);
        self.ad5940.apply_switch_config(switch_config).await.unwrap();
        self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await;
        self.ad5940.sequencer_wait(16*10).await; // 10 us
        self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await;
        self.ad5940.sequencer_wait(wait_time).await; // Determined above
        self.ad5940.sequencer_wait(16*20).await; // 10 us
        self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await;

        // Toggle leds
        self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b010).await.unwrap();
        self.ad5940.sequencer_wait(16 * 1_000).await; // 0.025 second
        self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap();

        self.ad5940.sequencer_enable(false).await;

        // Write sequence to SRAM
        let start_address = 0;
        self.ad5940.sequencer_cmd_write(start_address).await;

        self.ad5940.sequencer_info_configure(0, self.ad5940.seq_len, start_address).await;

        self.start_measurement().await;
    }

    pub async fn start_measurement(&mut self) {
        self.ad5940.sequencer_trigger(0).await;
    }

    pub async fn get_fifo_count(&mut self) ->  Result<u32, Error> {
        self.ad5940.get_fifo_count().await
    }

    pub async fn read_fifo(&mut self, data: &mut [u32]) -> Result<(), Error> {
        self.ad5940.read_fifo(data).await
    }
}