#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include "libsys.h"
#include "mips_ps2.h"

uint32_t ps2_readchar(ps2_if_t *pIF);

const ps2_if_t ps2_if[2] =
{
	{ps2_type_unknown, (uint32_t*)SYS_PS2_0_STAT, (uint32_t*)SYS_PS2_0_DATA},
	{ps2_type_unknown, (uint32_t*)SYS_PS2_1_STAT, (uint32_t*)SYS_PS2_1_DATA}
};

uint32_t PS2_get_num_if(void)
{
	return (sizeof(ps2_if)/sizeof(ps2_if_t));
}

int PS2_readchar(void const *pInst)
{
	assert(pInst);
	ps2_if_t *pIF = (ps2_if_t*)pInst;

	return (int)ps2_readchar(pIF);
}

void PS2_open(void const *pInst)
{
	assert(pInst);
	ps2_if_t *pIF = (ps2_if_t*)pInst;

	PS2_init(pIF);
	
}

void PS2_close(void const *pInst)
{
	
}

void PS2_byte_write(ps2_if_t *pIF, uint8_t byte)
{
	assert(pIF);

	while(!(*pIF->pCTRL & SYS_PS2_BIT_TX_EMPTY));

	*pIF->pDATA = (uint32_t)byte;
}

uint32_t PS2_byte_read(ps2_if_t *pIF, uint32_t timeout_ms)
{
	assert(pIF);

	while(!(*pIF->pCTRL & SYS_PS2_BIT_RX_AVAIL))
	{
		timeout_ms--;
		if (!timeout_ms)
			return PS2_ERR_TIMEOUT;

		sleep(1);
	}
	return (uint32_t)*pIF->pDATA;
}

uint32_t PS2_cmd(ps2_if_t *pIF, uint8_t cmd, uint32_t ack_timeout_ms)
{
	uint32_t result;

	PS2_byte_write(pIF, cmd);
	result = PS2_byte_read(pIF, ack_timeout_ms);
	if (0xFA != result)
	{
		printf ("PS2_cmd(%p): no ACK received. Result = %08X\n", pIF->pCTRL, result);
		return PS2_ERR_PS2_PREFIX | result;
	}
	return 0;
}

void PS2_flush(ps2_if_t *pIF)
{
	while (!IS_ERROR(PS2_byte_read(pIF, 10)));
}

