//! Driver for the Anyleaf pH module

#![no_std]
#![allow(non_snake_case)]

#[macro_use(block)]
extern crate nb;

use ads1x1x::{
    self,
    channel::{DifferentialA0A1, SingleA2},
    ic::{Ads1115, Resolution16Bit},
    interface::I2cInterface,
    Ads1x1x, SlaveAddr,
};
use embedded_hal::{
    adc::OneShot,
    blocking::i2c::{Read, Write, WriteRead},
};
use filter::kalman::kalman_filter::KalmanFilter;
use nalgebra::{Vector1, dimension::{U1, U2}};

mod filter_;

// Compensate for temperature diff between readings and calibration.
const PH_TEMP_C: f32 = -0.05694; // pH/(V*T);

pub type Adc<I> = Ads1x1x<
    I2cInterface<I>,
    Ads1115,
    Resolution16Bit,
    ads1x1x::mode::OneShot,
    // todo: How do we do type for continuous variant?
>;

#[derive(Debug, Clone, Copy)]
/// Keeps our calibration organized, so we track when to overwrite.
pub enum CalSlot {
    One,
    Two,
    Three,
}

#[derive(Debug, Clone)]
/// Data for a single pH (or other ion measurement) calibration point.
pub struct CalPt {
    V: f32, // voltage, in Volts
    pH: f32,
    T: f32, // in Celsius
}

impl CalPt {
    pub fn new(V: f32, pH: f32, T: f32) -> Self {
        Self { V, pH, T }
    }
}

pub struct PhSensor<I2C: Read<Error = E> + Write<Error = E> + WriteRead<Error = E>, E> {
    adc: Ads1x1x<
        ads1x1x::interface::I2cInterface<I2C>,
        Ads1115,
        Resolution16Bit,
        ads1x1x::mode::OneShot, // todo continuous support
    >,
    pub filter: KalmanFilter<f32, U2, U1, U1>,
    cal_1: CalPt,
    cal_2: CalPt,
    cal_3: Option<CalPt>,
}

impl<I2C: WriteRead<Error = E> + Write<Error = E> + Read<Error = E>, E> PhSensor<I2C, E> {
    pub fn new(i2c: I2C, dt: f32) -> Self {
        // `dt` is in seconds.
        let adc = Ads1x1x::new_ads1115(i2c, SlaveAddr::default());
        // Leave the default range of 2.048V; this is overkill (and reduces precision of)
        // the pH sensor, but is needed for the temp sensor.

        Self {
            adc,
            filter: filter_::create(dt),
            cal_1: CalPt::new(0., 7., 23.),
            cal_2: CalPt::new(0.18, 4., 23.),
            cal_3: None,
        }
    }

    /// Make a prediction using the Kalman filter. Not generally used directly.
    pub fn predict(&mut self) {
        self.filter.predict(None, None, None, None)
    }

    /// Update the Kalman filter with a pH reading. Not generally used directly.
    pub fn update(&mut self) -> Result<(), ads1x1x::Error<E>> {
        let z = Vector1::new(self.read_raw()?);
        self.filter.update(&z, None, None);

        Ok(())
    }

    /// Take a pH reading, using the Kalman filter. This reduces sensor
    /// noise, and provides a more accurate reading.
    // pub fn read(&mut self) -> Result<f32, ReadError> {
    pub fn read(&mut self) -> Result<f32, ads1x1x::Error<E>> {
        self.predict();
        self.update()?;
        // self.filter.x is mean, variance.We only care about the mean
        Ok(self.filter.x[0])
    }

    /// Take a pH reading, without using the Kalman filter
    // todo: find the right error type for nb/ads111x
    // todo: Error type: is this right? Read:Error instead?
    pub fn read_raw(&mut self) -> Result<f32, ads1x1x::Error<E>> {
        let T = temp_from_voltage(voltage_from_adc(
            block!(self.adc.read(&mut SingleA2))?
        ));

        Ok(ph_from_voltage(
            voltage_from_adc(block!(self.adc.read(&mut DifferentialA0A1))?),
            T,
            &self.cal_1,
            &self.cal_2,
            &self.cal_3,
        ))
    }

    /// Useful for getting calibration data
    pub fn read_voltage(&mut self) -> Result<f32, ads1x1x::Error<E>> {
        Ok(voltage_from_adc(
            block!(self.adc.read(&mut DifferentialA0A1))?,
        ))
    }

