#include <Arduino.h>
#include <stdint.h>
#include <hardware.h>
#include <ps2_raw_kbd.h>
#include "kbd.h"

//#define DEBUGGING

// http://www.6502.org/users/andre/petindex/keyboards.html

// maps scan codes to pet rows/cols (graphics keyboard)
static const uint8_t scanmap[128] PROGMEM = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x00
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x08
	0xff, 0xff, 0x80, 0xff, 0xff, 0x20, 0x00, 0xff, // 0x10
	0xff, 0xff, 0x60, 0x50, 0x40, 0x30, 0x10, 0xff, // 0x18
	0xff, 0x61, 0x70, 0x41, 0x21, 0x11, 0x01, 0xff, // 0x20
	0xff, 0x92, 0x71, 0x51, 0x22, 0x31, 0x02, 0xff, // 0x28
	0xff, 0x72, 0x62, 0x52, 0x42, 0x32, 0x25, 0xff, // 0x30
	0xff, 0xff, 0x63, 0x43, 0x23, 0x03, 0x57, 0xff, // 0x38
	0xff, 0x73, 0x53, 0x33, 0x24, 0x14, 0x04, 0xff, // 0x40
	0xff, 0x96, 0x37, 0x44, 0x64, 0x34, 0x87, 0xff, // 0x48
	0xff, 0xff, 0x12, 0xff, 0x91, 0x97, 0xff, 0xff, // 0x50
	0xff, 0x85, 0x65, 0x82, 0xff, 0x01, 0xff, 0xff, // 0x58
	0xff, 0x13, 0xff, 0xff, 0xff, 0x05, 0x17, 0xff, // 0x60
	0xff, 0x66, 0xff, 0x46, 0x26, 0xff, 0xff, 0xff, // 0x68
	0x86, 0x96, 0x76, 0x56, 0x47, 0x36, 0xff, 0x25, // 0x70
	0xff, 0x77, 0x67, 0x87, 0x57, 0x27, 0xff, 0xff, // 0x78
};

// accessed by R-Shift
static const uint8_t shiftmap[128] PROGMEM = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x00
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x08
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0xff, // 0x10
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x10, 0xff, // 0x18
	0xff, 0xff, 0xff, 0xff, 0xff, 0x11, 0xff, 0xff, // 0x20
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x02, 0xff, // 0x28
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x25, 0xff, // 0x30
	0xff, 0xff, 0xff, 0xff, 0xff, 0x03, 0x57, 0xff, // 0x38
	0xff, 0x93, 0xff, 0xff, 0xff, 0x14, 0x04, 0xff, // 0x40
	0xff, 0x84, 0x74, 0xff, 0x54, 0xff, 0xff, 0xff, // 0x48
	0xff, 0xff, 0x81, 0xff, 0xff, 0x77, 0xff, 0xff, // 0x50
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x58
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x60
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x68
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x70
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x78
};

// accessed by Ctrl
static const uint8_t ctrlmap[128] PROGMEM = {
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x00
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x08
	0xff, 0xff, 0xff, 0xff, 0xff, 0x16, 0xff, 0xff, // 0x10
	0xff, 0xff, 0xff, 0x06, 0xff, 0xff, 0xff, 0xff, // 0x18
	0xff, 0x94, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x20
	0xff, 0xff, 0xff, 0xff, 0x17, 0x90, 0xff, 0xff, // 0x28
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x30
	0xff, 0xff, 0x65, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x38
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x40
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x48
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x50
	0xff, 0xff, 0xff, 0x07, 0xff, 0xff, 0xff, 0xff, // 0x58
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x60
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x68
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x70
	0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, // 0x78
};

uint8_t kbd::_map(uint8_t scan) {
	if (_shift)
		return pgm_read_byte(shiftmap + scan);

	if (_ctrl)
		return pgm_read_byte(ctrlmap + scan);

	return pgm_read_byte(scanmap + scan);
}
	
void kbd::_reset(uint8_t k) {
	_rows[(k & 0xf0) >> 4] &= ~(1 << (k & 0x0f));
}

void kbd::up(uint8_t scan) {

	DBG(printf("key up: %02x", scan));
	DBG(println());

	if (_ext) {
		_ext = false;
		switch(scan) {
		case 0x69:	// end
			_reset(0x80);
		case 0x6c:	// home
			_reset(0x06);
			break;
		case 0x75:	// up
			_reset(0x80);
		case 0x72:	// down
			_reset(0x16);
			break;
		case 0x6b:	// left
			_reset(0x80);
		case 0x74:	// right
			_reset(0x07);
			break;
		case 0x70:	// ins
			_reset(0x80);
		case 0x71:	// del
			_reset(0x17);
			break;
		default:
			if (scan < 0x80)
				_reset(_map(scan));
			break;
		}
	} else if (scan == 0x59)	// R-Shift
		_shift = false;
	else if (is_ps2_ctrl(scan))
		_ctrl = false;
	else if (scan < 0x80)
		_reset(_map(scan));
}

void kbd::_set(uint8_t k) {
	_rows[(k & 0xf0) >> 4] |= 1 << (k & 0x0f);
}

void kbd::down(uint8_t scan) {

	DBG(printf("key down: %02x", scan));
	DBG(println());

	if (scan == 0xe0)
		_ext = true;
	else if (_ext) {
		_ext = false;
		switch(scan) {
		case 0x69:	// end
			_set(0x80);
		case 0x6c:	// home
			_set(0x06);
			break;
		case 0x75:	// up
			_set(0x80);
		case 0x72:	// down
			_set(0x16);
			break;
		case 0x6b:	// left
			_set(0x80);
		case 0x74:	// right
			_set(0x07);
			break;
		case 0x70:	// ins
			_set(0x80);
		case 0x71:	// del
			_set(0x17);
			break;
		default:
			if (scan < 0x80)
				_set(_map(scan));
		}
	} else if (scan == 0x59) {	// R-Shift
		// reset any depressed keys so they don't get stuck
		reset();
		_shift = true;
	} else if (is_ps2_ctrl(scan)) {
		reset();
		_ctrl = true;
	} else if (scan < 0x80)
		_set(_map(scan));
}

void kbd::reset() {
	for (int i = sizeof(_rows); i--; )
		_rows[i] = 0;
	_ctrl = false;
	_shift = false;
}
