#include <stdint.h>

#include <drivers/ps2/ps2.h>

#include <vfs.h>

static const char scancode_to_ascii[] = {
    0,  27, '1', '2', '3', '4', '5', '6', '7', '8', '9', '0', '-', '=', '\b',
  '\t', 'q', 'w', 'e', 'r', 't', 'y', 'u', 'i', 'o', 'p', '[', ']', '\n', 
    0,  'a', 's', 'd', 'f', 'g', 'h', 'j', 'k', 'l', ';', '\'', '`',   0, 
  '\\', 'z', 'x', 'c', 'v', 'b', 'n', 'm', ',', '.', '/',   0, '*',   0, ' '
};

extern void ps2_wait_input_empty(void);
extern void ps2_wait_output_full(void);
extern void ps2_write_command(uint8_t);
extern void ps2_write_data(uint8_t);
extern void ps2_flush_output_buffer(void);
extern void io_wait(void);

static uint8_t read_ccb() {
    ps2_write_command(CMD_READ_CCB);
    io_wait();
    return ps2_read_data();
}

static void write_ccb(uint8_t ccb) {
    ps2_write_command(CMD_WRITE_CCB);
    io_wait();
    ps2_write_data(ccb);
    io_wait();
}

static int perform_self_test() {
    ps2_write_command(CMD_SELF_TEST);
    io_wait();
    return ps2_read_data();
}

static int test_port_1() {
    ps2_write_command(CMD_TEST_P1);
    io_wait();
    return ps2_read_data();
}

static void enable_port_1() {
    ps2_write_command(CMD_ENABLE_P1);
    io_wait();
}

int init_ps2() {
    ps2_write_command(CMD_DISABLE_P1);
    ps2_write_command(CMD_DISABLE_P2);

    ps2_flush_output_buffer();

    uint8_t ccb = read_ccb();
    ccb = ccb & 0xEC;
    write_ccb(ccb);

    int result = perform_self_test();
    if (result != SELF_TEST_SUCCESS) {
        return -1;
    }

    result = test_port_1();
    if (result != PORT_TEST_SUCCESS) {
        return -1;
    }

    enable_port_1();

    ccb = read_ccb();
    ccb |= 0x01;    // this enables interrupts for port 1, which we want
    write_ccb(ccb);

    return 0;
}

void ps2_init_kbd(vfs_node_t* dev) {

}

char ps2_get_ascii(uint8_t scancode) {
    if (scancode & 0x80) {
        return 0;
    }

    if (scancode < sizeof(scancode_to_ascii)) {
        char ascii = scancode_to_ascii[scancode];
        if (ascii != 0) {
            return ascii;
        }
    }

    return 0;
}