#![no_std]
#![no_main]

use embedded_hal::digital::v2::{OutputPin, InputPin};

use embedded_hal::blocking::i2c::Write;

use hal::gpio::bank0::{Gpio0, Gpio1};
use hal::gpio::PullDown;
use rp2040_hal as hal;
use hal::{
    pac as pac, 
    gpio::{FunctionI2C, Pin},
    fugit::RateExtU32,
};

use panic_probe as _;

mod sprite;
mod dpad;

use crate::dpad::Dpad;
use crate::sprite::*;

#[link_section = ".boot_loader"]
#[used]
pub static BOOT_LOADER: [u8; 256] = rp2040_boot2::BOOT_LOADER_W25Q080;

#[repr(u8)]
#[derive(PartialEq, Copy, Clone)]
enum SH1106Cmd {
    SetColumnLo = 0,
    SetColumnHi = 0x10,
    DisplayInvert = 0xA6,
    DisplayOnOff = 0xAE,
    ColDirectionNorm = 0xC0,
    ColDirectionRev = 0xC8,
    SetPage = 0xB0,
}

const SH1106_ADDR:u8 = 0x78u8 >> 1;

#[rp2040_hal::entry]
fn main() -> ! {
    // Grab our singleton objects
    let mut pac = pac::Peripherals::take().unwrap();
    let core = pac::CorePeripherals::take().unwrap();

    // Set up the watchdog driver - needed by the clock setup code
    let mut watchdog = hal::Watchdog::new(pac.WATCHDOG);

    const XTAL_FREQ_HZ: u32 = 12_000_000u32;

    // Configure the clocks
    let clocks = hal::clocks::init_clocks_and_plls(
        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, 133_000_000u32);

    let sio = hal::Sio::new(pac.SIO);

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


    // Configure two pins as being I²C, not GPIO
    // let sda_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio0.reconfigure();
    // let scl_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio1.reconfigure();
    // let not_an_scl_pin: Pin<_, FunctionI2C, PullUp> = pins.gpio20.reconfigure();

    // Create the I²C drive, using the two pre-configured pins. This will fail
    // at compile time if the pins are in the wrong mode, or if this I²C
    // peripheral isn't available on these pins!
    let mut i2c = hal::I2C::i2c0(
        pac.I2C0,
        pins.gpio0.into_function(),
        pins.gpio1.into_function(), // Try `not_an_scl_pin` here
        400.kHz(),
        &mut pac.RESETS,
        &clocks.system_clock,
    );

    let mut led_pin = pins.gpio25.into_push_pull_output();
    led_pin.set_high().unwrap();
    
    send_command(&mut i2c, &SH1106Cmd::DisplayOnOff, 0x00);
    delay.delay_ms(100);
    send_command(&mut i2c, &SH1106Cmd::DisplayInvert, 0);
    send_command(&mut i2c, &SH1106Cmd::ColDirectionNorm, 0);

    set_col(&mut i2c, 2);
    set_page(&mut i2c, 0);
    clr_screen(&mut i2c);
    delay.delay_ms(500);
    send_command(&mut i2c, &SH1106Cmd::DisplayOnOff, 0x01);

    let mut gfx_buf = [0u8; 8192];

    // gfx_buf.copy_from_slice(&TILES[0..8192]);



    let sprite_atlas = SpriteAtlas::new(include_bytes!("../sprites/sprites_atlas.bin"));

    let mut sprites = [Sprite::new(); 30];

    // for (i, sprite) in sprites.iter_mut().enumerate() {
    //     sprite.sprite_index = i+30;

    //     let i = i as i32;
    //     sprite.x = (i*10) % 128;
    //     sprite.y = (i*10) % 64;
    //     sprite.x_vel = i % 3 - 2;
    //     sprite.y_vel = i % 4 - 1;
    // }

    sprites[0].sprite_index = 94;
    sprites[0].x = 90;
    sprites[0].y = 30;
    sprites[0].visible = true;

    let dpad = Dpad::new([
        pins.gpio2.into_pull_up_input().into_dyn_pin(),
        pins.gpio3.into_pull_up_input().into_dyn_pin(),
        pins.gpio4.into_pull_up_input().into_dyn_pin(),
        pins.gpio5.into_pull_up_input().into_dyn_pin(),
    ]);

    // let dpad_down = pins.gpio2.into_pull_up_input();
    // let dpad_left = pins.gpio3.into_pull_up_input();
    // let dpad_up = pins.gpio4.into_pull_up_input();
    // let dpad_right = pins.gpio5.into_pull_up_input();

    loop {

        gfx_buf.iter_mut().for_each(|x| *x = 0);

        sprites.iter_mut().for_each(|s| {
            if s.visible {
                sprite_atlas.draw_sprite(s.sprite_index, s.x as u8, s.y as u8, s.flip, &mut gfx_buf);
            }

            let dpad_state = dpad.get_state();

            if dpad_state.up {
                s.y -= 1;
            }
            if dpad_state.down {
                s.y += 1;
            }
            if dpad_state.left {
                s.x -= 1;
                s.flip = SpriteFlip::None;
            }
            if dpad_state.right {
                s.x += 1;
                s.flip = SpriteFlip::Horizontal;
            }

        });

        sprite_atlas.draw_string(10, 10, "Hello World!", &mut gfx_buf);

        sprite_atlas.draw_textfield(10, 20, 110, 10, "I think this font has a *lot* of 'character'! ;)", &mut gfx_buf);

        blit_framebuffer(&mut i2c, &gfx_buf);

        delay.delay_ms(1);
    }
}

