#[cfg(feature = "ps2")]
use crate::ps2::PS2;
use crate::{keyboard_layouts::SCANCODE_LOOKUP, switch::Switch};
use bitvec::prelude::*;
use core::convert::Infallible;
use embedded_hal::digital::v2::{InputPin, OutputPin, StatefulOutputPin};
use ringbuffer::{ConstGenericRingBuffer, RingBuffer, RingBufferRead, RingBufferWrite};

#[allow(clippy::module_name_repetitions)]
#[cfg(feature = "ps2")]
pub struct PS2Keyboard<M, const MC: usize, Ps2Data, Ps2Clock>
where
    M: ScanableMatrix,
    Ps2Data: InputPin<Error = Infallible> + StatefulOutputPin<Error = Infallible>,
    Ps2Clock: InputPin<Error = Infallible> + StatefulOutputPin<Error = Infallible>,
{
    matricies: [M; MC],
    ps2_interface: PS2<Ps2Data, Ps2Clock>,
    key_buffer: [Switch<5>; 96],
    scancode_buffer: ConstGenericRingBuffer<u8, 32>,
    command_buffer: ConstGenericRingBuffer<u8, 32>,
    enabled_scanning: bool,
}

#[cfg(feature = "ps2")]
impl<M, const MC: usize, Ps2Data, Ps2Clock> PS2Keyboard<M, MC, Ps2Data, Ps2Clock>
where
    M: ScanableMatrix,
    Ps2Data: InputPin<Error = Infallible> + StatefulOutputPin<Error = Infallible>,
    Ps2Clock: InputPin<Error = Infallible> + StatefulOutputPin<Error = Infallible>,
{
    pub fn new(matricies: [M; MC], ps2_data: Ps2Data, ps2_clock: Ps2Clock) -> Self {
        let mut kb = Self {
            matricies,
            key_buffer: [Switch::<5>::new(); 96],
            scancode_buffer: ConstGenericRingBuffer::new(),
            command_buffer: ConstGenericRingBuffer::new(),
            ps2_interface: PS2::new(ps2_data, ps2_clock),
            enabled_scanning: false,
        };
        kb.scancode_buffer.push(0xAA);
        kb
    }
    pub fn scan(&mut self) {
        for matrix in &mut self.matricies {
            matrix.scan(&mut self.key_buffer);
        }
    }
    fn send_ack(&mut self) {
        self.scancode_buffer.push(0xFA);
    }
    #[allow(clippy::unwrap_used, clippy::pedantic)]
    pub fn process_keystrokes(&mut self) {
        if !self.command_buffer.is_empty() {
            let command = self.command_buffer.dequeue().unwrap();
            match command {
                0xFF => {
                    self.send_ack();
                    self.scancode_buffer.push(0xAA);
                }
                0xFE => {
                    self.send_ack();
                }
                0xF5 => {
                    self.enabled_scanning = false;
                    self.send_ack();
                }
                0xF4 => {
                    self.enabled_scanning = true;
                    self.send_ack();
                }
                0xF3 => {
                    self.send_ack();
                }
                0xF2 => {
                    self.send_ack();
                    self.scancode_buffer.push(0xAB);
                    self.scancode_buffer.push(0x83);
                }
                0xF0 => {
                    self.send_ack();
                }
                0xEE => {
                    self.scancode_buffer.push(0xEE);
                }
                0xED => {
                    self.send_ack();
                }
                _ => {
                    self.send_ack();
                }
            }
        }
        //        if self.enabled_scanning {
        for (key, val) in self.key_buffer.iter().enumerate() {
            // read change bit
            if val.changed() {
                if key >= SCANCODE_LOOKUP.len() {
                    continue;
                }
                if val.is_down() {
                    for code in SCANCODE_LOOKUP[key] {
                        self.scancode_buffer.push(*code);
                    }
                } else if SCANCODE_LOOKUP[key].len() == 1 {
                    self.scancode_buffer.push(0xF0); //break code
                    self.scancode_buffer.push(SCANCODE_LOOKUP[key][0]);
                } else {
                    //Extended code
                    self.scancode_buffer.push(0xE0);
                    self.scancode_buffer.push(0xF0);
                    for code in &SCANCODE_LOOKUP[key][1..] {
                        self.scancode_buffer.push(*code);
                    }
                }

                // val.set(1, false);
            }
            //          }
        }
    }
    pub fn update_interface(&mut self) {
        self.ps2_interface
            .update(&mut self.scancode_buffer, &mut self.command_buffer);
    }
}

#[macro_export]
macro_rules! pp_output {
    ($gpio:expr,$($pin:expr),+) => {
        [$($pin.into_push_pull_output(&mut $gpio.crl).erase()),+]
    };
}
#[macro_export]
macro_rules! pd_input {
    ($gpio:expr,$($pin:expr),+) => {
        [$($pin.into_pull_down_input(&mut $gpio.crh).erase()),+]
    };
}

pub trait ScanableMatrix {
    fn scan<const T: u8>(&mut self, keybuffer: &mut [Switch<T>]);
}

pub struct KeyMatrix<A, D, const AC: usize, const DC: usize>
where
    A: OutputPin,
    D: InputPin,
{
    address: [A; AC],
    data: [D; DC],
    offset: u8,
}

impl<A, D, const AC: usize, const DC: usize> KeyMatrix<A, D, AC, DC>
where
    A: OutputPin,
    D: InputPin,
{
    pub const fn new(address_pins: [A; AC], data_pins: [D; DC], offset: u8) -> Self {
        Self {
            address: address_pins,
            data: data_pins,
            offset,
        }
    }
}
impl<A, D, const AC: usize, const DC: usize> ScanableMatrix for KeyMatrix<A, D, AC, DC>
where
    A: OutputPin,
    D: InputPin,
{
    #[allow(clippy::unwrap_used)]
    fn scan<const T: u8>(&mut self, keybuffer: &mut [Switch<T>]) {
        #[cfg(debug_assertions)]
        {
            assert!(
                keybuffer.len() > (self.offset as usize + self.address.len() * self.data.len())
            );
        }
        let address_len = self.address.len();
        for (i, addr) in self.address.iter_mut().enumerate() {
            let _ = addr.set_high();
            for (j, dat) in self.data.iter().enumerate() {
                match dat.is_high() {
                    Err(_) => {}
                    Ok(down) => {
                        let key = self.offset as usize + i * address_len + j;
                        keybuffer[key].set(down);
                    }
                }
            }
            let _ = addr.set_low();
        }
    }
}
