use core::{
    marker::PhantomData,
    cell::RefCell,
    ops::Range,
};

use embedded_hal::adc::*;

use stm32f1xx_hal::{
    backup_domain::BackupDomain,
    crc::Crc,
};

use libm::*;

use half::f16;

use crate::config::SystemConfig;

//--------------------------------------------------------------------------------------------------
/* 2nd order linearisation factor for the temperature probe */
const A: f32 = -0.000574;
const B: f32 = 0.0676;
const C: f32 = -0.07;

//--------------------------------------------------------------------------------------------------
/* valid temperature range */
pub const TEMP_RANGE: Range<f32> = -10.0..45.0;

//--------------------------------------------------------------------------------------------------
/* error management */
#[derive(Debug)]
pub enum ProbeError {
    ReadError,
    Initializing,
    OutOfRange,
}

//--------------------------------------------------------------------------------------------------
/* TemperatureProbe */
const FILTER_TIME_CONSTANT: f32 = 100.0;

pub struct TemperatureProbe<'a, ADC, O, PINP, PINN> 
where 
    PINP: Channel<ADC>,
    PINN: Channel<ADC>,
    O: OneShot<ADC, u16, PINP> + OneShot<ADC, u16, PINN>,
{
    adc: &'a RefCell<O>,
    pos_pin: PINP,
    neg_pin: PINN,
    phantom: PhantomData<ADC>,

    filtered_temp: f32,
    stabilized: bool,
}

fn read_temp<'a, ADC, O, PINP, PINN>(adc: &'a RefCell<O>, pos: &mut PINP, neg: &mut PINN) 
    -> Result<f32, ProbeError>
where
    PINP: Channel<ADC>,
    PINN: Channel<ADC>,
    O: OneShot<ADC, u16, PINP> + OneShot<ADC, u16, PINN>,
{
    //first read is bugged, may be due to fake chip 
    let _ = adc.borrow_mut().read(pos);
    let pos = adc.borrow_mut().read(pos)
        .map_err(|_| ProbeError::ReadError)? as f32;

    //first read is bugged, may be due to fake chip 
    let _ = adc.borrow_mut().read(neg);
    let neg = adc.borrow_mut().read(neg)
        .map_err(|_| ProbeError::ReadError)? as f32;

    // compute back voltage, then reverse linearisation to compute temperature
    let volt = (pos - neg) * 3.3 / 4095.0;
    Ok((-B + sqrtf(B*B - 4.0*(C - volt)*A)) / (2.0*A))
}

impl<'a, ADC, O, PINP, PINN> TemperatureProbe<'a, ADC, O, PINP, PINN>
where
    PINP: Channel<ADC>,
    PINN: Channel<ADC>,
    O: OneShot<ADC, u16, PINP> + OneShot<ADC, u16, PINN>,
{
    pub fn new(adc: &'a RefCell<O>, pos: PINP, neg: PINN) 
        -> Result<TemperatureProbe<ADC, O, PINP, PINN>, ProbeError> {
        
        let mut pos_pin = pos;
        let mut neg_pin = neg;
        // compute first temp approximation to speed up stabilization
        let mut temp = 0.0;
        for _ in 0..10 {
            temp += read_temp(adc, &mut pos_pin, &mut neg_pin)?;        
        }
        temp /= 10.0;

        Ok(TemperatureProbe {
            adc,
            pos_pin,
            neg_pin,
            phantom: PhantomData,
            filtered_temp: temp,
            stabilized: false,
        })
    }

    pub fn read(&mut self) -> Result<f32, ProbeError> {

        let temp = read_temp(self.adc, &mut self.pos_pin, &mut self.neg_pin)?;

        match self.stabilized {
            true => {
                self.filtered_temp += (temp - self.filtered_temp)/FILTER_TIME_CONSTANT;
                match TEMP_RANGE.contains(&self.filtered_temp) { 
                    true => Ok(self.filtered_temp),
                    false => Err(ProbeError::OutOfRange),
                }
            },
            false => {  // filter is not yet stabilized
                let old_temp = self.filtered_temp;
                self.filtered_temp += (temp - self.filtered_temp)/FILTER_TIME_CONSTANT;

                match fabsf(old_temp - self.filtered_temp) < 0.01 {
                    true => self.stabilized = true,
                    false => (),
                }

                Err(ProbeError::Initializing)
            },
        }
    }

    pub fn reset(&mut self) -> Result<(), ProbeError>{

        // re-compute first temp approximation to speed up stabilization
        let mut temp = 0.0;
        for _ in 0..10 {
            temp += read_temp(self.adc, &mut self.pos_pin, &mut self.neg_pin)?;        
        }
        temp /= 10.0;
        
        self.filtered_temp = temp;
        self.stabilized = false;

        Ok(())
    }
}

//--------------------------------------------------------------------------------------------------
/* ConfigStorage */

pub struct ConfigStorage {
    backup_domain: BackupDomain,
    crc: Crc,
}

impl ConfigStorage {

    pub fn new(backup_domain: BackupDomain, crc: Crc) -> ConfigStorage {
    
        ConfigStorage {
            backup_domain,
            crc,
        }
    }

    pub fn load_config(&mut self) -> Result<SystemConfig, ()> {
        use crate::config::{CalData, FanOutput};

        // read from persistent memory
        let mut values: [u16; 10] = [0; 10];
        for (i, val) in values.iter_mut().enumerate() {
            *val = self.backup_domain.read_data_register_low(i);
        }

        // compute CRC
        self.crc.reset();
        for i in (2..8).step_by(2) {
            self.crc.write((values[i] as u32) << 16 | values[i+1] as u32);
        }
        let crc = (values[0] as u32) << 16 | values[1] as u32;

        // parse config if CRC is valid
        match crc != 0 && crc == self.crc.read() {
            true => {
                Ok(SystemConfig {
                    max_temp_diff: f16::from_bits(values[2]).to_f32(),
                    min_temp_diff: f16::from_bits(values[3]).to_f32(),
                    ext_offset: CalData::new(f16::from_bits(values[4]).to_f32()),
                    p1_offset: CalData::new(f16::from_bits(values[5]).to_f32()), 
                    p2_offset: CalData::new(f16::from_bits(values[6]).to_f32()), 
                    fan_output: FanOutput::from_bits(values[7]),
                })
            },
            false => Err(()),
        }
    }

    pub fn save_config(&mut self, config: &SystemConfig) {

        // prepare buffer for later storage
        let mut values: [u16; 10] = [0; 10];
        values[2] = f16::from_f32(config.max_temp_diff).to_bits();
        values[3] = f16::from_f32(config.min_temp_diff).to_bits();
        values[4] = f16::from_f32(config.ext_offset.offset).to_bits();
        values[5] = f16::from_f32(config.p1_offset.offset).to_bits();
        values[6] = f16::from_f32(config.p2_offset.offset).to_bits();
        values[7] = config.fan_output.to_bits();
 
        // compute CRC
        self.crc.reset();
        for i in (2..8).step_by(2) {
            self.crc.write((values[i] as u32) << 16 | values[i+1] as u32);
        }
        let crc = self.crc.read();
        values[0] = ((crc >> 16) & 0xFFFF) as u16;
        values[1] = (crc & 0xFFFF) as u16;

        // write to persistent memory
        for (i, val) in values.iter().enumerate() {
            self.backup_domain.write_data_register_low(i, *val);
        }
    }
}