use uom::si::{
    electric_current::ampere, electric_potential::volt, electrical_resistance::ohm, f64::{
    ElectricCurrent, ElectricPotential, ElectricalResistance, ThermodynamicTemperature
    }, thermodynamic_temperature::degree_celsius
};
use crate::thermostat::{
    ad7172,
    steinhart_hart as sh,
};
use idsp::iir::{Pid, Action, Biquad};
use crate::debug;

const R_INNER: f64 = 2.0 * 5100.0;
const VREF_SENS: f64 = 3.3 / 2.0;

pub struct PidState {
    adc_data: Option<u32>,
    adc_calibration: ad7172::ChannelCalibration,
    pid_engaged: bool,
    pid: Biquad<f32>,
    pid_out_min: ElectricCurrent,
    pid_out_max: ElectricCurrent,
    xy: [f32; 4],
    set_point: ThermodynamicTemperature,
    settings: Pid<f32>,
    sh: sh::Parameters,
}

impl Default for PidState {
    fn default() -> Self {
        const OUT_MIN: f32 = -1.0;
        const OUT_MAX: f32 = 1.0;

        let mut pid_settings = Pid::<f32>::default();
        pid_settings
            // Pid Update Period depends on the AD7172 Output Data Rate Set
            .period(0.1)
            .limit(Action::Kp, 10.0)
            .limit(Action::Ki, 10.0)
            .limit(Action::Kd, 10.0);

        let mut pid: Biquad<f32> = pid_settings.build().unwrap().into();
        pid.set_min(OUT_MIN);
        pid.set_max(OUT_MAX);

        PidState {
            adc_data: None,
            adc_calibration: ad7172::ChannelCalibration::default(),
            pid_engaged: false,
            pid: pid,
            pid_out_min: ElectricCurrent::new::<ampere>(OUT_MIN as f64),
            pid_out_max: ElectricCurrent::new::<ampere>(OUT_MAX as f64),
            xy: [0.0; 4],
            set_point: ThermodynamicTemperature::new::<degree_celsius>(0.0),
            settings: pid_settings,
            sh: sh::Parameters::default(),
        }
    }
}

pub enum PidSettings {
    Kp,
    Ki,
    Kd,
    Min,
    Max,
}

impl PidState {
    pub fn update(&mut self, adc_data: u32) {
        self.adc_data = if adc_data == ad7172::MAX_VALUE {
            // this means there is no thermistor plugged into the ADC.
            None
        } else {
            Some(adc_data)
        };
    }

    /// Update PID state on ADC input, calculate new DAC output
    pub fn update_pid(&mut self) -> Option<f64> {
        let input = (self.get_temperature()?.get::<degree_celsius>() as f32) - (self.set_point.get::<degree_celsius>() as f32);
        let pid_output = self.pid.update(&mut self.xy, input);
        debug!("BiQuad Storage [x0, x1, y0, y1]: {:?}", self.xy);
        Some(pid_output as f64)
    }

    pub fn reset_pid_state(&mut self){
        self.xy = [0.0; 4];
    }

    pub fn get_adc(&self) -> Option<ElectricPotential> {
        Some(self.adc_calibration.convert_data(self.adc_data?))
    }

    /// Get `SENS[01]` input resistance
    pub fn get_sens(&self) -> Option<ElectricalResistance> {
        let r_inner = ElectricalResistance::new::<ohm>(R_INNER);
        let vref = ElectricPotential::new::<volt>(VREF_SENS);
        let adc_input = self.get_adc()?;
        let r = r_inner * adc_input / (vref - adc_input);
        Some(r)
    }

    pub fn get_temperature(&self) -> Option<ThermodynamicTemperature> {
        let r = self.get_sens()?;
        let temperature = self.sh.get_temperature(r);
        Some(temperature)
    }

    pub fn set_pid_params(&mut self, param: PidSettings, val: f64){
        match param {
            PidSettings::Kp => { 
                self.settings.gain(Action::Kp, val as f32);
            }
            PidSettings::Ki => { 
                self.settings.gain(Action::Ki, val as f32);
            }
            PidSettings::Kd => { 
                self.settings.gain(Action::Kd, val as f32);
            }
            PidSettings::Min => { 
                self.pid_out_min = ElectricCurrent::new::<ampere>(val);
            }
            PidSettings::Max => { 
                self.pid_out_max = ElectricCurrent::new::<ampere>(val);
            }
        }
        self.update_pid_settings();
    }
    
    pub fn set_pid_period(&mut self, period: f32){
        self.settings.period(period);
        self.pid = self.settings.build().unwrap().into();
    }

    pub fn update_pid_settings(&mut self){
        self.pid = self.settings.build().unwrap().into();
        self.pid.set_max(self.pid_out_max.get::<ampere>() as f32);
        self.pid.set_min(self.pid_out_min.get::<ampere>() as f32);
    }

    pub fn set_pid_setpoint(&mut self, temperature: ThermodynamicTemperature){
        self.set_point = temperature;
    }

    pub fn get_pid_setpoint(&mut self) -> ThermodynamicTemperature {
        self.set_point
    }

    pub fn set_sh_t0(&mut self, t0: ThermodynamicTemperature){
        self.sh.t0 = t0
    }

    pub fn set_sh_r0(&mut self, r0: ElectricalResistance){
        self.sh.r0 = r0
    }

    pub fn set_sh_beta(&mut self, beta: f64){
        self.sh.b = beta
    }

    pub fn set_adc_calibration(&mut self, adc_cal: ad7172::ChannelCalibration){
        self.adc_calibration = adc_cal;
    }

    pub fn set_pid_engaged(&mut self, pid_engaged: bool){
        self.pid_engaged = pid_engaged;
    }

    pub fn get_pid_engaged(&mut self) -> bool {
        self.pid_engaged
    }

    pub fn get_pid_settings(&mut self) -> Pid<f32> {
        unimplemented!()
    }

    pub fn get_sh(&mut self) -> sh::Parameters {
        unimplemented!()
    }
}