use core::convert::Infallible;
use embedded_hal::{
    blocking::{delay::DelayMs, i2c::WriteRead},
    digital::v2::OutputPin,
};
use rp2040_hal::Timer;

const ADDRESS: u8 = 0x3B;

pub enum Registers {
    DeltaX = 0x03,
    DeltaY = 0x04,
}

pub struct Touchpad<I: WriteRead> {
    i2c: I,
}

impl<I: WriteRead> Touchpad<I> {
    pub fn new<R: OutputPin<Error = Infallible>, S: OutputPin<Error = Infallible>>(
        i2c: I,
        mut reset: R,
        mut shutdown: S,
        timer: &mut Timer,
    ) -> Self {
        shutdown.set_low().unwrap();
        reset.set_low().unwrap();
        timer.delay_ms(100);
        reset.set_high().unwrap();

        Self { i2c }
    }

    pub fn get_delta_motion(&mut self) -> Result<(i8, i8), I::Error> {
        let mut x = [0];
        self.i2c
            .write_read(ADDRESS, &[Registers::DeltaX as u8], &mut x)?;
        let x = x[0];

        let mut y = [0];
        self.i2c
            .write_read(ADDRESS, &[Registers::DeltaY as u8], &mut y)?;
        let y = y[0];

        let x = if x < 128 {
            x.try_into().unwrap()
        } else {
            i8::try_from(i16::from(x) - 256).unwrap()
        };

        let y = if y < 128 {
            y.try_into().unwrap()
        } else {
            i8::try_from(i16::from(y) - 256).unwrap()
        };

        Ok((x, y))
    }
}