use core::convert::Infallible;

use embedded_hal::{
    blocking::delay::{DelayMs, DelayUs},
    digital::v2::{InputPin, OutputPin},
};
use rp2040_hal::Timer;
use rtt_target::rprintln;

pub struct Keyboard<
    // col
    CCLK: OutputPin<Error = Infallible>,
    CSER: OutputPin<Error = Infallible>,
    CRCLK: OutputPin<Error = Infallible>,
    // row
    RCLK: OutputPin<Error = Infallible>,
    RSER: InputPin,
    RLD: OutputPin<Error = Infallible>,
> {
    timer: Timer,
    col_clk: CCLK,
    col_ser: CSER,
    col_rclk: CRCLK,
    row_clk: RCLK,
    row_ser: RSER,
    row_ld: RLD,
}

impl<
        CCLK: OutputPin<Error = Infallible>,
        CSER: OutputPin<Error = Infallible>,
        CRCLK: OutputPin<Error = Infallible>,
        RCLK: OutputPin<Error = Infallible>,
        RSER: InputPin<Error = Infallible>,
        RLD: OutputPin<Error = Infallible>,
    > Keyboard<CCLK, CSER, CRCLK, RCLK, RSER, RLD>
{
    pub fn new(
        timer: Timer,
        col_clk: CCLK,
        col_ser: CSER,
        col_rclk: CRCLK,
        row_clk: RCLK,
        row_ser: RSER,
        row_ld: RLD,
    ) -> Self {
        Self {
            timer,

            col_clk,
            col_ser,
            col_rclk,

            row_clk,
            row_ser,
            row_ld,
        }
    }

    pub fn read_state(&mut self) -> u64 {
        let mut out = 0;

        for x in 0..6 {
            // +2 gets us to QF (col 6)
            self.write_cols(1 << (x + 2));

            self.timer.delay_ms(1);

            out |= u64::from(read_rows(
                &mut self.timer,
                &mut self.row_clk,
                &mut self.row_ser,
                &mut self.row_ld,
            )) << (x * 8);
        }

        out
    }

    // 1s in val are low
    // 0s are high
    // indexes from furthest pin (QH on second shift register)
    fn write_cols(&mut self, val: u16) {
        for i in 0..16 {
            self.col_ser
                .set_state(((val & (1 << i)) == 0).into())
                .unwrap();

            self.col_clk.set_low().unwrap();
            self.col_clk.set_high().unwrap();
        }

        self.col_rclk.set_low().unwrap();
        self.col_rclk.set_high().unwrap();
    }
}

fn read_rows<
    RCLK: OutputPin<Error = Infallible>,
    RSER: InputPin<Error = Infallible>,
    RLD: OutputPin<Error = Infallible>,
>(
    timer: &mut Timer,
    row_clk: &mut RCLK,
    row_ser: &mut RSER,
    row_ld: &mut RLD,
) -> u8 {
    row_ld.set_low().unwrap();
    row_ld.set_high().unwrap();

    let mut out = 0;
    for i in 0..8 {
        timer.delay_us(10);

        if row_ser.is_low().unwrap() {
            out |= 1 << i;
        }

        row_clk.set_low().unwrap();
        row_clk.set_high().unwrap();
    }

    out
}