//! Blinks the LED on a Pico board
//!
//! This will blink an LED attached to GP25, which is the pin the Pico uses for the on-board LED.
#![no_std]
#![no_main]

use bsp::entry;
use defmt::*;
use defmt_rtt as _;
use embedded_hal::digital::v2::OutputPin;
use num_traits::{Float, FloatConst};
use panic_probe as _;

// Provide an alias for our BSP so we can switch targets quickly.
// Uncomment the BSP you included in Cargo.toml, the rest of the code does not need to change.
use rp_pico as bsp;
// use sparkfun_pro_micro_rp2040 as bsp;

use bsp::hal::{
    clocks::{init_clocks_and_plls, Clock},
    pac,
    sio::Sio,
    watchdog::Watchdog,
};

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

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

fn write_display<
    ColClockPin,
    ColDataPin,
    RowClockPin,
    RowDataPin,
    StrobePin,
    WhiteOEPin,
    BlackOEPin,
>(
    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,
    ColDataPin: OutputPin,
    RowClockPin: OutputPin,
    RowDataPin: OutputPin,
    StrobePin: OutputPin,
    WhiteOEPin: OutputPin,
    BlackOEPin: OutputPin,
{
    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();
        delay.delay_us(1);
        strobe_pin.set_low();

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

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

#[entry]
fn main() -> ! {
    info!("Program start");
    let mut pac = pac::Peripherals::take().unwrap();
    let core = pac::CorePeripherals::take().unwrap();
    let mut watchdog = Watchdog::new(pac.WATCHDOG);
    let sio = Sio::new(pac.SIO);

    // External high-speed crystal on the pico board is 12Mhz
    let external_xtal_freq_hz = 12_000_000u32;
    let clocks = init_clocks_and_plls(
        external_xtal_freq_hz,
        pac.XOSC,
        pac.CLOCKS,
        pac.PLL_SYS,
        pac.PLL_USB,
        &mut pac.RESETS,
        &mut watchdog,
    )
    .ok()
    .unwrap();

    let mut delay = cortex_m::delay::Delay::new(core.SYST, clocks.system_clock.freq().to_Hz());

    let pins = bsp::Pins::new(
        pac.IO_BANK0,
        pac.PADS_BANK0,
        sio.gpio_bank0,
        &mut pac.RESETS,
    );

    let mut led_pin = pins.led.into_push_pull_output();

    let mut row_data_pin = pins.gpio4.into_push_pull_output();
    let mut row_clock_pin = pins.gpio5.into_push_pull_output();

    let mut col_data_pin = pins.gpio2.into_push_pull_output();
    let mut col_clock_pin = pins.gpio3.into_push_pull_output();

    let mut white_pin = pins.gpio6.into_push_pull_output();
    let mut black_pin = pins.gpio7.into_push_pull_output();
    let mut strobe_pin = pins.gpio8.into_push_pull_output();

    write_display(
        &[0; 16],
        &mut col_clock_pin,
        &mut col_data_pin,
        &mut row_clock_pin,
        &mut row_data_pin,
        &mut strobe_pin,
        &mut white_pin,
        &mut black_pin,
        &mut delay,
    );

    let pattern1 = [
        0b1010_1010_1010_1010_1010,
        0b0101_0101_0101_0101_0101,
        0b1010_1010_1010_1010_1010,
        0b0101_0101_0101_0101_0101,
        0b1010_1010_1010_1010_1010,
        0b0101_0100_0000_0101_0101,
        0b1010_1010_1111_0010_1010,
        0b0101_0100_0001_0101_0101,
        0b1010_1010_0001_0010_1010,
        0b0101_0100_1001_0101_0101,
        0b1010_1010_0000_0010_1010,
        0b0101_0101_0101_0101_0101,
        0b1010_1010_1010_1010_1010,
        0b0101_0101_0101_0101_0101,
        0b1010_1010_1010_1010_1010,
        0b0101_0101_0101_0101_0101,
    ];

    let pattern2 = [
        0b0101_0101_0101_0101_0101,
        0b1010_1010_1010_1010_1010,
        0b0101_0101_0101_0101_0101,
        0b1010_1010_1010_1010_1010,
        0b0101_0101_0101_0101_0101,
        0b1010_1010_0000_0010_1010,
        0b0101_0100_1111_0101_0101,
        0b1010_1010_0001_0010_1010,
        0b0101_0100_0001_0101_0101,
        0b1010_1010_1001_0010_1010,
        0b0101_0100_0000_0101_0101,
        0b1010_1010_1010_1010_1010,
        0b0101_0101_0101_0101_0101,
        0b1010_1010_1010_1010_1010,
        0b0101_0101_0101_0101_0101,
        0b1010_1010_1010_1010_1010,
    ];

    loop {
        led_pin.set_low().unwrap();
        write_display(
            &pattern1,
            &mut col_clock_pin,
            &mut col_data_pin,
            &mut row_clock_pin,
            &mut row_data_pin,
            &mut strobe_pin,
            &mut white_pin,
            &mut black_pin,
            &mut delay,
        );
        delay.delay_ms(1000);

        led_pin.set_high().unwrap();
        write_display(
            &pattern2,
            &mut col_clock_pin,
            &mut col_data_pin,
            &mut row_clock_pin,
            &mut row_data_pin,
            &mut strobe_pin,
            &mut white_pin,
            &mut black_pin,
            &mut delay,
        );
        delay.delay_ms(1000);
    }
}

// End of file