use crate::thermostat::ad5680;
use core::ptr::addr_of_mut;
use fugit::KilohertzU32;
use stm32f4xx_hal::{
    adc::{config::{self, AdcConfig}, Adc}, 
    dma::{config::DmaConfig, PeripheralToMemory, Stream2, StreamsTuple, Transfer as DMA_Transfer}, 
    gpio::{gpioa::*, gpiob::*, gpioc::*, Analog, Output, PushPull}, 
    hal::{self, spi::SpiBus, digital::OutputPin}, 
    pac::{ADC1, ADC2, DMA2, SPI1, TIM4, Peripherals, NVIC}, 
    spi::Spi, 
    timer::pwm::PwmChannel,
    interrupt
};

use uom::si::{
    electric_potential::millivolt,
    f32::ElectricPotential,
    ratio::ratio,
};

pub const PWM_FREQ_KHZ: KilohertzU32 = KilohertzU32::from_raw(20);

pub trait ChannelPins {
    type DacSpi: SpiBus<u8>;
    type DacSync: OutputPin;
    type ShdnPin: OutputPin;
    type VRefPin;
    type ItecPin;
    type DacFeedbackPin;
    type VTecPin;
    type MaxVPin;
    type MaxIPosPin;
    type MAXINegPin;
}

pub struct Channel0;

impl ChannelPins for Channel0 {
    type DacSpi = DacSpi;
    type DacSync = DacSync;
    type ShdnPin = PA5<Output<PushPull>>;
    type VRefPin = PA6<Analog>;
    type ItecPin = PB1<Analog>;
    type DacFeedbackPin = PC0<Analog>;
    type VTecPin = PB0<Analog>;
    type MaxVPin = PwmChannel<TIM4, 1>;
    type MaxIPosPin = PwmChannel<TIM4, 2>;
    type MAXINegPin = PwmChannel<TIM4, 0>;
}

pub struct MAX1968Phy<C: ChannelPins> {
    pub dac: ad5680::Dac<C::DacSpi, C::DacSync>,
    pub shdn: C::ShdnPin,
    pub vref_pin: C::VRefPin,
    pub itec_pin: C::ItecPin,
    pub dac_feedback_pin: C::DacFeedbackPin,
    pub vtec_pin: C::VTecPin,
    pub max_v: C::MaxVPin,
    pub max_i_pos: C::MaxIPosPin,
    pub max_i_neg: C::MAXINegPin,
}

pub struct MAX1968PinSet<C: ChannelPins> {
    pub dac: ad5680::Dac<C::DacSpi, C::DacSync>,
    pub shdn: C::ShdnPin,
    pub vref_pin: C::VRefPin,
    pub itec_pin: C::ItecPin,
    pub dac_feedback_pin: C::DacFeedbackPin,
    pub vtec_pin: C::VTecPin,
    pub max_v: C::MaxVPin,
    pub max_i_pos: C::MaxIPosPin,
    pub max_i_neg: C::MAXINegPin,
}

type DacSpi = Spi<SPI1>;
type DacSync = PB4<Output<PushPull>>;

static mut DMA_TRANSFER_COMPLETE: bool = true;

pub struct MAX1968 {
    pub phy: MAX1968Phy<Channel0>,
    pub pins_adc: Adc<ADC1>,
    pub dma_adc: DMA_Transfer<Stream2<DMA2>, 1, Adc<ADC2>, PeripheralToMemory, &'static mut [u16; 16]>,
    prev_vtec_volt: ElectricPotential,
    prev_itec_volt: ElectricPotential,
}

pub enum PwmPinsEnum {
    MaxV,
    MaxPosI,
    MaxNegI,
}

#[allow(unused)]
pub enum AdcReadTarget {
    VREF,
    DacVfb,
    ITec,
    VTec,
}

impl<C: ChannelPins> MAX1968Phy<C> {
    pub fn new(pins: MAX1968PinSet<C>) -> Self {
        MAX1968Phy {
            dac: pins.dac,
            shdn: pins.shdn,
            vref_pin: pins.vref_pin,
            itec_pin: pins.itec_pin,
            dac_feedback_pin: pins.dac_feedback_pin,
            vtec_pin: pins.vtec_pin,
            max_v: pins.max_v,
            max_i_pos: pins.max_i_pos,
            max_i_neg: pins.max_i_neg,
        }
    }
}

static mut ADC2_FIRST_BUFFER : [u16; 16] = [0; 16];
static mut ADC2_LOCAL_BUFFER : [u16; 16] = [0; 16];

