use smoltcp::time::{Duration, Instant};
use uom::si::{
    f64::{
        ElectricPotential,
        ElectricalResistance,
        ThermodynamicTemperature,
        Time,
    },
    electric_potential::volt,
    electrical_resistance::ohm,
    thermodynamic_temperature::degree_celsius,
    time::millisecond,
};
use crate::thermostat::{
    ad7172,
    steinhart_hart as sh,
};
use crate::pid::pid;

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

pub struct PidState {
    pub adc_data: Option<u32>,
    pub adc_calibration: ad7172::ChannelCalibration,
    pub update_ts: Time,
    pub update_interval: Time,
    /// i_set 0A center point
    pub center_point: ElectricPotential,
    pub dac_volt: ElectricPotential,
    pub pid_engaged: bool,
    pub pid: pid::Controller,
    pub sh: sh::Parameters,
}

impl PidState {
    fn adc_calibration(mut self, adc_calibration: ad7172::ChannelCalibration) -> Self {
        self.adc_calibration = adc_calibration;
        self
    }
}

impl Default for PidState {
    fn default() -> Self {
        PidState {
            adc_data: None,
            adc_calibration: ad7172::ChannelCalibration::default(),
            update_ts: Time::new::<millisecond>(0.0),
            // default: 10 Hz
            update_interval: Time::new::<millisecond>(100.0),
            center_point: ElectricPotential::new::<volt>(1.5), 
            dac_volt: ElectricPotential::new::<volt>(0.0),
            pid_engaged: false,
            pid: pid::Controller::new(pid::Parameters::default()),
            sh: sh::Parameters::default(),
        }
    }
}

impl PidState {
    pub fn new(adc_calibration: ad7172::ChannelCalibration) -> Self {
        PidState::default().adc_calibration(adc_calibration)
    }

    pub fn update(&mut self, now: Instant, 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)
        };
        self.update_interval = Time::new::<millisecond>(now.millis() as f64) - self.update_ts;
        self.update_ts = Time::new::<millisecond>(now.millis() as f64);
    }

    /// Update PID state on ADC input, calculate new DAC output
    pub fn update_pid(&mut self) -> Option<f64> {
        let temperature = self.get_temperature()?
            .get::<degree_celsius>();
        let pid_output = self.pid.update(temperature);
        Some(pid_output)
    }

    pub fn get_update_ts(&self) -> Time {
        self.update_ts
    }

    pub fn get_update_interval(&self) -> Time {
        self.update_interval
    }

    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)
    }
}