use miniconf::Tree;
use serde::{Deserialize, Serialize};
use uom::si::{
    electric_potential::volt, electrical_resistance::ohm, f32::{
    ElectricPotential, ElectricalResistance, ThermodynamicTemperature
    }, thermodynamic_temperature::degree_celsius
};
use crate::thermostat::{
    ad7172,
    steinhart_hart as sh,
};
const R_INNER: f32 = 2.0 * 5100.0;
const VREF_SENS: f32 = 3.3 / 2.0;

#[derive(Deserialize, Serialize, Clone, Copy, Debug, PartialEq, Tree)]
pub struct Parameters {
    /// Gain coefficient for proportional term
    pub kp: f32,
    /// Gain coefficient for integral term
    pub ki: f32,
    /// Gain coefficient for derivative term
    pub kd: f32,
    /// Output limit minimum
    pub output_min: f32,
    /// Output limit maximum
    pub output_max: f32,
}

impl Default for Parameters {
    fn default() -> Self {
        Parameters {
            kp: 0.0,
            ki: 0.0,
            kd: 0.0,
            output_min: -1.0,
            output_max: 1.0,
        }
    }
}

#[derive(Clone)]
pub struct Controller {
    pub parameters: Parameters,
    u1 : f64,
    x1 : f64,
    x2 : f64,
    pub y1 : f64,
}


pub struct PidState {
    adc_data: Option<u32>,
    adc_calibration: ad7172::ChannelCalibration,
    pid_engaged: bool,
    set_point: ThermodynamicTemperature,
    sh: sh::Parameters,
    controller: Controller,
}

impl Default for PidState {
    fn default() -> Self {
        PidState {
            adc_data: None,
            adc_calibration: ad7172::ChannelCalibration::default(),
            pid_engaged: false,
            set_point: ThermodynamicTemperature::new::<degree_celsius>(0.0),
            sh: sh::Parameters::default(),
            controller: Controller {
                parameters: Parameters::default(),
                u1 : 0.0,
                x1 : 0.0,
                x2 : 0.0,
                y1 : 0.0,
            },
        }
    }
}

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

    // Based on https://hackmd.io/IACbwcOTSt6Adj3_F9bKuw PID implementation
    // Input x(t), target u(t), output y(t)
    // y0' =   y1 - ki * u0   
    //       + x0 * (kp + ki + kd)
    //       - x1 * (kp + 2kd)
    //       + x2 * kd
    //       + kp * (u0 - u1)
    // y0  = clip(y0', ymin, ymax)
    pub fn update_pid(&mut self) -> Option<f64> {
        let input = self.get_temperature()?.get::<degree_celsius>();
        let setpoint = self.set_point.get::<degree_celsius>();
        let mut output: f64 = self.controller.y1 - setpoint as f64 * f64::from(self.controller.parameters.ki)
                            + input as f64 * f64::from(self.controller.parameters.kp + self.controller.parameters.ki + self.controller.parameters.kd)
                            - self.controller.x1 * f64::from(self.controller.parameters.kp + 2.0 * self.controller.parameters.kd)
                            + self.controller.x2 * f64::from(self.controller.parameters.kd)
                            + f64::from(self.controller.parameters.kp) * (setpoint as f64 - self.controller.u1);
        if output < self.controller.parameters.output_min.into() {
            output = self.controller.parameters.output_min.into();
        }
        if output > self.controller.parameters.output_max.into() {
            output = self.controller.parameters.output_max.into();
        }
        self.controller.x2 = self.controller.x1;
        self.controller.x1 = input as f64;
        self.controller.u1 = setpoint as f64;
        self.controller.y1 = output;        
        Some(output)
    }

    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: f32){
        match param {
            PidSettings::Kp => { 
                self.controller.parameters.kp = val;
            }
            PidSettings::Ki => { 
                self.controller.parameters.ki = val;
            }
            PidSettings::Kd => { 
                self.controller.parameters.kd = val;
            }
            PidSettings::Min => { 
                self.controller.parameters.output_min = val;
            }
            PidSettings::Max => { 
                self.controller.parameters.output_max = val;
            }
        }
    }

    pub fn reset_pid_state(&mut self){
        self.controller.u1 = 0.0;
        self.controller.x1 = 0.0;
        self.controller.x2 = 0.0;
        self.controller.y1 = 0.0;
    }

    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: f32){
        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) -> Parameters {
        self.controller.parameters
    }

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