impl MAX1968 {
    pub fn new(phy_ch0: MAX1968Phy<Channel0>, adc1: ADC1, adc2: ADC2, dma2: DMA2) -> Self {
        let adc_config = AdcConfig::default()
            .clock(config::Clock::Pclk2_div_8)
            .default_sample_time(config::SampleTime::Cycles_480);
        // Do not set reset RCCs as it causes other ADCs' clock to be disabled
        let mut pins_adc1 = Adc::adc1(adc1, false, adc_config);
        pins_adc1.calibrate();

        let adc_config  = AdcConfig::default()
            .clock(config::Clock::Pclk2_div_8)
            .default_sample_time(config::SampleTime::Cycles_480)
            .dma(config::Dma::Continuous)
            .scan(config::Scan::Enabled)
            .reference_voltage(pins_adc1.reference_voltage());
        let dma_config = DmaConfig::default()
            .transfer_complete_interrupt(true)
            .memory_increment(true)
            .double_buffer(false);
        let mut pins_adc2 = Adc::adc2(adc2, false, adc_config);
        pins_adc2.configure_channel(&phy_ch0.itec_pin, config::Sequence::One, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.vtec_pin, config::Sequence::Two, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.itec_pin, config::Sequence::Three, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.vtec_pin, config::Sequence::Four, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.itec_pin, config::Sequence::Five, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.vtec_pin, config::Sequence::Six, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.itec_pin, config::Sequence::Seven, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.vtec_pin, config::Sequence::Eight, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.itec_pin, config::Sequence::Nine, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.vtec_pin, config::Sequence::Ten, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.itec_pin, config::Sequence::Eleven, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.vtec_pin, config::Sequence::Twelve, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.itec_pin, config::Sequence::Thirteen, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.vtec_pin, config::Sequence::Fourteen, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.itec_pin, config::Sequence::Fifteen, config::SampleTime::Cycles_480);
        pins_adc2.configure_channel(&phy_ch0.vtec_pin, config::Sequence::Sixteen, config::SampleTime::Cycles_480);

        let dma = StreamsTuple::new(dma2);
        let dma_adc : DMA_Transfer<Stream2<DMA2>, 1, Adc<ADC2>, PeripheralToMemory, &'static mut [u16; 16]>;
        unsafe {
            dma_adc = DMA_Transfer::init_peripheral_to_memory(dma.2, pins_adc2, addr_of_mut!(ADC2_FIRST_BUFFER).as_mut().unwrap(), None, dma_config);
            NVIC::unmask(interrupt::DMA2_STREAM2);
        }

        MAX1968 {
            phy: phy_ch0,
            pins_adc: pins_adc1,
            dma_adc: dma_adc,
            prev_vtec_volt: ElectricPotential::new::<millivolt>(0.0), 
            prev_itec_volt: ElectricPotential::new::<millivolt>(0.0), 
        }
    }

    pub fn dma_adc_start_conversion(&mut self){
        if unsafe {DMA_TRANSFER_COMPLETE} {
            unsafe { DMA_TRANSFER_COMPLETE = false; }
            self.dma_adc.start(|adc| {       
                adc.clear_end_of_conversion_flag();
                adc.start_conversion();
            });
        }  
    }

    pub fn get_tec_readings(&mut self) -> (ElectricPotential, ElectricPotential) {
        if unsafe { DMA_TRANSFER_COMPLETE } {
            let buffer: &[u16; 16];
            unsafe {
                (buffer, _) = self.dma_adc
                    .next_transfer(addr_of_mut!(ADC2_LOCAL_BUFFER).as_mut().unwrap())
                    .unwrap();
            }
            
            let sample_to_millivolts = self.dma_adc.peripheral().make_sample_to_millivolts();
            let mut itec: u16 = 0;
            for data in buffer.into_iter().step_by(2) {
                itec += *data;
            }
            itec = itec >> 3;

            let mut vtec: u16 = 0;
            for data in buffer.into_iter().skip(1).step_by(2) {
                vtec += *data;
            }
            vtec = vtec >> 3;

            unsafe {
                ADC2_LOCAL_BUFFER = *buffer;
            }
            self.prev_vtec_volt = ElectricPotential::new::<millivolt>(sample_to_millivolts(vtec) as f32);
            self.prev_itec_volt = ElectricPotential::new::<millivolt>(sample_to_millivolts(itec) as f32);
        }
        (self.prev_vtec_volt, self.prev_itec_volt)
    }

