use embedded_hal::digital::v2::{InputPin, OutputPin};
use rp2040_hal::gpio::{DynPinId, FunctionSio, Pin, PullUp, SioInput};
use shift_register_driver::sipo::ShiftRegisterPin;

pub struct Keyboard {
    cols: [ShiftRegisterPin<'static>; 7],
    rows: [Pin<DynPinId, FunctionSio<SioInput>, PullUp>; 7],
}

impl Keyboard {
    pub fn new(
        mut cols: [ShiftRegisterPin<'static>; 7],
        rows: [Pin<DynPinId, FunctionSio<SioInput>, PullUp>; 7],
    ) -> Self {
        for c in &mut cols {
            c.set_high().unwrap();
        }

        Self { cols, rows }
    }

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

        for (x, c) in self.cols.iter_mut().enumerate() {
            c.set_low().unwrap();
            // TODO we might need a delay in here

            for (y, r) in self.rows.iter_mut().enumerate() {
                if r.is_low().unwrap() {
                    out |= 1 << (x * 7 + y);
                }
            }

            c.set_high().unwrap();
        }

        out
    }
}