#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "irq.h"

#define	CPU_FREQ_HZ	100000000

#include "libsys.h"

char buffer[16384];
char * volatile pPtr_r;
char * volatile pPtr_w;

// -------------------------------------------------------------
typedef struct _sps2_if_t
{
	volatile UINT32 *pCTRL;
	volatile UINT32 *pDATA;
} ps2_if_t;

static ps2_if_t ps2_if[2] =
{
	{(UINT32*)SYS_PS2_0_STAT, (UINT32*)SYS_PS2_0_DATA},
	{(UINT32*)SYS_PS2_1_STAT, (UINT32*)SYS_PS2_1_DATA}
};

void handler0(void)
{
	printf("Interrupt 0\n");
	interrupt_clr(0);
}

void handler1(void)
{
	printf("Interrupt 1\n");
	interrupt_clr(1);
}

void handler2(void)
{
	printf("Interrupt 2\n");
}

void handler3(void)
{
	volatile UINT32 *pUART0_stat = (UINT32*)SYS_UART0_STAT;
	volatile UINT32 *pUART0_data = (UINT32*)SYS_UART0_DATA;
	volatile UINT32 *pUART1_stat = (UINT32*)SYS_UART1_STAT;
	volatile UINT32 *pUART1_data = (UINT32*)SYS_UART1_DATA;

	while(0x200 & *pUART0_stat)
	{
//		sputs("w: "); print_word((int)pPtr_w); sputs("\n");
		if (pPtr_w == &buffer[16383])
			pPtr_w = buffer;
		*(pPtr_w++) = *pUART0_data;
	}

	while(0x200 & *pUART1_stat)
	{
//		sputs("w: "); print_word((int)pPtr_w); sputs("\n");
		if (pPtr_w == &buffer[16383])
			pPtr_w = buffer;
		*(pPtr_w++) = *pUART1_data;
	}

}

void handler4(void)
{
	printf("Interrupt 4\n");
}

void handler5(void)
{
	printf("Interrupt 5\n");
}

static INT32 mouse_x, mouse_y;

void handler6(void)
{
	int p, i;
	ps2_if_t *pIF;
	UINT8 mouse_rsp[3];
	UINT32 timeout_count, key;

	INT32 dx, dy;

	for (p=0; p < sizeof(ps2_if)/sizeof(ps2_if_t); p++)
	{
		pIF = &ps2_if[p];
		if (SYS_PS2_BIT_RX_ERR_FLAG & *pIF->pCTRL)
			fprintf(stderr, "PS2[%d]: Status = 0x%08X\n", p, *(pIF->pCTRL));

		if (p==0)
		{
			while(SYS_PS2_BIT_RX_IRQ & *pIF->pCTRL)
			{
				key = *pIF->pDATA;
				printf("Keyboard scan code = 0x%02X\n", key);
			}
		}
		if (p==1)
		{
			if(!(SYS_PS2_BIT_RX_IRQ & *pIF->pCTRL))
				continue;

			for (i=0; i < 3; i++)
			{
				timeout_count = 100;
				while(!(SYS_PS2_BIT_RX_IRQ & *pIF->pCTRL))
				{
					timeout_count--;
					if (!timeout_count)
					{
						break;
					}
					sleep(1);
				}
				if (!timeout_count)
				{
					break;
				}
				mouse_rsp[i] = (UINT8)*pIF->pDATA;
			}
			if (!timeout_count)
			{
				continue;
			}
			dx = mouse_rsp[1];
			if (mouse_rsp[0] &  0x10)
				dx |= 0xFFFFFF00;
			dy = mouse_rsp[2];
			if (mouse_rsp[0] &  0x20)
				dy |= 0xFFFFFF00;

//			printf("dx = %5d, dy = %5d\n", dx, dy);

			mouse_x += dx;
			mouse_y -= dy;
			if ((mouse_rsp[0] & 0x03) == 0x03)
			{
				mouse_x = 0;
				mouse_y = 0;
			}
			if ((mouse_rsp[0] & 0x03) == 0x01)
			{
				mouse_x = 0;
				Screen_line_clr();
			}
			if ((mouse_rsp[0] & 0x03) == 0x02)
				mouse_y = 0;

			Screen_csr_set_x(mouse_x);
			Screen_csr_set_y(mouse_y);

//			printf("x = %5d, y = %5d\n", mouse_x, mouse_y);

		}
	}
}

void handler7(void)
{
	printf("Interrupt 7\n");
}

#define	PS2_MODULE_PREFIX	0x1000000
#define	PS2_SUCCESS			LSYS_SUCCESS
#define	PS2_ERROR			(LSYS_ERR_BASE | PS2_MODULE_PREFIX)
#define	PS2_ERR_TIMEOUT		(PS2_ERROR | 0x0000001)
#define	PS2_ERR_PS2_PREFIX	(PS2_ERROR | 0x0010000)

