#include <stdio.h>
#include <system.h>
#include <io.h>
#include "altera_ps2/alt_up_ps2_port.h"
#include "altera_ps2/ps2_keyboard.h"
#include "raytracer.h"
#include "simulations.h"

void DontCrash();

/**
 * Returns 0 on success, 1 on failure.
 **/
int init_keyboard() {
    PS2_DEVICE ps2_mode;
    
    printf("Initializing keyboard.\n");
    clear_FIFO();
    ps2_mode = get_mode();
    if (ps2_mode != PS2_KEYBOARD) {
        printf("Error: Didn't detect keyboard.\n");
        return 1;
    }
    return 0;
}

int simulation = 0;

alt_u8 LatestKey() {
	KB_CODE_TYPE kb_decode_mode;
	alt_u8 key;
	int ps2_status;
    ps2_status = read_make_code_no_block(&kb_decode_mode, &key);
    if (ps2_status == PS2_NOT_READY) {
        return 0x00;
    } else if (ps2_status == PS2_ERROR) {
        printf("Error reading from ps2 port.\n");
        return 0x00;
    } // If we get here, ps2_status == PS2_SUCCESS
	if (KB_1_DOWN == key) {
		simulation = HOT_POTATO;
	} else if (KB_2_DOWN == key) {
		simulation = BOUNCING_SQUARE;
	} else if (KB_3_DOWN == key) {
		simulation = FLASHING_CUBES;
	} else if (KB_4_DOWN == key) {
		simulation = DRIVING;
    } else if (KB_ESC_DOWN == key) {
        simulation = DONE;
    } else if (KB_5_DOWN == key) {
        simulation = DONT_CRASH;
    }
	printf("Got key %x\n", key);
	return key;
}
    
int direction(current, min, max, location) {
	if ((location <= min && current < 0) || (location >= max && current > 0)) {
		current *= -1;
	}
	return current;
}

void BouncingSquare() {
	write_rect(0, 0, 0, 0, 254, 254, 0, rgb_to_color(3, 3, 3));
	int thirds = 0, move = 0, left = 0, left_direction = 1, front = 0, front_direction = 1, top = 0, top_direction = 1;
	int rectangleWidth = 50;
	while (BOUNCING_SQUARE == simulation) {
		LatestKey();
		if (thirds == 0) {
			write_rect(1, front, left, top, front, rectangleWidth + left, rectangleWidth + top, rgb_to_color(3, 0, 0));
			write_rect(2, rectangleWidth + front, left, top, rectangleWidth + front, rectangleWidth + left, rectangleWidth + top, rgb_to_color(0, 3, 0));
		} else if (thirds == 1) {
			write_rect(1, front, left, top, rectangleWidth + front, rectangleWidth + left, top, rgb_to_color(0, 0, 3));
			write_rect(2, front, left, rectangleWidth + top, rectangleWidth + front, rectangleWidth + left, rectangleWidth + top, rgb_to_color(3, 3, 0));
		} else if (thirds == 2) {
			write_rect(1, front, left, top, rectangleWidth + front, left, rectangleWidth + top, rgb_to_color(3, 0, 3));
			write_rect(2, front, rectangleWidth + left, top, rectangleWidth + front, rectangleWidth + left, rectangleWidth + top, rgb_to_color(0, 3, 3));
		}
		if (move % 250 == 0) {
			left_direction = direction(left_direction, 0, 250 - rectangleWidth, left);
			left += left_direction;
		}
		if (move % 200 == 0) {
			front_direction = direction(front_direction, 0, 250 - rectangleWidth, front);
			front += front_direction;
		}
		if (move % 100 == 0) {
			top_direction = direction(top_direction, 0, 250 - rectangleWidth, top);
			top += top_direction;
		}
		move = (move + 1) % 500;
		thirds = (thirds + 1) % 3;
	}

}