    // Return the calibrated VDDA Voltage
    // Can be used to set reference voltage for other ADC
    pub fn get_calibrated_vdda(&mut self) -> u32 {
        self.pins_adc.reference_voltage()
    }

    pub fn is_powered_on(&mut self) -> bool {
        self.phy.shdn.is_set_high()
    }

    pub fn power_down(&mut self) {
        self.phy.shdn.set_low();
    }

    pub fn power_up(&mut self) {
        self.phy.shdn.set_high();
    }


    pub fn set_dac(&mut self, voltage: ElectricPotential, dac_out_v_max: ElectricPotential) -> ElectricPotential {
        let value = ((voltage / dac_out_v_max).get::<ratio>()
            * (ad5680::MAX_VALUE as f32)) as u32;
        self.phy.dac.set(value).unwrap();
        voltage
    }

    // AN4073: ADC Reading Dispersion can be reduced through Averaging
    // Upon test, 16 Point Averaging = +-3 LSB Dispersion
    pub fn adc_read(&mut self, adc_read_target: AdcReadTarget, avg_pt: u16) -> ElectricPotential {
        let mut sample: u32 = 0;
        sample = match adc_read_target {
            AdcReadTarget::VREF => {
                for _ in (0..avg_pt).rev() {
                    sample += self.pins_adc.convert(
                        &self.phy.vref_pin,
                        stm32f4xx_hal::adc::config::SampleTime::Cycles_480,
                    ) as u32;
                }
                sample / avg_pt as u32
            }
            AdcReadTarget::DacVfb => {
                for _ in (0..avg_pt).rev() {
                    sample += self.pins_adc.convert(
                        &self.phy.dac_feedback_pin,
                        stm32f4xx_hal::adc::config::SampleTime::Cycles_480,
                    ) as u32;
                }
                sample / avg_pt as u32
            }
            AdcReadTarget::ITec => {
                for _ in (0..avg_pt).rev() {
                    sample += self.pins_adc.convert(
                        &self.phy.itec_pin,
                        stm32f4xx_hal::adc::config::SampleTime::Cycles_480,
                    ) as u32;
                }
                sample / avg_pt as u32
            }
            AdcReadTarget::VTec => {
                for _ in (0..avg_pt).rev() {
                    sample += self.pins_adc.convert(
                        &self.phy.vtec_pin,
                        stm32f4xx_hal::adc::config::SampleTime::Cycles_480,
                    ) as u32;
                }
                sample / avg_pt as u32
            }
        };
        let mv = self.pins_adc.sample_to_millivolts(sample as u16);
        ElectricPotential::new::<millivolt>(mv as f32)
    }

    pub fn set_pwm(&mut self, pwm_pin: PwmPinsEnum, duty: f64, max_duty: f64) -> f64 {
        fn duty_cycle_value(duty_cycle: f64, duty_cycle_limit: f64, max_value: u16) -> u16 {
            ((duty_cycle.min(duty_cycle_limit) * (max_value as f64)) as u16).min(max_value)
        }

        let value: u16;
        let max_value: u16;
        match pwm_pin {
            PwmPinsEnum::MaxV => {
                self.phy.max_v.enable();
                max_value = self.phy.max_v.get_max_duty();
                value = duty_cycle_value(duty, max_duty, max_value);
                self.phy.max_v.set_duty(value);
                
            }
            PwmPinsEnum::MaxPosI => {
                self.phy.max_i_pos.enable();
                max_value = self.phy.max_i_pos.get_max_duty();
                value = duty_cycle_value(duty, max_duty, max_value);
                self.phy.max_i_pos.set_duty(value);
            }
            PwmPinsEnum::MaxNegI => {
                self.phy.max_i_neg.enable();
                max_value = self.phy.max_i_neg.get_max_duty();
                value = duty_cycle_value(duty, max_duty, max_value);
                self.phy.max_i_neg.set_duty(value);
            }
        }
        return (value as f64) / (max_value as f64);
    }

}

#[interrupt]
fn DMA2_STREAM2(){
    cortex_m::interrupt::free(|_| {
            unsafe {
                // Clear all DMA2_STREAM2 interrupt flags
                Peripherals::steal().DMA2.lifcr.write(|w| w
                    .ctcif2().set_bit()
                    .cdmeif2().set_bit()
                    .chtif2().set_bit()
                    .cteif2().set_bit()
                );
                DMA_TRANSFER_COMPLETE = true;
            }
        }
    )
}