#include <xparameters.h>
#include <xiomodule.h>

XIOModule gpo;

Xint8 x = 0;
Xint8 y = 0;
Xint8 scroll = 0;

Xint8 reversed = 0;
Xint8 foreground = 0b111;
Xint8 background = 0;

Xint16 properties = 0;

void commit() {
    XIOModule_DiscreteSet(&gpo, 3, 1);
    XIOModule_DiscreteClear(&gpo, 3, 1);
}

void calculate_properties() {
	if (reversed) {
		properties = ((foreground << 4) + background) << 8;
	} else {
		properties = ((background << 4) + foreground) << 8;
	}
}

void set_foreground(Xint8 color) {
	foreground = color;

	calculate_properties();
}

void set_background(Xint8 color) {
	background = color;

	calculate_properties();
}

void reverse(u8 enable) {
	reversed = enable;

	calculate_properties();
}

void clear_screen() {
	reverse(0);
	set_foreground(0b111);
	set_background(0);
	XIOModule_DiscreteWrite(&gpo, 1, 0);
    XIOModule_DiscreteWrite(&gpo, 2, 0);
    XIOModule_DiscreteSet(&gpo, 3, 1);

    int x;
    int y;
	for (y = 0; y < (45+2); ++y) {
	    Xint16 temp = (y << 8);
		for (x = 0; x < (80+24); ++x) {
		    XIOModule_DiscreteWrite(&gpo, 2, temp + x);
		}
	}

    XIOModule_DiscreteClear(&gpo, 3, 1);

	x = 0;
	y = 0;
	scroll = 0;
    XIOModule_DiscreteWrite(&gpo, 4, scroll);
}

void clear_eol() {
	reverse(0);
    Xint16 temp = (y << 8);

	XIOModule_DiscreteWrite(&gpo, 1, 0);
    XIOModule_DiscreteWrite(&gpo, 2, temp + x);
    XIOModule_DiscreteSet(&gpo, 3, 1);

    int i;
	for (i = x; i < (80+24); ++i) {
	    XIOModule_DiscreteWrite(&gpo, 2, temp + i);
	}

    XIOModule_DiscreteClear(&gpo, 3, 1);
}

void write(u8 c) {
	XIOModule_DiscreteWrite(&gpo, 1, properties + c);
    XIOModule_DiscreteWrite(&gpo, 2, (y << 8) + x);
    commit();
}

void next() {
	x++;
	if (x >= 80) {
		y++;
		x %= 80;
	}

	if (y >= 45) {
		y--;
		scroll = (scroll + 1) % 45;
	    XIOModule_DiscreteWrite(&gpo, 4, scroll);
		clear_eol();
	}
}

// @todo This does not appear to work quite correctly
void previous() {
	x--;
	if (x < 0) {
		y--;
		x %= 80;
	}

	if (y < 0) {
		y = 0;
		x = 0;
	}
}

u8 data = 0;
u8 had = 0;
u8 escape = 0;
u8 escape_parameter_1 = 0;
u8 escape_parameter_2 = 0;


void clock() {
	u8 signals = XIOModule_DiscreteRead(&gpo, 3) & 0b11;

	if (!(signals & 0b01)) {
		return;
	}

	if (signals & 0b10) {
		data = XIOModule_DiscreteRead(&gpo, 1) & 0xFF;
		had = 1;
	} else if (had) {
		had = 0;

		if (escape == 1) {
			if (data == '[') {
				escape = 2;
			} else {
				escape = 0;
			}
		} else if (escape) {
			switch (data) {
				// For now we are only going to implement what we actually use
				case 'K':
					// Assume parameter 0
					clear_eol();
					escape = 0;
					break;

				case 'H':
					if (escape_parameter_1 == 0) {
						escape_parameter_1 = 1;
					}

					if (escape_parameter_2 == 0) {
						escape_parameter_2 = 1;
					}

					x = escape_parameter_1 - 1;
					y = escape_parameter_2 - 1;
					escape = 0;
					break;

				case 'm':
					if (escape_parameter_1 == 0) {
						reverse(0);
						set_foreground(0b111);
						set_background(0);
					} else if (escape_parameter_1 == 7) {
						reverse(1);
					} else if (escape_parameter_1 >= 30 && escape_parameter_1 <= 37) {
						set_foreground(escape_parameter_1 - 30);
					} else if (escape_parameter_1 >= 40 && escape_parameter_1 <= 47) {
						set_background(escape_parameter_1 - 40);
					} else if (escape_parameter_1 >= 90 && escape_parameter_1 <= 97) {
						set_foreground(escape_parameter_1 - 90 + 8);
					} else if (escape_parameter_1 >= 100 && escape_parameter_1 <= 107) {
						set_background(escape_parameter_1 - 100 + 8);
					}
					escape = 0;
					break;

				case 'J':
					// Assume parameter 2
					clear_screen();
					escape = 0;
					break;

				case '0' ... '9':
					escape_parameter_1 *= 10;
					escape_parameter_1 += (data - 48);
					break;

				case ';':
					escape_parameter_2 = escape_parameter_1;
					escape_parameter_1 = 0;
					break;

				default:
					escape = 0;
					break;
			}
		} else {
			switch (data) {
				case '\n':
					y++;
					if (y >= 45) {
						y--;
						scroll = (scroll + 1) % 45;
					    XIOModule_DiscreteWrite(&gpo, 4, scroll);
						clear_eol();
					}
					break;

				case '\r':
					x = 0;
					break;

				case 0x08:
					previous();
					break;

				case 0x1B:
					escape = 1;
					escape_parameter_1 = 0;
					escape_parameter_2 = 0;
					// Handle escape code
					break;

				default:
					write(data - 32);
					next();
					break;
			}
		}
	    XIOModule_DiscreteWrite(&gpo, 2, (y << 8) + x);
	}
}

int main() {
  XIOModule_Initialize(&gpo, XPAR_IOMODULE_0_DEVICE_ID); // Initialize the GPO module

  microblaze_register_handler(XIOModule_DeviceInterruptHandler,
                              XPAR_IOMODULE_0_DEVICE_ID); // register the interrupt handler

  XIOModule_Start(&gpo); // start the GPO module

  XIOModule_Connect(&gpo, XIN_IOMODULE_GPI_2_INTERRUPT_INTR, clock,
                    NULL); // register timerTick() as our interrupt handler
  XIOModule_Enable(&gpo, XIN_IOMODULE_GPI_2_INTERRUPT_INTR); // enable the interrupt

  microblaze_enable_interrupts(); // enable global interrupts

  // Clear the screen
  clear_screen();

  while (1) {

  }
}