uint32_t PS2_init(ps2_if_t *pIF)
{
	int i;
	uint8_t id[2];

	pIF->type = ps2_type_unknown;

	uint32_t result;

	// Disable RX-interrupt
	*pIF->pCTRL &= ~SYS_PS2_BIT_RX_INTEN;

	printf ("PS2_init(%p) - Initialize... ", pIF->pCTRL);

	// Flush buffer
	PS2_flush(pIF);

	// send RESET
	result = PS2_cmd(pIF, 0xFF, 1000);
	if (IS_ERROR(result))
	{
		printf ("Reset error %08X\n", result);
		return result;
	}

	// Read BAT
	result = PS2_byte_read(pIF, 1000);
	if (0xAA != result)
	{
		printf ("BAT error %08X\n", result);
		return result;
	}

	printf ("Success\n");

	while(1)
	{
		// Read ID
		result = PS2_byte_read(pIF, 10);
		if (IS_ERROR(result))
		{
			// No ID => keyboard
			printf ("PS2_init(%p) - Keyboard detected\n", pIF->pCTRL);

			// send Get Device-ID
			result = PS2_cmd(pIF, 0xF2, 1000);
			if (IS_ERROR(result))
			{
				printf ("Get Device-ID error %08X\n", result);
				break;
			}
			id[0] = PS2_byte_read(pIF, 1000);
			id[1] = PS2_byte_read(pIF, 1000);
			printf ("PS2_init(%p) - Device-ID: %02X%02X\n", pIF->pCTRL, id[0], id[1]);

			// Get Scan code set (phase 1)
			result = PS2_cmd(pIF, 0xF0, 1000);
			if (IS_ERROR(result))
			{
				printf ("1. Get Scan code error %08X\n", result);
				break;
			}
			// Get Scan code set (phase 2)
			result = PS2_cmd(pIF, 0x00, 1000);
			if (IS_ERROR(result))
			{
				printf ("2. Get Scan code error %08X\n", result);
				break;
			}
			result = PS2_byte_read(pIF, 1000);
			printf ("PS2_init(%p) - Current scan code set = %d\n", pIF->pCTRL, result);

			// if Scan code SET != 2
			if (result != 2)
			{
				printf ("PS2_init(%p) - Set scan code set = 2\n", pIF->pCTRL);
				// Set Scan code set (phase 1)
				result = PS2_cmd(pIF, 0xF0, 1000);
				if (IS_ERROR(result))
				{
					printf ("1. Set Scan code error %08X\n", result);
					break;
				}
				// Set Scan code set (phase 2)
				result = PS2_cmd(pIF, 0x02, 1000);
				if (IS_ERROR(result))
				{
					printf ("2. Set Scan code error %08X\n", result);
					break;
				}
			}

			// Test LEDs
			for (i=0; i < 4; i++)
			{
				result = PS2_cmd(pIF, 0xED, 1000);
				if (IS_ERROR(result))
				{
					printf ("1. LED error %08X\n", result);
					break;
				}
				result = PS2_cmd(pIF, 0x02, 1000);
				if (IS_ERROR(result))
				{
					printf ("2. LED error %08X\n", result);
					break;
				}
				sleep(100);
				result = PS2_cmd(pIF, 0xED, 1000);
				if (IS_ERROR(result))
				{
					printf ("1. LED error %08X\n", result);
					break;
				}
				result = PS2_cmd(pIF, 0x04, 1000);
				if (IS_ERROR(result))
				{
					printf ("2. LED error %08X\n", result);
					break;
				}
				sleep(100);
				result = PS2_cmd(pIF, 0xED, 1000);
				if (IS_ERROR(result))
				{
					printf ("1. LED error %08X\n", result);
					break;
				}
				result = PS2_cmd(pIF, 0x01, 1000);
				if (IS_ERROR(result))
				{
					printf ("2. LED error %08X\n", result);
					break;
				}
				sleep(100);
			}
			result = PS2_cmd(pIF, 0xED, 1000);
			if (IS_ERROR(result))
			{
				printf ("1. LED error %08X\n", result);
				break;
			}
			result = PS2_cmd(pIF, 0x00, 1000);
			if (IS_ERROR(result))
			{
				printf ("2. LED error %08X\n", result);
				break;
			}

			pIF->type = ps2_type_keyb;
			result = pIF->type;
			break;
		}

		// We got a byte => maybe mouse
		if (result == 0)
		{
			// We got zero => yes, it's a mouse
			printf ("PS2_init(%p) - Mouse device detected\n", pIF->pCTRL);

			// send Get Device-ID
			result = PS2_cmd(pIF, 0xF2, 1000);
			if (IS_ERROR(result))
			{
				printf ("Get Device-ID error %08X\n", result);
				break;
			}
			id[0] = PS2_byte_read(pIF, 1000);
			printf ("PS2_init(%p) - Device-ID: %02X\n", pIF->pCTRL, id[0]);
			printf ("PS2_init(%p) - Enable stream mode...", pIF->pCTRL);
			result = PS2_cmd(pIF, 0xF4, 1000);
			if (IS_ERROR(result))
			{
				printf ("error %08X\n", result);
				break;
			}
			printf ("Success\n");
			pIF->type = ps2_type_mouse;
			result = pIF->type;
			break;
		}
	}

	// Flush buffer
	PS2_flush(pIF);

	if (IS_ERROR(result))
		return result;

	// Enable RX-interrupt
	*pIF->pCTRL |= SYS_PS2_BIT_RX_INTEN;

	return result;
}

// -------------------------------------------------------------
// BEGIN QUICK AND DIRTY
// -------------------------------------------------------------
//keymap for the AT keyboard SG layout
typedef struct
{
    uint8_t scancode;
    uint8_t unshifted;
    uint8_t shifted;
    uint8_t altered;
} libsys_key_entry_t;

#define SHIFTED (1<<0)
#define ALTERED (1<<1)
#define CONTROLLED (1<<2)

