#include "kbd.h"
#include <stdint.h>

#include <drivers/ps2/ps2.h>

#include <lib/print.h>
#include <lib/ringbuf.h>

#include <vfs.h>
#include <scheduler.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);

void ps2_task();

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();
}

static 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;
}

static ring_buf_t scancode_buf;
static void push_scancode(uint8_t scancode) {
    ring_buf_write(&scancode_buf, scancode);
}

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

    ps2_flush_output_buffer();

    if (perform_self_test() != SELF_TEST_SUCCESS) {
        return 0;
    }

    uint8_t ccb = read_ccb();
    ccb &= ~(0x01 | 0x02); // Disable interrupts
    ccb |= (1 << 6);       // Enable translation
    write_ccb(ccb);

    if (test_port_1() != PORT_TEST_SUCCESS) {
        return 0;
    }

    enable_port_1();

    ccb = read_ccb();
    ccb |= 0x01;
    write_ccb(ccb);

    ps2_flush_output_buffer();

    // start the tasklet
    ring_buf_init(&scancode_buf);
    sched_create_ktask(ps2_task);

    return 1;
}

void ps2_task() {
    kprintf("started ps2 task\n");
    while (1) {
        char sc;
        ring_buf_read(&scancode_buf, &sc);
        kprintf("ps2 read\n");
        if (sc == -1) yield(); continue;
        kprintf("pushing char\n");
        kbd_push(ps2_get_ascii(sc));
    }
}

void ps2_handle_scancode(uint8_t scancode) {
    push_scancode(scancode);
}