use crate::sys_timer;
use crate::thermostat::ad5680;
use crate::thermostat::max1968::{MAX1968, AdcReadTarget, DAC_OUT_V_MAX};
use log::info;
use uom::si::{
    electric_current::ampere,
    electric_potential::{millivolt, volt},
    electrical_resistance::ohm,
    f64::{ElectricCurrent, ElectricPotential, ElectricalResistance},
    ratio::ratio,
};

pub struct Thermostat {
    max1968: MAX1968,
    // TADC
}

impl Thermostat{
    pub fn new (max1968: MAX1968) -> Self {
        Thermostat{
            max1968
        }
    }
    pub fn setup(&mut self){
        self.max1968.setup();
    }

    /// 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 samples = 50;
        let mut target_voltage = ElectricPotential::new::<volt>(0.0);
        for _ in 0..samples {
            target_voltage = target_voltage + self.max1968.adc_read(AdcReadTarget::VREF, 1);
        }
        target_voltage = target_voltage / samples as f64;
        let mut start_value = 1;
        let mut best_error = ElectricPotential::new::<volt>(100.0);
        let before_cal = self.max1968.phy.center_pt;
        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");
                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) * ElectricPotential::new::<volt>(DAC_OUT_V_MAX);
                    self.max1968.set_center_point(vref);

                }
                prev_value = value;
            }
        }
        
        loop {
            info!("Before Calibration, VREF = {:?}", before_cal);
            info!("After Calibration, VREF = {:?}", self.max1968.phy.center_pt);
            self.max1968.set_i(ElectricCurrent::new::<ampere>(0.0));
            info!("VREF Value {:?}", self.max1968.adc_read(AdcReadTarget::VREF, 64));
            info!("DAC VFB Value {:?}", self.max1968.adc_read(AdcReadTarget::DacVfb, 64));
            sys_timer::sleep(100);
        }
    }
    
}