use core::marker::PhantomData;
use smoltcp::time::Instant;
use crate::sys_timer;
use crate::thermostat::ad5680;
use crate::thermostat::max1968::{MAX1968, AdcReadTarget, PwmPinsEnum};
use crate::thermostat::ad7172;
use crate::thermostat::channel_state::ChannelState;
use log::info;
use uom::si::{
    electric_current::ampere,
    electric_potential::volt,
    electrical_resistance::ohm,
    f64::{ElectricCurrent, ElectricPotential, ElectricalResistance, Time},
    ratio::ratio,
};
use miniconf::Miniconf;

pub const R_SENSE: ElectricalResistance = ElectricalResistance {
    dimension: PhantomData,
    units: PhantomData,
    value: 0.05,
};

#[derive(Clone, Debug, Miniconf)]
pub struct Settings {
    pub center_pt: ElectricPotential,
    pub max_v_set: ElectricPotential,
    pub max_i_pos_set: ElectricCurrent,
    pub max_i_neg_set: ElectricCurrent,
    pub i_set: ElectricCurrent,
}

impl Settings{
    // FixMe: Rev0_2 is 3.3V max while Rev0_3 is 3.0V max 
    pub const DAC_OUT_V_MAX: ElectricPotential = ElectricPotential {
        dimension: PhantomData,
        units: PhantomData,
        value: 3.3,
    };
    pub const TEC_VSEC_BIAS_V: ElectricPotential = ElectricPotential {
        dimension: PhantomData,
        units: PhantomData,
        value: 1.65,
    };
    
    // Kirdy Design Specs:
    // MaxV = 5.0V
    // MAX Current = +- 1.0A
    const MAX_V_DUTY_TO_CURRENT_RATE: ElectricPotential = ElectricPotential {
        dimension: PhantomData,
        units: PhantomData,
        value: 4.0 * 3.3,
    };
    pub const MAX_V_MAX: ElectricPotential = ElectricPotential {
        dimension: PhantomData,
        units: PhantomData,
        value: 5.0,
    };
    const MAX_V_DUTY_MAX: f64 = Settings::MAX_V_MAX.value / Settings::MAX_V_DUTY_TO_CURRENT_RATE.value;
    const MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE: ElectricCurrent = ElectricCurrent {
        dimension: PhantomData,
        units: PhantomData,
        value: 1.0 / (10.0 * R_SENSE.value / 3.3),
    };
    pub const MAX_I_POS_CURRENT: ElectricCurrent = ElectricCurrent {
        dimension: PhantomData,
        units: PhantomData,
        value: 1.0,
    };
    pub const MAX_I_NEG_CURRENT: ElectricCurrent = ElectricCurrent {
        dimension: PhantomData,
        units: PhantomData,
        value: 1.0,
    };
    // .get::<ratio>() is not implemented for const
    const MAX_I_POS_DUTY_MAX: f64 = Settings::MAX_I_POS_CURRENT.value / Settings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE.value;
    const MAX_I_NEG_DUTY_MAX: f64 = Settings::MAX_I_NEG_CURRENT.value / Settings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE.value;
}

impl Default for Settings {
    fn default() -> Self {
        Self {
            center_pt: ElectricPotential::new::<volt>(1.5),
            max_v_set: ElectricPotential::new::<volt>(5.0),
            max_i_pos_set: ElectricCurrent::new::<ampere>(1.0),
            max_i_neg_set: ElectricCurrent::new::<ampere>(1.0),
            i_set: ElectricCurrent::new::<ampere>(0.0),
        }
    }
}

pub struct Thermostat {
    max1968: MAX1968,
    ad7172: ad7172::AdcPhy,
    pub tec_setting: Settings,
    pid_ctrl_ch0: ChannelState,
}

impl Thermostat{
    pub fn new (max1968: MAX1968, ad7172: ad7172::AdcPhy) -> Self {
        Thermostat{
            max1968: max1968,
            ad7172: ad7172,
            tec_setting: Settings::default(),
            pid_ctrl_ch0: ChannelState::default(),
        }
    }
    pub fn setup(&mut self){
        self.tec_setup();
        let t_adc_ch0_cal = self.t_adc_setup();
        self.pid_ctrl_ch0.adc_calibration = t_adc_ch0_cal;
    }

    fn tec_setup(&mut self) {
        self.power_down();

        self.tec_setting = Settings::default();

        self.set_i(self.tec_setting.i_set);

        self.set_max_v(self.tec_setting.max_v_set);
        self.set_max_i_pos(self.tec_setting.max_i_pos_set);
        self.set_max_i_neg(self.tec_setting.max_i_neg_set);
    }

    fn t_adc_setup(&mut self)->ad7172::ChannelCalibration{
        self.ad7172.set_sync_enable(false).unwrap();
        self.ad7172.setup_channel(0, ad7172::Input::Ain0, ad7172::Input::Ain1).unwrap();
        let adc_calibration0 = self.ad7172.get_calibration(0)
            .expect("adc_calibration0");
        self.ad7172.start_continuous_conversion().unwrap();
        adc_calibration0
    }

    pub fn poll_adc(&mut self, instant: Instant) -> Option<u8> {
        self.ad7172.data_ready().unwrap().map(|channel| {
            let data = self.ad7172.read_data().unwrap();
            let state: &mut ChannelState = &mut self.pid_ctrl_ch0;
            state.update(instant, data);
            match state.update_pid() {
                Some(pid_output) if state.pid_engaged => {
                    // Forward PID output to i_set DAC
                    self.set_i(ElectricCurrent::new::<ampere>(pid_output));
                    self.power_up();
                }
                None if state.pid_engaged => {
                    self.power_down();
                }
                _ => {}
            }

            channel
        })
    }