    /// Useful for getting calibration data
    pub fn read_temp(&mut self) -> Result<f32, ads1x1x::Error<E>> {
        Ok(temp_from_voltage(voltage_from_adc(
            block!(self.adc.read(&mut SingleA2))?
        )))
    }

    /// Calibrate by measuring voltage and temp at a given pH. Set the
    /// calibration, and return (Voltage, Temp).
    pub fn calibrate(&mut self, slot: CalSlot, pH: f32) -> Result<(f32, f32), ads1x1x::Error<E>> {
        let T = temp_from_voltage(voltage_from_adc(
            block!(self.adc.read(&mut SingleA2))?
        ));
        let V = voltage_from_adc(block!(self.adc.read(&mut DifferentialA0A1))?);
        let pt = CalPt::new(V, pH, T);

        match slot {
            CalSlot::One => self.cal_1 = pt,
            CalSlot::Two => self.cal_2 = pt,
            CalSlot::Three => self.cal_3 = Some(pt),
        }
        Ok((V, T))
    }

    pub fn calibrate_all(&mut self, pt0: CalPt, pt1: CalPt, pt2: Option<CalPt>) {
        self.cal_1 = pt0;
        self.cal_2 = pt1;
        self.cal_3 = pt2;
    }

    pub fn reset_calibration(&mut self) {
        self.cal_1 = CalPt::new(0., 7., 25.);
        self.cal_2 = CalPt::new(0.18, 4., 25.);
        self.cal_3 = None;
    }
}

/// Convert the adc's 16-bit digital values to voltage.
/// Input ranges from +- 2.048V; this is configurable.
/// Output ranges from -32_768 to +32_767.
pub fn voltage_from_adc(digi: i16) -> f32 {
    let vref = 2.048;
    (digi as f32 / 32_768.) * vref
}

/// Compute the result of a Lagrange polynomial of order 3.
/// Algorithm created from the `P(x)` eq
/// [here](https://mathworld.wolfram.com/LagrangeInterpolatingPolynomial.html).
/// todo: Figure out how to just calculate the coefficients for a more
/// todo flexible approach. More eloquent, but tough to find info on compared
/// todo to this approach.
fn lg(pt0: (f32, f32), pt1: (f32, f32), pt2: (f32, f32), X: f32) -> f32 {
    let mut result = 0.;

    let x = [pt0.0, pt1.0, pt2.0];
    let y = [pt0.1, pt1.1, pt2.1];

    for j in 0..3 {
        let mut c = 1.;
        for i in 0..3 {
            if j == i {
                continue;
            }
            c *= (X - x[i]) / (x[j] - x[i]);
        }
        result += y[j] * c;
    }

    result
}

/// Convert voltage to pH
/// We model the relationship between sensor voltage and pH linearly
/// using 2-pt calibration, or quadratically using 3-pt. Temperature
/// compensated. Input `temp` is in Celsius.
fn ph_from_voltage(V: f32, temp: f32, cal_0: &CalPt, cal_1: &CalPt, cal_2: &Option<CalPt>) -> f32 {
    // We infer a -.05694 pH/(V*T) sensitivity linear relationship
    // (higher temp means higher pH/V ratio)
    let T_diff = temp - cal_0.T;
    let T_comp = PH_TEMP_C * T_diff; // pH / V

    match cal_2 {
        // Model as a quadratic Lagrangian polynomial, to compensate for slight nonlinearity.
        Some(c2) => {
            let result = lg((cal_0.V, cal_0.pH), (cal_1.V, cal_1.pH), (c2.V, c2.pH), V);
            result + T_comp * V
        }
        // Model as a line
        None => {
            // a is the slope, pH / v.
            let a = (cal_1.pH - cal_0.pH) / (cal_1.V - cal_0.V);
            let b = cal_1.pH - a * cal_1.V;
            (a + T_comp) * V + b
        }
    }
}

/// Map voltage to temperature for the TI LM61, in °C
/// Datasheet: https://datasheet.lcsc.com/szlcsc/Texas-
/// Instruments-TI-LM61BIM3-NOPB_C132073.pdf
pub fn temp_from_voltage(V: f32) -> f32 {
    100. * V - 60.
}