#include "ps2keyboard.h"
#include "stdio.h"

// status register on command port (read)
#define STATUS_REG_OUTPUT_BUF_FULL 0x01
#define STATUS_REG_INPUT_BUF_FULL 0x02

// generic PS/2 commands
#define COMMAND_GET_STATE 0x20
#define COMMAND_SET_STATE 0x60
#define COMMAND_DISABLE_PORT1 0xAD 
#define COMMAND_ENABLE_PORT1 0xAE 

// status control register flags
#define STATUS_CONTROL_CONFIG_ENABLE_IRQ_PORT1 0x01
#define STATUS_CONTROL_CONFIG_CLOCK_PORT1 0x10

// keyboard specific commands, PSAUX1
#define KBD_COMMAND_SET_LEDS 0xED
#define KBD_SET_SCANCODE 0xF0
#define KBD_ACTIVATE 0xF4

// keyboard LED status codes
#define KBD_LED_SCROLL_LOCK_ON 0x01
#define KBD_LED_NUM_LOCK_ON 0x02
#define KBD_LED_CAPS_LOCK_ON 0x04
#define KBD_LED_ALL_OFF 0x00
#define KBD_LED_ALL_ON KBD_LED_SCROLL_LOCK_ON | KBD_LED_NUM_LOCK_ON | KBD_LED_CAPS_LOCK_ON

// keyboard scan codes
#define KBD_SCANCODE_1 0x01
#define KBD_SCANCODE_2 0x02
#define KBD_SCANCODE_3 0x03

#undef DEBUG

static uint8_t read_data( ps2keyboard_t *ps2keyboard )
{
	while( ( port8_read( &ps2keyboard->command_port ) & STATUS_REG_OUTPUT_BUF_FULL ) == 0 ) { }
	return port8_read( &ps2keyboard->data_port );
}

static void send_command( ps2keyboard_t *ps2keyboard, uint8_t command )
{
	while( port8_read( &ps2keyboard->command_port ) & STATUS_REG_INPUT_BUF_FULL ) { }
	port8_write( &ps2keyboard->command_port, command );
}

static void write_data( ps2keyboard_t *ps2keyboard, uint8_t data )
{
	while( port8_read( &ps2keyboard->command_port ) & STATUS_REG_INPUT_BUF_FULL ) { }
	port8_write( &ps2keyboard->data_port, data );
}

/*
static void read_ack( ps2keyboard_t *ps2keyboard )
{
	uint8_t data = read_data( ps2keyboard );
	if( data == 0xFA ) {
		return;
	} else {
		kernel_warn( "ERR PS/2 keyboard: acknoledgment failed 0x%X\n", data );
	}
}
*/

static keyboard_vtable_t const ps2keyboard_vtable = {
	{
	ps2keyboard_activate,
	ps2keyboard_deactivate,
	ps2keyboard_deinit,
	ps2keyboard_print_info
	}
};

void ps2keyboard_init( ps2keyboard_t *ps2keyboard, keyboard_event_handler_t handler, interrupt_t *interrupt, void *context )
{
	memset( ps2keyboard, 0, sizeof( ps2keyboard_t ) );

	driver_init( (driver_t *)ps2keyboard, DRIVER_TYPE_KEYBOARD, interrupt, context );
	
	port8_init( &ps2keyboard->command_port, 0x64 );
	port8_init( &ps2keyboard->data_port, 0x60 );

	ps2keyboard->shift = false;
	ps2keyboard->base.handler = handler;

	((driver_t *)ps2keyboard)->vtable = (driver_vtable_t *)&ps2keyboard_vtable;
}

void ps2keyboard_activate( void *obj )
{
	puts( "Activating driver for PS/2 keyboard.." );

	driver_t *driver = obj;
	ps2keyboard_t *ps2keyboard = obj;

	// first switch off port 1
	send_command( ps2keyboard, COMMAND_DISABLE_PORT1 );
	
	// consume character pressed before executing the kernel
	while( port8_read( &ps2keyboard->command_port ) & STATUS_REG_OUTPUT_BUF_FULL ) {
		port8_read( &ps2keyboard->data_port );
	}
	
	// enable interrupts for port 1
	send_command( ps2keyboard, COMMAND_GET_STATE );
	uint8_t status = read_data( ps2keyboard );
	status |= STATUS_CONTROL_CONFIG_ENABLE_IRQ_PORT1;
	status &= ~STATUS_CONTROL_CONFIG_CLOCK_PORT1;
	send_command( ps2keyboard, COMMAND_SET_STATE );
	write_data( ps2keyboard, status );
	
	// disable all LEDs on keyboard
	//~ send_command( ps2keyboard, KBD_COMMAND_SET_LEDS );
	//~ write_data( ps2keyboard, KBD_LED_ALL_ON );
	
	// set scan code
	//~ send_command( ps2keyboard, KBD_SET_SCANCODE );
	//~ write_data( ps2keyboard, KBD_SCANCODE_2 );
	
	// activate keyboard
	//send_command( ps2keyboard, KBD_ACTIVATE );
	//read_ack( ps2keyboard );

	interrupt_handler_init( &ps2keyboard->interrupt_handler, IRQ_BASE + 0x01, driver->interrupt, ps2keyboard_handle_interrupt, obj );
	interrupts_register_interrupt_handler( ps2keyboard->interrupt_handler );

	// enable port 1
	send_command( ps2keyboard, COMMAND_ENABLE_PORT1 );
}