void FlashingColumns() {
	write_rect(0, 75, 0, 0, 75, 254, 254, rgb_to_color(0, 0, 0));
	int fourths = 0;
	IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_X, 5);
	int camera_c = 0;
	int camera_x = 0, camera_dx = 1, camera_y = 0, camera_dy = 1,
		camera_z = 0, camera_dz = 1;
	while (FLASHING_CUBES == simulation) {
		LatestKey();
		camera_c = (camera_c + 1) % 500;
		if (0 == camera_c) {
			camera_dx = direction(camera_dx, 0, 74, camera_x);
			camera_x += camera_dx; 
			camera_dy = direction(camera_dy, 0, 254, camera_y);
			camera_y += camera_dy;
			camera_dz = direction(camera_dz, 0, 201, camera_z);
			camera_z += camera_dz;
			IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_X, camera_x);
			IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_Y, camera_y);
			IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_Z, camera_z);
		} 
		fourths = (fourths + 1) % 4;
		if (0 == fourths) {
			write_rect(1, 40, 25, 0, 40, 50, 250, rgb_to_color(3, 3, 0));
			write_rect(2, 40, 225, 0, 40, 200, 250, rgb_to_color(3, 0, 3));
		} else if (1 == fourths) {
			write_rect(1, 65, 25, 0, 65, 50, 250, rgb_to_color(3, 3, 0));
			write_rect(2, 65, 225, 0, 65, 200, 250, rgb_to_color(3, 0, 3));
		} else if (2 == fourths) {
			write_rect(1, 40, 50, 0, 65, 50, 250, raw_rgb_to_color(3, 0, 0));
			write_rect(2, 40, 200, 0, 65, 200, 250, raw_rgb_to_color(0, 3, 0));
		} else if (3 == fourths) {
			write_rect(1, 40, 25, 0, 65, 25, 250, raw_rgb_to_color(3, 0, 0));
			write_rect(2, 40, 225, 0, 65, 225, 250, raw_rgb_to_color(0, 3, 0));
		}
	}
}

void Driving() {
	IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_Z, 10);
	write_rect(0, 0, 0, 0, 254, 254, 0, rgb_to_color(3, 3, 0));
	write_rect(1, 254, 0, 0, 254, 254, 254, rgb_to_color(0, 0, 3));
	int camera_c = 0;
	int camera_x = 230, camera_y = 170, camera_dy = 1;
	int ship_x1, ship_x2, ship_z = 1, ship_dz = 1;
	while (DRIVING == simulation) {
		LatestKey();
		camera_c = (camera_c + 1) % 4000;
		if (0 == camera_c) {
			ship_dz = direction(ship_dz, 1, 10, ship_z);
			ship_z += ship_dz;
		}
		if (0 == camera_c % 1000) {
			camera_dy = direction(camera_dy, 11, 245, camera_y);
			camera_y += camera_dy;
			IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_Y, camera_y);
		}
		if (0 == camera_c % 500) {
			camera_x = (camera_x + 1) % 254; 
			IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_X, camera_x);
			ship_x1 = camera_x + 10;
			if (ship_x1 > 254) { ship_x1 = 254; }
			ship_x2 = camera_x + 20;
			if (ship_x2 > 254) { ship_x2 = 254; }
			write_rect(2, ship_x1, camera_y - 10, ship_z, ship_x2, camera_y + 10, ship_z, rgb_to_color(0, 3, 0));
			write_rect(3, ship_x1, camera_y - 10, ship_z + 10, ship_x2, camera_y + 10, ship_z + 10, rgb_to_color(0, 3, 0));
			write_rect(4, ship_x1, camera_y - 10, ship_z + 20, ship_x2, camera_y + 10, ship_z + 20, rgb_to_color(0, 3, 0));
			write_rect(5, 200, 50, 254, 200, 200, ship_x2, rgb_to_color(3, 0, 3));
		}
	}
}

int Abs(int value) {
	if (value < 0) { return - value; }
	return value;
}

int ClosestValid(int coordinate) {
	if (coordinate < 0) {
		return 0;
	} else if (coordinate > 254) {
		return 254;
	} else {
		return coordinate;
	}
}

int recX1[] = {11, 250, 75,  10 }, recX2[] = {0, 250};
int recY1[] = {128,120, 125, 100}, recY2[] = {0, 140};
int recZ1[] = {1,  120, 175, 200}, recZ2[] = {1, 140};

int recR[] = {0, 3};
int recG[] = {0, 0};
int recB[] = {3, 0};

void RenderShipAim() {
	int i;
	for (i = 0; i < 2; i ++) {
		write_rect(i,
			ClosestValid(recX1[i]), ClosestValid(recY1[i]), ClosestValid(recZ1[i]),
			ClosestValid(recX2[i]), ClosestValid(recY2[i]), ClosestValid(recZ2[i]),
			raw_rgb_to_color(recR[i], recG[i], recB[i]));
	}
}

