use embedded_hal::adc::OneShot;
use rp2040_hal::{
    adc::AdcPin,
    gpio::{
        bank0::{Gpio26, Gpio28},
        FunctionNull, Pin, PullNone,
    },
    Adc,
};

// u16, 3.3V ADC, resistor half-divider
const FACTOR: f64 = 3.3 * 2.0 / 4095.0;

type Batt = Pin<Gpio26, FunctionNull, PullNone>;
type Usb = Pin<Gpio28, FunctionNull, PullNone>;

trait Abs {
    fn abs(self) -> Self;
}

impl Abs for f64 {
    fn abs(self) -> Self {
        if self > 0.0 {
            self
        } else {
            -self
        }
    }
}

pub struct VoltMeter {
    batt: AdcPin<Batt>,
    usb: AdcPin<Usb>,
    adc: Adc,

    batt_average: f64,
    usb_average: f64,
}

impl VoltMeter {
    pub fn new(adc: Adc, batt: Batt, usb: Usb) -> Self {
        let batt = AdcPin::new(batt).unwrap();
        let usb = AdcPin::new(usb).unwrap();

        let mut s = Self {
            batt,
            usb,
            adc,

            batt_average: 0.0,
            usb_average: 0.0,
        };

        let (cbat, cusb) = s.cur_voltage();
        s.batt_average = cbat;
        s.usb_average = cusb;

        s
    }

    pub fn voltage(&mut self) -> (f64, f64) {
        let (cbat, cusb) = self.cur_voltage();

        if (self.batt_average - cbat).abs() > 0.1 {
            self.batt_average = cbat;
        } else {
            // geometric averaging
            self.batt_average *= 0.95;
            self.batt_average += cbat * 0.05;
        }

        if (self.usb_average - cusb).abs() > 0.1 {
            self.usb_average = cusb;
        } else {
            self.usb_average *= 0.95;
            self.usb_average += cusb * 0.05;
        }

        (self.batt_average, self.usb_average)
    }

    fn cur_voltage(&mut self) -> (f64, f64) {
        let batt: u16 = self.adc.read(&mut self.batt).unwrap();
        let usb: u16 = self.adc.read(&mut self.usb).unwrap();

        (batt as f64 * FACTOR, usb as f64 * FACTOR)
    }
}