void ps2keyboard_deactivate( void *obj )
{
	puts( "Deactivating driver for PS/2 keyboard.." );

	ps2keyboard_t *ps2keyboard = obj;

	send_command( ps2keyboard, COMMAND_DISABLE_PORT1 );	
}

void ps2keyboard_deinit( void *obj )
{
	// nothing to do
}

typedef enum {
	KEYCODE_UNKNOWN,
	KEYCODE_ASCII_CHAR,
	KEYCODE_ESC
} keycode_type_t;

typedef struct {
	keycode_type_t type;
	char c;
} keycode_t;

typedef enum {
	SCANCODE_SET_NORMAL = 0,
	SCANCODE_SET_SHIFT = 1,
	SCANCODE_SET_E0 = 2,
	SCANCODE_SET_E1 = 3
} scancode_set_t;

// TODO: here come the translations to other keyboard layouts,
// assumming a QWERTY US keyboard here..
static char scancode_map[4][128] = {
	{
		0,   0,   '1', '2', '3', '4', '5', '6',
		'7', '8', '9', '0', '-', '=', '\b',   0,
		'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,
		0, ' ', 0, 0, 0, 0, 0, 0,
		0, 0, 0  , 0  , 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0  , 0  , 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0  , 0  , 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0  , 0  , 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0
	},
	{
		0,   0,   '!', '@', '#', '$', '%', '^',
		'&', '*', '(', ')', '_',   '+',   0,   0,
		'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,
		0, ' ', 0, 0, 0, 0, 0, 0,
		0, 0, 0  , 0  , 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0  , 0  , 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0  , 0  , 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0  , 0  , 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0
	},
	{
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0
	},
	{
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0,
		0, 0, 0, 0, 0, 0, 0, 0
	}		
};

#define SCAN_CODE_ESC 0x01

static keycode_t scancode_to_keycode( scancode_set_t set, uint8_t scan_code )
{
	keycode_t key_code;
	key_code.type = KEYCODE_UNKNOWN;

	key_code.c = scancode_map[set][scan_code];
	if( key_code.c > 0 ) {
		key_code.type = KEYCODE_ASCII_CHAR;
	}
	
	if( scan_code == SCAN_CODE_ESC ) {
		key_code.type = KEYCODE_ESC;
	}
	
	return key_code;
}

#define SCAN_CODE_BREAK_CODE 0x80
#define SCAN_CODE_LEFT_SHIFT 0x2A
#define SCAN_CODE_RIGHT_SHIFT 0x36

uint32_t ps2keyboard_handle_interrupt( interrupt_handler_t *handler, uint32_t esp )
{
	ps2keyboard_t *ps2keyboard = (ps2keyboard_t *)handler->driver;
	
	// we can only wake up on IRQ1 because we don't send commands
	// after initialization
	uint8_t scan_code = port8_read( &ps2keyboard->data_port );

	// we get break code when the high bit is set, so the
	// the key is released
	bool break_code = false;
	if( scan_code & SCAN_CODE_BREAK_CODE ) {
		break_code = true;
	}

	scan_code &= ~SCAN_CODE_BREAK_CODE;

	if( scan_code == SCAN_CODE_LEFT_SHIFT ||
		scan_code == SCAN_CODE_RIGHT_SHIFT ) {
		if( break_code ) {
			ps2keyboard->shift = false;
		} else {
			ps2keyboard->shift = true;
		}
	}

	scancode_set_t code_set = SCANCODE_SET_NORMAL;
	if( ps2keyboard->shift ) {
		code_set = SCANCODE_SET_SHIFT;
	}

	keycode_t key_code = scancode_to_keycode( code_set, scan_code );

#ifdef DEBUG
	printf( "KBD SCAN:0x%X S:%d B:%d S:%d T:0x%X '%c' 0x%X\n",
		scan_code, code_set, break_code, ps2keyboard->shift,
		key_code.type, key_code.c, key_code.c );
#endif

	keyboard_event_t event;
	if( break_code ) {
		event.type = KEYBOARD_EVENT_TYPE_KEY_RELEASED;
	} else {
		event.type = KEYBOARD_EVENT_TYPE_KEY_PRESSED;
	}
	event.modifiers = 0;
	if( ps2keyboard->shift ) {
		event.modifiers |= KEYBOARD_MODIFIER_SHIFT;
	}
	// TODO: for control, alt, meta, etc.
	event.key = KEYBOARD_KEY_UNKNOWN;
	if( key_code.type == KEYCODE_ASCII_CHAR ) {
		event.key = KEYBOARD_KEY_ASCII;
		event.ascii_key = key_code.c;
	} else if( key_code.type == KEYCODE_ESC ) {
		event.key = KEYBOARD_KEY_ESC;
	}

	ps2keyboard->base.handler( &event, ps2keyboard->base.base.context );
	
	return esp;
}

void ps2keyboard_print_info( void *obj )
{
	puts( "PS/2 keyboard driver" );
}