fn draw_bresenham_line(
    x0: u8, y0: u8,
    x1: u8, y1: u8,
    gfx_buf: &mut [u8; 8192],
) {
    let mut dx = x1 as i32 - x0 as i32;
    let mut dy = y1 as i32 - y0 as i32;

    let mut x = x0 as i32;
    let mut y = y0 as i32;

    let mut err = 0;

    let mut step_x = 1;
    let mut step_y = 1;

    if dx < 0 {
        dx = -dx;
        step_x = -1;
    }

    if dy < 0 {
        dy = -dy;
        step_y = -1;
    }

    if dx > dy {
        for _ in 0..dx {
            gfx_buf[(y*128+x) as usize] = 1;

            x += step_x;
            err += 2 * dy;

            if err > dx {
                y += step_y;
                err -= 2 * dx;
            }
        }
    } else {
        for _ in 0..dy {
            gfx_buf[(y*128+x) as usize] = 1;

            y += step_y;
            err += 2 * dx;

            if err > dy {
                x += step_x;
                err -= 2 * dy;
            }
        }
    }

}



fn blit_framebuffer(
    i2c: &mut hal::I2C<pac::I2C0, (Pin<Gpio0, FunctionI2C, PullDown>, Pin<Gpio1, FunctionI2C, PullDown>)>,
    graphic: &[u8; 8192]) {

        // for each row of 8x8 pixels
        for r in 0..8 {
            set_page(i2c, r);
            set_col(i2c, 2);
            // for each cel of 8x8 pixels
            for c in 0..16 {
                let mut cel = [0u8; 8];

                // so our x,y here would be r * 128 + c * 8

                // start assembling the 8x8 cel
                for x in 0..8 {
                    let mut col_byte = 0u8;

                    // for each row of the cel
                    for y in 0..8 {
                        let pix_x = (c as usize) * 8 + x;
                        let pix_y = (r as usize) * 8 + y;
                        col_byte |= graphic[pix_y*128 + pix_x] << y;
                    }

                    cel[x] = col_byte;
                }

                write_cel_pixels(i2c, &cel);        
            }

        }
    
}

// as our display is in 8 pages of 128 columns, we need to transform the graphic
// to match the display's orientation
// fn transform_hcompress_graphic(graphic: [u8; 1024]) -> [u8; 1024] {

//     let mut cel:[u8;8];
//     let mut transformed_graphic = [0u8;1024];

//     for row in 0..8 {
//         for x in 0..16 {
//             cel = [0u8; 8];
//             for y in 0..8 {
//                 let byte = graphic[(row*8+y)*16+x];

//                 for i in 0..8 {
//                     cel[i] |= ((byte & (0b1000_0000 >> i)) << i) >> y;
//                 }
//             }

//             transformed_graphic[row*128+x*8..row*128+x*8+8].copy_from_slice(&cel[0..8]);
//             // transformed_graphic[x*8..x*8+8].copy_from_slice(&cel[0..8]);
//         }
//     }

//     transformed_graphic
// }

fn set_col(i2c: &mut hal::I2C<pac::I2C0, (Pin<Gpio0, FunctionI2C, PullDown>, Pin<Gpio1, FunctionI2C, PullDown>)>,
            col: u8) {
    let col_lo = col & 0b0000_1111;
    let col_hi = (col & 0b1111_0000) >> 4;

    send_command(i2c, &SH1106Cmd::SetColumnLo, col_lo);
    send_command(i2c, &SH1106Cmd::SetColumnHi, col_hi);
}

fn set_page(i2c: &mut hal::I2C<pac::I2C0, (Pin<Gpio0, FunctionI2C, PullDown>, Pin<Gpio1, FunctionI2C, PullDown>)>,
            page: u8) {
    send_command(i2c, &SH1106Cmd::SetPage, page & 0b0000_0111);
}

fn clr_screen(i2c: &mut hal::I2C<pac::I2C0, (Pin<Gpio0, FunctionI2C, PullDown>, Pin<Gpio1, FunctionI2C, PullDown>)>) {
    let mut writebuf = [0u8; 132*8+1];

    writebuf[0] = 0b0100_0000;

    for i in 0..8 {
        match i2c.write(SH1106_ADDR, &writebuf) {
            _ => ()
        };

        set_page(i2c, i);
    }
}


fn write_cel_pixels(
    i2c: &mut hal::I2C<pac::I2C0, (Pin<Gpio0, FunctionI2C, PullDown>, Pin<Gpio1, FunctionI2C, PullDown>)>,
    data: &[u8]) {

    let mut writebuf = [0b0100_0000u8; 9];

    writebuf[0] = 0b0100_0000;
    writebuf[1..].copy_from_slice(&data[0..8]);

    match i2c.write(SH1106_ADDR, &writebuf) {
        _ => ()
    };
}


fn send_command(i2c: &mut hal::I2C<pac::I2C0, (Pin<Gpio0, FunctionI2C, PullDown>, Pin<Gpio1, FunctionI2C, PullDown>)>,
                command: &SH1106Cmd,
                data:u8) {
    let mut writebuf: [u8; 2] = [0; 2];

    writebuf[0] = 0b1000_0000;
    writebuf[1] = (command.clone() as u8) | data;

    match i2c.write(SH1106_ADDR, &writebuf) {
        _ => ()
    };
}

// static GRAPHIC:[u8;8192] = *include_bytes!("../image.bin");
// static TILES_COMPRESSED:[u8;12800] = *include_bytes!("../kenney_comp.bin");
// static TILES:[u8;102400] = *include_bytes!("../kenney_mono.bin");