static const libsys_key_entry_t __keymap_german[] =
{
    {0x1c, 'a', 'A', 'a'},
    {0x32, 'b', 'B', 'b'},
    {0x21, 'c', 'C', 'c'},
    {0x23, 'd', 'D', 'd'},
    {0x24, 'e', 'E', '€'},
    {0x2b, 'f', 'F', 'f'},
    {0x34, 'g', 'G', 'g'},
    {0x33, 'h', 'H', 'h'},
    {0x43, 'i', 'I', 'i'},
    {0x3b, 'j', 'J', 'j'},
    {0x42, 'k', 'K', 'k'},
    {0x4b, 'l', 'L', 'l'},
    {0x3a, 'm', 'M', 'µ'},
    {0x31, 'n', 'N', 'n'},
    {0x44, 'o', 'O', 'o'},
    {0x4d, 'p', 'P', 'p'},
    {0x15, 'q', 'Q', '@'},
    {0x2d, 'r', 'R', 'r'},
    {0x1b, 's', 'S', 's'},
    {0x2c, 't', 'T', 't'},
    {0x3c, 'u', 'U', 'u'},
    {0x2a, 'v', 'V', 'v'},
    {0x1d, 'w', 'W', 'w'},
    {0x22, 'x', 'X', 'x'},
    {0x1a, 'y', 'Y', 'y'},
    {0x35, 'z', 'Z', 'z'},
    {0x16, '1', '!', '1'},
    {0x1e, '2', '"', '2'},
    {0x26, '3', '§', '3'},
    {0x25, '4', '$', '4'},
    {0x2e, '5', '%', '5'},
    {0x36, '6', '&', '6'},
    {0x3d, '7', '/', '{'},
    {0x3e, '8', '(', '['},
    {0x46, '9', ')', ']'},
    {0x45, '0', '=', '}'},
    {0x4e, 'ß', '?', '\\'},
    {0x29, ' ', ' ', ' '},
    {0x41, ',', ';', ','},
    {0x49, '.', ':', '.'},
    {0x4a, '-', '_', '-'},
    {0x52, 'ä', 'Ä', 'ä'},
    {0x4c, 'ö', 'Ö', 'ö'},
    {0x54, 'ü', 'Ü', 'ü'},
    {0x5a, '\n', '\n', '\n'},       //RETURN
    {0x5b, '+', '*', '~'},
    {0x5d, '#', '\'', '#'},
    {0x0e, '^', '°', '^'},
    {0x61, '<', '>', '|'},
    {0x66, '\b', '\b', '\b'},
    {0}       //sentinel
	
};

static char __findkey(char scancode, int mode)
{
    const libsys_key_entry_t *p = __keymap_german;
    while (p->scancode)
    {
        if (p->scancode == scancode)
        {
            switch (mode)
            {
                case SHIFTED:
                    return p->shifted;
                case ALTERED:
                    return p->altered;
                case ALTERED|SHIFTED:
                default:
                    return p->unshifted;
            }
        }
        p++;
    }
    return 0;
}

#define RELEASE         0x8000
#define SPECIALKEY      0x4000
#define EXTENDED        0x2000
#define X_CONTROLLED    0x0400
#define X_ALTERED       0x0200
#define X_SHIFTED       0x0100
#define CAPS    (SPECIALKEY|0x58)
#define F1      (SPECIALKEY|0x05)
#define F2      (SPECIALKEY|0x06)
#define F3      (SPECIALKEY|0x04)
#define F4      (SPECIALKEY|0x0c)
#define F5      (SPECIALKEY|0x03)
#define F6      (SPECIALKEY|0x0b)
#define F7      (SPECIALKEY|0x83)
#define F8      (SPECIALKEY|0x0a)
#define F9      (SPECIALKEY|0x01)
#define F10     (SPECIALKEY|0x09)
#define F11     (SPECIALKEY|0x78)
#define F12     (SPECIALKEY|0x07)
#define ARROW_UP        (SPECIALKEY|EXTENDED|0x75)
#define ARROW_DOWN      (SPECIALKEY|EXTENDED|0x72)
#define ARROW_LEFT      (SPECIALKEY|EXTENDED|0x6b)
#define ARROW_RIGHT     (SPECIALKEY|EXTENDED|0x74)

uint32_t ps2_readchar(ps2_if_t *pIF)
{
	uint8_t c;
	static char mode = 0;
	static char nextrelease = 0;
	static char extended = 0;
	int flags = 0;
	int char_valid = 0;
	uint8_t key;

	while (!(SYS_PS2_BIT_RX_AVAIL & *pIF->pCTRL));
	key = *pIF->pDATA & 0xFF;

	switch (key)
	{
		case 0xf0: //release key
			nextrelease = 1;
			break;
		case 0x14: //control key
			if (nextrelease)
			{
				mode &= ~CONTROLLED;
			} else
			{
				mode |= CONTROLLED;
			}
			nextrelease = 0;
			break;
		case 0x12: //left SHIFT
		case 0x59: //right SHIFT
			if (nextrelease)
			{
				mode &= ~SHIFTED;
			} else
			{
				mode |= SHIFTED;
			}
			nextrelease = 0;
			break;
		case 0x11: //left and right ALT (right one also with E0)
			if (nextrelease)
			{
				mode &= ~ALTERED;
			} else
			{
				mode |= ALTERED;
			}
			nextrelease = 0;
			break;
		case 0xe0:  //extended key following
			extended = 1;
			break;
		default:
			if (nextrelease) flags |= RELEASE;
			if (extended) flags |= EXTENDED;
			flags |= mode << 8;
			c = __findkey(key, mode);
			char_valid = (c && ((flags & RELEASE) != RELEASE));
			nextrelease = 0;
			extended = 0;
			break;
	}

	if (char_valid)
	{
		// fprintf(stderr, "c = %c (%d), flags = 0x%08X, mode = 0x%08X\n", c, c, flags, mode);
		return c;
	}

	return -1;
}
// -------------------------------------------------------------
// END QUICK AND DIRTY
// -------------------------------------------------------------