#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 (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;

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

		}
	}
}

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

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 ERROR;

		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 ERROR;
	}
	return 0;
}

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

void PS2_init(UINT32 port)
{
	ps2_if_t *pIF;

	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;
	}

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

	printf ("Success\n");

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

	if (result == 0)
	{
		printf ("PS2_init(%d) - Mouse device detected\n", port);
		printf ("PS2_init(%d) - Enable stream mode...", port);
		result = PS2_cmd(port, 0xF4, 1000);
		if (IS_ERROR(result))
		{
			printf ("error %08X\n", result);
			return;
		}
		printf ("Success\n");
	}

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

	// Flush buffer
	while (ERROR != PS2_byte_read(port, 10));

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

}

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);

	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;
}