int MoveFromD(int distance) {
	if (distance > 0) {
		return 1;
	} else if (distance < 0) {
		return -1;
	}
	return 0;
}

int dest_y = 128;
int dest_z = 0;
void MoveHotPotatoShip() {
	recX1[0] = ClosestValid(recX1[0] + 1);
	if (254 <= recX1[0]) {
		recX1[0] = 0;
	}
	recY1[0] += MoveFromD(dest_y - recY1[0]);
	int z_dist = dest_z - recZ1[0];
	recZ1[0] += MoveFromD(z_dist);
	if (0 == z_dist && dest_z > 0) {
		dest_z = 0;
	}
	recX2[0] = recX1[0] + 20;
	recY2[0] = recY1[0] + 20;
	recZ2[0] = recZ1[0];
	IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_X, ClosestValid(recX1[0] - 20));
	IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_Y, ClosestValid(recY1[0] + 10));
	IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_Z, ClosestValid(recZ1[0] + 10));
}

const int MAX_MOVEMENT = 25;
int movement[] = {
	1, 0, 0, 0, 1, 0, 0, 1,
	0, 1, 0, 1, 1, 1, 0, 1, 0,
	1, 0, 0, 1, 0, 0, 0, 1};

int a_dy = 0, a_ddy = 0;
int a_dz = 0, a_ddz = 0;
void MoveAimer() {
	if (a_ddy > 0) {
		recY1[1] += a_dy * movement[a_ddy];
		recY2[1] = recY1[1] + 20;
		a_ddy --;
	}
	if (a_ddz > 0) {
		recZ1[1] += a_dz * movement[a_ddz];
		recZ2[1] = recZ1[1] + 20;
		a_ddz --;
	}
}

int movingBack[] = {0, 0, 0};
int aimX[] = {0, 0, 0};
int aimY[] = {0, 0, 0};
void MoveBullets() {
	int b;
	for (b = 0; b < 2; b ++) {
		if (1 == movingBack[b]) {
			int xs = 250 - recX1[2 + b];
			int ys = recY1[1] - recY1[2 + b];
			int zs = recZ1[1] - recZ1[2 + b];
			if (xs == 0 && ys == 0 && zs == 0) {
				aimX[b] = ClosestValid(recX1[0] + 50);
				aimY[b] = recY1[1];
				movingBack[b] = 0;
			} else {
				recX1[2 + b] += MoveFromD(xs);
				recY1[2 + b] += MoveFromD(ys);
				recZ1[2 + b] += MoveFromD(zs);
			}
		} else {
			recX1[2 + b] += MoveFromD(aimX[b] - recX1[2 + b]);
			recY1[2 + b] += MoveFromD(aimY[b] - recY1[2 + b]);
			recZ1[2 + b] += MoveFromD(0 - recZ1[2 + b]);
		}
	}
}

void NewY(int newY) {
	dest_y = newY;
	dest_z = Abs(recY1[0] - dest_y) / 2;
}

int nextBullet = 0;
void HandleKey() {
	alt_u8 key = LatestKey();
	if (0x39 == key) {
		while (LatestKey() != 0x39) {
		} 
	} else if (0x29 == key) { // space fire
		int nb, found = 0;
		for (nb = 0; nb < 3; nb ++) {
			int anb = (nextBullet + nb) % 3;
			if (0 == found && 0 == movingBack[anb]) {
				found = 1;
				movingBack[anb] = 1;
				nextBullet = (nextBullet + 1) % 3;
			}
		}
	} else {
		if (0x41 == key) { // a p1
			NewY(20);
		} else if (0x53 == key) { // s p1
			NewY(70);
		} else if (0x44 == key) { // d p1
			NewY(125);
		} else if (0x46 == key) { // f p1
			NewY(175);
		} else if (0x47 == key) { // g p1
			NewY(230);
		} else if (0x4a == key) { // j p2 left
			a_dy = -1; a_ddy = MAX_MOVEMENT;
		} else if (0x49 == key) { // i p2 uip
			a_dz = 1; a_ddz = MAX_MOVEMENT;
		} else if (0x4b == key) { // k p2 down
			a_dz = -1; a_ddz = MAX_MOVEMENT;
		} else if (0x4c == key) { // l p2 right
			a_dy = 1; a_ddy = MAX_MOVEMENT;
		}
	}
}