void PS2_byte_write(UINT32 port, UINT8 byte)
{
	ps2_if_t *pIF;

	pIF = &ps2_if[port];

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

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

UINT32 PS2_byte_read(UINT32 port, UINT32 timeout_ms)
{
	ps2_if_t *pIF;

	pIF = &ps2_if[port];

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

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

UINT32 PS2_cmd(UINT32 port, UINT8 cmd, UINT32 ack_timeout_ms)
{
	UINT32 result;

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

void PS2_flush(UINT32 port)
{
	while (!IS_ERROR(PS2_byte_read(port, 10)));
}

UINT32 PS2_init(UINT32 port)
{
	int i;
	ps2_if_t *pIF;
	UINT8 id[2];

	pIF = &ps2_if[port];

	UINT32 result;

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

	printf ("PS2_init(%d) - Initialize... ", port);

	// Flush buffer
	PS2_flush(port);

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

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

	printf ("Success\n");

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

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

			// Get Scan code set (phase 1)
			result = PS2_cmd(port, 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(port, 0x00, 1000);
			if (IS_ERROR(result))
			{
				printf ("2. Get Scan code error %08X\n", result);
				break;
			}
			result = PS2_byte_read(port, 1000);
			printf ("PS2_init(%d) - Current scan code set = %d\n", port, result);

			// if Scan code SET != 2
			if (result != 2)
			{
				printf ("PS2_init(%d) - Set scan code set = 2\n", port);
				// Set Scan code set (phase 1)
				result = PS2_cmd(port, 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(port, 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(port, 0xED, 1000);
				if (IS_ERROR(result))
				{
					printf ("1. LED error %08X\n", result);
					break;
				}
				result = PS2_cmd(port, 0x02, 1000);
				if (IS_ERROR(result))
				{
					printf ("2. LED error %08X\n", result);
					break;
				}
				sleep(100);
				result = PS2_cmd(port, 0xED, 1000);
				if (IS_ERROR(result))
				{
					printf ("1. LED error %08X\n", result);
					break;
				}
				result = PS2_cmd(port, 0x04, 1000);
				if (IS_ERROR(result))
				{
					printf ("2. LED error %08X\n", result);
					break;
				}
				sleep(100);
				result = PS2_cmd(port, 0xED, 1000);
				if (IS_ERROR(result))
				{
					printf ("1. LED error %08X\n", result);
					break;
				}
				result = PS2_cmd(port, 0x01, 1000);
				if (IS_ERROR(result))
				{
					printf ("2. LED error %08X\n", result);
					break;
				}
				sleep(100);
			}
			result = PS2_cmd(port, 0xED, 1000);
			if (IS_ERROR(result))
			{
				printf ("1. LED error %08X\n", result);
				break;
			}
			result = PS2_cmd(port, 0x00, 1000);
			if (IS_ERROR(result))
			{
				printf ("2. LED error %08X\n", result);
				break;
			}

			result = 1;
			break;
		}

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

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

	// Flush buffer
	PS2_flush(port);

	if (IS_ERROR(result))
		return result;

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

	return result;

}

int main(void)
{

	volatile UINT32 *pUART0_stat = (UINT32*)SYS_UART0_STAT;
	volatile UINT32 *pUART1_stat = (UINT32*)SYS_UART1_STAT;
	volatile UINT32 *pUART0_baud = (UINT32*)SYS_UART0_BAUD;
	volatile UINT32 *pUART1_baud = (UINT32*)SYS_UART1_BAUD;

	char string[65];

	UART1_setbaud(460800);
	printf("Hello\n");
	memset(buffer, 0, sizeof(buffer));
	pPtr_r = buffer;
	pPtr_w = buffer;
	interrupt_register(0, (void*)handler0);
	interrupt_enable(0);

	interrupt_register(1, (void*)handler1);
	interrupt_enable(1);

//	interrupt_register(2, (void*)handler2);
//	interrupt_enable(2);

	printf("UART0 Status = 0x%8.8X\n", *pUART0_stat);
	printf("UART0 Baud   = 0x%8.8X\n", *pUART0_baud);
	printf("UART1 Status = 0x%8.8X\n", *pUART1_stat);
	printf("UART1 Baud   = 0x%8.8X\n", *pUART1_baud);

	// UART: enable RX interrupt
	*pUART0_stat |= (1 << 6);
	*pUART1_stat |= (1 << 6);
	printf("UART0 Status = 0x%8.8X\n", *pUART0_stat);
	printf("UART1 Status = 0x%8.8X\n", *pUART1_stat);

	PS2_init(0);
	PS2_init(1);

	interrupt_register(3, (void*)handler3);
	interrupt_enable(3);

	interrupt_register(4, (void*)handler4);
//	interrupt_enable(4);

	interrupt_register(5, (void*)handler5);
	interrupt_enable(5);

	interrupt_register(6, (void*)handler6);
	interrupt_enable(6);

	interrupt_register(7, (void*)handler7);
	interrupt_enable(7);

	printf("Start:\n");

	interrupt_set(0);
	interrupt_set(1);
	interrupt_set(2);
	interrupt_set(3);
	interrupt_set(4);
	interrupt_set(5);
	interrupt_set(6);
	interrupt_set(7);

	sputs("r: "); print_word((int)pPtr_r); sputs("\n");
	sputs("w: "); print_word((int)pPtr_w); sputs("\n");

	// Print Status register
	sputs("Status  : ");
	print_word(CP0_SR_read());
	sputs("\n");
	sputs("\n");

	interrupt_disable(6);
	printf("Bitte gebe ein: ");
	scanf("%s", string);
	printf("Du schriebst: %s\n", string);

	// Flush buffer
	PS2_flush(0);
	PS2_flush(1);


//	while(1)
//	{
//		printf("PS2-Status : 0x%08X\n", *(ps2_if[0].pCTRL));
//		printf("PS2-Data   : 0x%08X\n", *(ps2_if[0].pDATA));
//		sleep(1000);
//	}

	interrupt_enable(6);

	while(1)
	{
		if(pPtr_w != pPtr_r)
		{
//			sputs("r: "); print_word((int)pPtr_r); sputs("\n");
			if (pPtr_r == &buffer[16383])
				pPtr_r = buffer;
			writechar(0, *(pPtr_r++));
		}
	}
	return 0;
}