use core::fmt::Debug;
use embedded_hal::digital::v2::OutputPin;

fn toggle_clock<ClockPin, Error>(clock_pin: &mut ClockPin, delay: &mut cortex_m::delay::Delay)
where
    ClockPin: OutputPin<Error = Error>,
    Error: Debug,
{
    clock_pin.set_high().unwrap();
    delay.delay_us(1);
    clock_pin.set_low().unwrap();
    delay.delay_us(1);
}

fn shift_out_bits<ClockPin, DataPin, Error>(
    data: u32,
    len: u8,
    clock_pin: &mut ClockPin,
    data_pin: &mut DataPin,
    delay: &mut cortex_m::delay::Delay,
) where
    ClockPin: OutputPin<Error = Error>,
    DataPin: OutputPin<Error = Error>,
    Error: Debug,
{
    for i in 0..len {
        if data & (1 << (len - i - 1) as u32) != 0 {
            data_pin.set_high().unwrap();
        } else {
            data_pin.set_low().unwrap();
        }
        toggle_clock(clock_pin, delay);
    }
}

pub fn write_display<
    ColClockPin,
    ColDataPin,
    RowClockPin,
    RowDataPin,
    StrobePin,
    WhiteOEPin,
    BlackOEPin,
    Error,
>(
    pixels: &[u32; 16],
    col_clock_pin: &mut ColClockPin,
    col_data_pin: &mut ColDataPin,
    row_clock_pin: &mut RowClockPin,
    row_data_pin: &mut RowDataPin,
    strobe_pin: &mut StrobePin,
    white_oe_pin: &mut WhiteOEPin,
    black_oe_pin: &mut BlackOEPin,
    delay: &mut cortex_m::delay::Delay,
) where
    ColClockPin: OutputPin<Error = Error>,
    ColDataPin: OutputPin<Error = Error>,
    RowClockPin: OutputPin<Error = Error>,
    RowDataPin: OutputPin<Error = Error>,
    StrobePin: OutputPin<Error = Error>,
    WhiteOEPin: OutputPin<Error = Error>,
    BlackOEPin: OutputPin<Error = Error>,
    Error: Debug,
{
    for y in 0..16 {
        shift_out_bits(pixels[y], 24, col_clock_pin, col_data_pin, delay);

        shift_out_bits(1 << y, 16, row_clock_pin, row_data_pin, delay);

        strobe_pin.set_high().unwrap();
        delay.delay_us(1);
        strobe_pin.set_low().unwrap();

        white_oe_pin.set_high().unwrap();
        delay.delay_ms(10);
        white_oe_pin.set_low().unwrap();

        black_oe_pin.set_high().unwrap();
        delay.delay_ms(10);
        black_oe_pin.set_low().unwrap();
    }
}