    pub fn power_up(&mut self){
        self.max1968.power_up();
    }

    pub fn power_down(&mut self){
        self.max1968.power_down();
    }

    fn set_center_pt(&mut self, value: ElectricPotential){
        info!("set center pt: {:?}", value);
        self.tec_setting.center_pt = value;
    }

    pub fn set_i(&mut self, i_tec: ElectricCurrent) -> ElectricCurrent {
        let voltage = i_tec * 10.0 * R_SENSE + self.tec_setting.center_pt;
        let voltage = self.max1968.set_dac(voltage, Settings::DAC_OUT_V_MAX);
        self.tec_setting.i_set = (voltage - self.tec_setting.center_pt) / (10.0 * R_SENSE);
        self.tec_setting.i_set
    }

    pub fn set_max_v(&mut self, max_v: ElectricPotential) -> ElectricPotential {
        let duty = (max_v / Settings::MAX_V_DUTY_TO_CURRENT_RATE).get::<ratio>();
        let duty = self.max1968.set_pwm(PwmPinsEnum::MaxV, duty, Settings::MAX_V_DUTY_MAX);
        self.tec_setting.max_v_set = duty * Settings::MAX_V_DUTY_TO_CURRENT_RATE;
        self.tec_setting.max_v_set
    }

    pub fn set_max_i_pos(&mut self, max_i_pos: ElectricCurrent) -> ElectricCurrent {
        let duty = (max_i_pos / Settings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE).get::<ratio>();
        let duty = self.max1968.set_pwm(PwmPinsEnum::MaxPosI, duty, Settings::MAX_I_POS_DUTY_MAX);
        self.tec_setting.max_i_pos_set = duty * Settings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE;
        self.tec_setting.max_i_pos_set
    }

    pub fn set_max_i_neg(&mut self, max_i_neg: ElectricCurrent) -> ElectricCurrent {
        let duty = (max_i_neg / Settings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE).get::<ratio>();
        let duty = self.max1968.set_pwm(PwmPinsEnum::MaxNegI, duty, Settings::MAX_I_NEG_DUTY_MAX);
        self.tec_setting.max_i_neg_set = duty * Settings::MAX_I_POS_NEG_DUTY_TO_CURRENT_RATE;
        self.tec_setting.max_i_neg_set
    }

    pub fn get_dac_vfb(&mut self) -> ElectricPotential {
        self.max1968.adc_read(AdcReadTarget::DacVfb, 16)
    }

    pub fn get_vref(&mut self) -> ElectricPotential {
        self.max1968.adc_read(AdcReadTarget::VREF, 16)
    }

    pub fn get_tec_i(&mut self) -> ElectricCurrent {
        let vref = self.get_vref();
        (self.max1968.adc_read(AdcReadTarget::ITec, 16) - vref) / ElectricalResistance::new::<ohm>(0.4)
    }

    pub fn get_tec_v(&mut self) -> ElectricPotential {
        // Fixme: Rev0_2 has Analog Input Polarity Reversed
        //  Remove the -ve sign for Rev0_3
        -(self.max1968.adc_read(AdcReadTarget::VTec, 16) - Settings::TEC_VSEC_BIAS_V) * 4.0
    }

    /// Calibrates the DAC output to match vref of the MAX driver to reduce zero-current offset of the MAX driver output.
    ///
    /// The thermostat DAC applies a control voltage signal to the CTLI pin of MAX driver chip to control its output current.
    /// The CTLI input signal is centered around VREF of the MAX chip. Applying VREF to CTLI sets the output current to 0.
    /// 
    /// This calibration routine measures the VREF voltage and the DAC output with the STM32 ADC, and uses a breadth-first     
    /// search to find the DAC setting that will produce a DAC output voltage closest to VREF. This DAC output voltage will 
    /// be stored and used in subsequent i_set routines to bias the current control signal to the measured VREF, reducing 
    /// the offset error of the current control signal.
    ///
    /// The input offset of the STM32 ADC is eliminated by using the same ADC for the measurements, and by only using the
    /// difference in VREF and DAC output for the calibration.
    /// 
    /// This routine should be called only once after boot, repeated reading of the vref signal and changing of the stored 
    /// VREF measurement can introduce significant noise at the current output, degrading the stabilily performance of the
    /// thermostat. 
    pub fn calibrate_dac_value(&mut self) {
        let target_voltage = self.max1968.adc_read(AdcReadTarget::VREF, 64);
        let mut start_value = 1;
        let mut best_error = ElectricPotential::new::<volt>(100.0);
        for step in (0..18).rev() {
            info!("Step: {} Calibrating", step);
            let mut prev_value = start_value;
            for value in (start_value..=ad5680::MAX_VALUE).step_by(1 << step) {
                info!("Calibrating: Value: {:?}", value);
                self.max1968.phy.dac.set(value).unwrap();
                sys_timer::sleep(5);

                let dac_feedback = self.max1968.adc_read(AdcReadTarget::DacVfb, 64);
                let error = target_voltage - dac_feedback;
                if error < ElectricPotential::new::<volt>(0.0) {
                    break;
                } else if error < best_error {
                    best_error = error;
                    start_value = prev_value;

                    let vref = (value as f64 / ad5680::MAX_VALUE as f64) * Settings::DAC_OUT_V_MAX;
                    self.set_center_pt(vref);
                }
                prev_value = value;
            }
        }
        info!("Best Error: {:?}", best_error);
    }
    
    pub fn pid_engaged(&mut self) -> bool {
        if self.pid_ctrl_ch0.pid_engaged {
            return true;
        }
        false
    }

}