use embassy_embedded_hal::shared_bus::blocking::i2c::I2cDevice;
use embassy_stm32::{i2c::I2c, mode::Blocking};
use embassy_sync::blocking_mutex::raw::NoopRawMutex;

use embedded_hal_1::i2c::I2c as _;

#[derive(Copy, Clone)]
#[allow(dead_code)]
pub enum SetX {
    X0 = 0b0000,
    X1,
    X2,
    X3,
    X4,
    X5,
    X6 = 0b1000,
    X7,
    X8,
    X9,
    X10,
    X11,
}

impl SetX{
    pub fn from_u8(value: u8) -> Self {
        const VARIANTS: [SetX; 12] = [
            SetX::X0, SetX::X1, SetX::X2, SetX::X3,
            SetX::X4, SetX::X5, SetX::X6, SetX::X7,
            SetX::X8, SetX::X9, SetX::X10, SetX::X11,
        ];

        *VARIANTS.get(value as usize).expect("Invalid value for SetX")
    }
}

#[derive(Copy, Clone)]
#[allow(dead_code)]
pub enum SetY {
    Y0 = 0b000,
    Y1,
    Y2,
    Y3,
    Y4,
    Y5,
    Y6,
    Y7,
}

impl SetY {
    pub fn from_u8(value: u8) -> Self {
        const VARIANTS: [SetY; 8] = [
            SetY::Y0, SetY::Y1, SetY::Y2, SetY::Y3,
            SetY::Y4, SetY::Y5, SetY::Y6, SetY::Y7,
        ];

        *VARIANTS.get(value as usize).expect("Invalid value for SetY")
    }
}

#[derive(Copy, Clone)]
#[allow(dead_code)]
pub enum GetX {
    X0  = 0b0011_0100,
    X1  = 0b0011_1100,
    X2  = 0b0111_0100,
    X3  = 0b0111_1100,
    X4  = 0b0011_0101,
    X5  = 0b0011_1101,
    X6  = 0b0111_0101,
    X7  = 0b0111_1101,
    X8  = 0b0011_0110,
    X9  = 0b0011_1110,
    X10 = 0b0111_0110,
    X11 = 0b0111_1110,
}

impl GetX {
    pub fn from_u8(value: u8) -> Self {
        const VARIANTS: [GetX; 12] = [
            GetX::X0, GetX::X1, GetX::X2, GetX::X3,
            GetX::X4, GetX::X5, GetX::X6, GetX::X7,
            GetX::X8, GetX::X9, GetX::X10, GetX::X11,
        ];

        *VARIANTS.get(value as usize).expect("Invalid value for GetX")
    }
}

#[allow(dead_code)]
pub enum State {
    DISABLED = 0b0,
    ENABLED  = 0b1,
}

#[allow(dead_code)]
enum LDSW {
    EN = 0b1,
    DIS = 0b0,
}

pub struct ADG2128 {
    i2c: I2cDevice<'static, NoopRawMutex, I2c<'static, Blocking>>,
    address: u8,
    states_xy: [u8; 12]
}

impl ADG2128 {
    pub fn new(i2c: I2cDevice<'static, NoopRawMutex, I2c<'static, Blocking>>, address: u8) -> Self {
        ADG2128 { i2c, address, states_xy: [0u8; 12]}
    }

    pub fn set(&mut self, io_1: SetX, io_2: SetY, state: State) {
        let mut data = [0u8; 2];
        data[0] = (state as u8) << 7 | (io_1 as u8) << 3 | (io_2 as u8);
        data[1] = LDSW::EN as u8;
        self.i2c.write(self.address, &data).unwrap()
    }

    pub fn get(&mut self, io: GetX) -> u8 {
        let mut data_out = [0u8; 2];
        let mut data_in = [0u8; 2];
        data_out[0] = io as u8;
        self.i2c.write_read(self.address, &mut data_out, &mut data_in).unwrap();
        
        data_in[1]
    }

    pub fn get_all(&mut self) -> [u8; 12] {
        let mut states_xy = [0u8; 12]; 

        for (x_line, y_states) in states_xy.iter_mut().enumerate() {
            let x_line_data = GetX::from_u8(x_line as u8);
            *y_states = self.get(x_line_data);
        }

        self.states_xy.copy_from_slice(states_xy.as_slice());

        states_xy
    }

    pub fn reset_all(&mut self) {
        let states_xy = self.get_all();

        // Find bits that are set
        for (x_line, &state) in states_xy.iter().enumerate() {
            if state == 0 {
                continue; // Skip zero states early
            }
            for value in 0..8 {
                if (state >> value) & 1 == 1 {
                    self.set(
                        SetX::from_u8(x_line as u8),
                        SetY::from_u8(value),
                        State::DISABLED,
                    );
                }
            }
        }
    }
}