void write_bullet(int b, int x1, int y1, int z1, int x2, int y2, int z2, int color) {
	write_rect(b + 2, x1, y1, z1, ClosestValid(x2), ClosestValid(y2), ClosestValid(z2), color);
}

const int BULLET_W = 40;
int flash = 0;
void FlashBullets() {
	int b;
	for (b = 0; b < 2; b ++) {
		switch (flash) {
			case 0:
				write_bullet(b, recX1[2 + b], recY1[2 + b], recZ1[2 + b], recX1[2 + b], recY1[2 + b] + BULLET_W, recZ1[2 + b] + BULLET_W, raw_rgb_to_color(3, 0, 3));
				break;
			case 1:
				write_bullet(b, recX1[2 + b], recY1[2 + b], recZ1[2 + b], recX1[2 + b] + BULLET_W, recY1[2 + b], recZ1[2 + b] + BULLET_W, raw_rgb_to_color(0, 0, 3));
				break;
			case 2:
				write_bullet(b, recX1[2 + b] + BULLET_W, recY1[2 + b], recZ1[2 + b], recX1[2 + b] + BULLET_W, recY1[2 + b] + BULLET_W, recZ1[2 + b] + BULLET_W, raw_rgb_to_color(3, 2, 0));
				break;
			case 3:
				write_bullet(b, recX1[2 + b], recY1[2 + b] + BULLET_W, recZ1[2 + b], recX1[2 + b] + BULLET_W, recY1[2 + b] + BULLET_W, recZ1[2 + b] + BULLET_W, raw_rgb_to_color(0, 3, 3));
				break;
		}
	}
	flash = (flash + 1) % 4;
}

int PlayerHit() {
	int b;
	for (b = 0; b < 3; b ++) {
		if (0 == recZ1[b + 2]) {
			if ((recX1[0] < recX1[2 + b] && recX2[0] > recX1[2 + b]) || (recX1[0] < recX1[2 + b] + BULLET_W && recX2[0] > recX1[2 + b] + BULLET_W)) {
				if ((recY1[0] < recY1[2 + b] && recY2[0] > recY1[2 + b]) || (recY1[0] < recY1[2 + b] + BULLET_W && recY2[0] > recY1[2 + b] + BULLET_W)) {
					return 1;
				}
			}
		}
	}
	return 0;
}
  
void HotPotato() {
	int i;
	for (i = 0; i < 4; i ++) {
		write_rect(i, 0, 0, 0, 0, 0, 0, rgb_to_color(0, 0, 0));
	}
	write_rect(4, 0, 0, 0, 254, 254, 0, rgb_to_color(3, 3, 0));
	write_rect(5, 254, 0, 0, 254, 254, 254, rgb_to_color(0, 3, 0));
	int iteration = 0, waitForHit = 0;
	while (HOT_POTATO == simulation) {
		HandleKey();
		if (0 == iteration % 50) {
			MoveHotPotatoShip();
			MoveAimer();
			RenderShipAim();
		}
		if (0 == iteration % 25) {
			MoveBullets();
		}
		if (0 == iteration % 10) {
			FlashBullets();
		}
		iteration = (iteration + 1) % 1000;
	}
}

void Reset() {
	int i;
	for (i = 0; i < 84; ++i) {
        IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, i, 255);
    }
    IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_X, ClosestValid(0));
	IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_Y, ClosestValid(128));
	IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_Z, ClosestValid(10));
}

int main() {
    init_keyboard();
    printf("Program running.\n");
    IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_X, ClosestValid(0));
	IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_Y, ClosestValid(128));
	IOWR_16DIRECT(RAY_TRACER_AVALON_MODULE_INST_BASE, CAM_Z, ClosestValid(10));
    write_rect(0, 0, 0, 0, 254, 254, 0, rgb_to_color(0, 3, 0));
    int done = 0;
	while (0 == done) {
		Reset();
		switch (simulation) {
			case 0:
				HotPotato();
				break;
			case 1:
				BouncingSquare();
				break;
			case 2:
				FlashingColumns();
				break;
			case 3:
				Driving();
                break;
			case 4:
				done = 1;
                break;
            case 5:
                DontCrash();
                break;
			default:
				HotPotato();
				break;
		}
	}
	return 0;
}
