#include <stdint.h>
#include <drivers/ps2/ps2.h>

extern void ps2_wait_input_empty(void);
extern void ps2_wait_output_full(void);
extern void ps2_write_command(uint8_t);
extern uint8_t ps2_read_data(void);
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;
}