use core::fmt::{self, Arguments};

use crate::stdlib;

const KEYCODE_MAP: [u8; 81] = [
    // Row 1
    53,  //Grave
    30,  //Keyboard1
    32,  //Keyboard3
    34,  //Keyboard5
    36,  //Keyboard7
    38,  //Keyboard9
    45,  //Minus
    42,  //DeleteBackspace
    120, //Stop (RUN/STOP)
    // Row 2
    41, //Escape
    31, //Keyboard2
    33, //Keyboard4
    35, //Keyboard6
    37, //Keyboard8
    39, //Keyboard0
    46, //Equal
    73, //Insert
    75, //PageUp
    // Row 3
    68, //F11, // Macro 1
    26, //W
    21, //R
    28, //Y
    12, //I
    19, //P
    48, //RightBrace
    74, //Home
    77, //End
    // Row 4
    43, //Tab
    20, //Q
    8,  //E
    23, //T
    24, //U
    18, //O
    47, //LeftBrace
    76, //DeleteForward
    78, //PageDown
    // Row 5
    69,  //F12 (Macro 2)
    4,   //A
    7,   //D
    9,   //F
    13,  //J
    15,  //L
    52,  //Apostrophe
    49,  //Backslash
    106, //F15 (Cyan)
    // Row 6
    224, //LeftControl
    225, //LeftShift
    22,  //S
    10,  //G
    14,  //K
    51,  //Semicolon
    56,  //ForwardSlash
    40,  //ReturnEnter
    107, //F16 (Magenta)
    // Row 7
    104, //F13 (Macro 3)
    29,  //Z
    27,  //X
    11,  //H
    16,  //M
    55,  //Dot
    229, //RightShift
    82,  //UpArrow
    108, //F17 (Yellow)
    // Row 8
    105, //F14 (Macro 4)
    224, //LeftControl (Meta Left)
    6,   //C
    5,   //B
    17,  //N
    54,  //Comma
    228, //RightControl (Meta Down)
    80,  //LeftArrow
    79,  //RightArrow
    // Row 9
    0,   //NoEventIndicated (Func; should never be sent)
    227, //LeftGUI (Meta)
    226, //LeftAlt (Meta Right)
    25,  //V
    44,  //Space
    230, //RightAlt (Meta Up)
    231, //RightGUI (Compose)
    81,  //DownArrow
    109, //F18 (Black)
];

fn print(buf: &mut heapless::String<80>, args: Arguments<'_>) {
    buf.clear();
    if fmt::write(buf, args).is_ok() {
        stdlib::print(buf);
    } else {
        stdlib::print("blonk");
    }
}

pub fn main(_base: u32, _size: u32) -> ! {
    let mut fmt_buffer: heapless::String<80> = heapless::String::new();
    let mut key_state = [0u32; 3];
    let mut new_key_state = [0u32; 3];
    let mut pressed = [0u32; 3];
    let mut released = [0u32; 3];
    let mut modifier = 0u8;
    let mut keycodes = [0u8; 6];

    loop {
        stdlib::kb_read_matrix(&mut new_key_state);
        for i in 0..3 {
            pressed[i] = new_key_state[i] & !key_state[i];
            released[i] = !new_key_state[i] & key_state[i];
            key_state[i] = new_key_state[i];
        }
        //print(&mut fmt_buffer, format_args!("{:08x?}\r\n", key_state));

        if (pressed[0] | pressed[1] | pressed[2] | released[0] | released[1] | released[2]) == 0 {
            stdlib::sleep(1);
            continue;
        }

        modifier = 0;
        for i in 0..6 {
            keycodes[i] = 0;
        }
        let mut c = 0;
        for i in 0..81 {
            let b = (key_state[i / 32] >> (i % 32) & 1) != 0;
            if b {
                let keycode = KEYCODE_MAP[i];
                if keycode >= 224 && keycode <= 231 {
                    modifier |= 1 << (keycode - 224);
                } else {
                    if c == 6 {
                        keycodes[0] = 1;
                    } else {
                        keycodes[c] = keycode;
                        c += 1;
                    }
                }
                //print(&mut fmt_buffer, format_args!("{} ", KEYCODE_MAP[i] as u32));
            }
        }
        /*
        if keys.len() > 0 {
            stdlib::print("\r\n");
        }
        */

        let mut report_bytes = [0u8; 8];
        report_bytes[0] = modifier;
        for i in 0..6 {
            report_bytes[i + 2] = keycodes[i];
        }
        if !stdlib::kb_write_hid(&report_bytes) {
            stdlib::print("Failed to push report\r\n");
        } else {
            print(&mut fmt_buffer, format_args!("wrote {:?}\r\n", report_bytes));
        }

        stdlib::sleep(10);
    }
}