#include <stdio.h>
#include <stdlib.h>
#include "vision_utilities/vision_utilities.c"

/*** The Scene ***/

const int RECTANGLES = 6;
const int ROWS = 240;
const int COLS = 320;
const int CAMERA_X_SLOPE = 50;
const int CAMERA_X = 231, CAMERA_Y = 171, CAMERA_Z = 10;
const int INFINITE = 255;
    
// 		Rectangle 	0		1	 	2	  	3		4		5	 	6	7	 8
int recCorner1X[] = {0,		254, 	241,	241, 	241, 	200,	125,125, 125};
int recCorner1Y[] = {0,		0, 		161,	161, 	161, 	50, 	75,  75, 75};
int recCorner1Z[] = {0,  	0, 		1, 		11, 	21, 	254,	50,  50, 50};
	
int recCorner2X[] = {254,	254, 	251,	251, 	251, 	200,	125,150, 150};
int recCorner2Y[] = {254,	254, 	181,	181, 	181, 	200, 	50,  50, 75};
int recCorner2Z[] = {0,   	254, 	1,		11, 	21, 	251, 	75,  50, 75};
	
int recRed[] =		{255, 	0,		0,		0,		0,   	255,	255,  0, 255};
int recGreen[] =	{255, 	0,  	255,	255,	255,   	0,   	0,  255, 255};
int recBlue[] =		{0,   	255,  	0,		0,		0,   	255, 	255,255, 255};

int forInverting[9];
void IntToArray(int toSet) {
	int mult = 256; int index = 0;
	for (index = 0; index < 9; index ++) {
		if (toSet >= mult) {
			forInverting[index] = 1;
			toSet -= mult;
		} else {
			forInverting[index] = 0;
		}
		mult /= 2;
	}
}

void PrintIntInBinary(int toPrint, int digits) {
	int index = 0;
	if (toPrint < 0) {
		toPrint *= -1;
		IntToArray(toPrint - 1);
		for (index = 0; index < 9; index ++) {
			forInverting[index] = (forInverting[index] + 1) % 2;
		}
	} else {
		IntToArray(toPrint);
	}
	for (index = 9 - digits; index < 9; index ++) {
		printf("%d", forInverting[index]);
	}
}

/*** Intersection ***/
int MathCore(int start, int slope, int known, int startKnown, int knownSlope) {
	int coreValue = 10;
	if (knownSlope == 0) {
		coreValue = INFINITE;
	} else {
		// uncomment for interesting effects
		int returnValue = (start + slope * (known - startKnown) / knownSlope); // % INFINITE;
		if (returnValue > INFINITE || returnValue < 0) {
			coreValue = INFINITE;
		} else if ((slope <= 0 && returnValue <= start) || (slope >= 0 && returnValue >= start)) {
			coreValue = returnValue;
		} else {
			coreValue = INFINITE;
		}
	}
	return coreValue;
}

int red, green, blue; // color of intersection
int intersectX, intersectY, intersectZ; // point of intersection
int reflectX, reflectY, reflectZ; // direction of reflection

void SetIntersection(int xStart, int yStart, int zStart, int xSlope, int ySlope, int zSlope) {
	blue = 0;
	red = 0; green = 0;
	if (zSlope >= -50) {
		blue = zSlope + 50; // Background color
	}
	intersectX = INFINITE; intersectY = INFINITE; intersectZ = INFINITE;
	reflectX = xSlope; reflectY = ySlope; reflectZ = zSlope;
	int closestDistance = INFINITE, nextDistance;
	int x, y, z; // where the ray intersects the plane through this rectangle
	int rec;
	for (rec = 0; rec < RECTANGLES; rec ++) {
		if (recCorner1X[rec] == recCorner2X[rec]) {
			x = recCorner1X[rec];
			y = MathCore(yStart, ySlope, x, xStart, xSlope);
			z = MathCore(zStart, zSlope, x, xStart, xSlope);
		} else if (recCorner1Y[rec] == recCorner2Y[rec]) {
			y = recCorner1Y[rec];
			x = MathCore(xStart, xSlope, y, yStart, ySlope);
			z = MathCore(zStart, zSlope, y, yStart, ySlope);
		} else if (recCorner1Z[rec] == recCorner2Z[rec]) {
			z = recCorner1Z[rec];
			x = MathCore(xStart, xSlope, z, zStart, zSlope);
			y = MathCore(yStart, ySlope, z, zStart, zSlope);
		} else {
			printf("Rectangle %d is not parallel to x = 0, y = 0, or z = 0", rec);
			exit(1);
		}
		// Is the intersection inside our coordinate system?
		if (x >= 0 && x < INFINITE && y >= 0 && y < INFINITE && z >= 0 && z < INFINITE && x != xStart) {
			// Is the intersection actually inside the rectangle?
			if (
					(
						(x <= recCorner1X[rec] && x >= recCorner2X[rec]) ||
						(x >= recCorner1X[rec] && x <= recCorner2X[rec])
					) && (
						(y <= recCorner1Y[rec] && y >= recCorner2Y[rec]) ||
						(y >= recCorner1Y[rec] && y <= recCorner2Y[rec])
					) && (
						(z <= recCorner1Z[rec] && z >= recCorner2Z[rec]) ||
						(z >= recCorner1Z[rec] && z <= recCorner2Z[rec])
					)
				) {
				// Is the intersection closer than the previous closest?
				if (x < xStart) {
					nextDistance = xStart - x;
				} else {
					nextDistance = x - xStart;
				}
				if (nextDistance < closestDistance) {
					closestDistance = nextDistance;
					red = recRed[rec]; green = recGreen[rec]; blue = recBlue[rec];
					intersectX = x; intersectY = y; intersectZ = z;
					reflectX = xSlope; reflectY = ySlope; reflectZ = zSlope;
					if (recCorner1X[rec] == recCorner2X[rec]) {
						reflectX *= -1;
					} else if (recCorner1Y[rec] == recCorner2Y[rec]) {
						reflectY *= -1;
					} else if (recCorner1Z[rec] == recCorner2Z[rec]) {
						reflectZ *= -1;
					}
				}
			}
		}
	}
}

void PrintSingleCoord(int value, int location) {
	// for buffered_line_generator_tester.vhd
	/* printf("\t\telsif rect_memory_address_delayed = \"");
	PrintIntInBinary(location, 8);
	printf("\" then\n\t\t\trect_memory_data <= \"");
	PrintIntInBinary(value, 8);
	printf("\";\n"); */
	// for ray_tracer_avalon_module_tester.vhd
	printf("\t\taddress <= \"");
	PrintIntInBinary(location + 3, 7);
	printf("\"; writedata <= \"00000000");
	PrintIntInBinary(value, 8);
	printf("\"; write <= '1'; wait for 20 ns; write <= '0'; wait for 20 ns;\n");
}

/*** Run the ray trace ***/
void PrintRecCoord(int index) {
	int baseLocation = 7 * index;
	PrintSingleCoord(recCorner1X[index], baseLocation);
	PrintSingleCoord(recCorner1Y[index], baseLocation + 1);
	PrintSingleCoord(recCorner1Z[index], baseLocation + 2);
	PrintSingleCoord(recCorner2X[index], baseLocation + 3);
	PrintSingleCoord(recCorner2Y[index], baseLocation + 4);
	PrintSingleCoord(recCorner2Z[index], baseLocation + 5);
	int colorValue = 0;
	if (recRed[index] == 255) { colorValue += 3 * 4 * 4; }
	if (recGreen[index] == 255) { colorValue += 3 * 4; }
	if (recBlue[index] == 255) { colorValue += 3; }
	PrintSingleCoord(colorValue, baseLocation + 6);
}

const int TEST_PIXEL_SPACE = 29;
int main() {
	ImageColor newImg;
	int rec, col, row;
	setSizeColor(&newImg, ROWS, COLS);
	setColorsColor(&newImg, 255);
	
	int imageRow, imageColumn;
	int currentRed, currentGreen, currentBlue;
	int shouldPrint = 0;
	
	int i;
	for (i = 0; i < 6; i ++) {
		PrintRecCoord(i);
	}
	
	printf("\n\t\tcamera_x => \"");
	PrintIntInBinary(CAMERA_X, 9);
	printf("\", camera_y => \"");
	PrintIntInBinary(CAMERA_Y, 9);
	printf("\", camera_z => \"");
	PrintIntInBinary(CAMERA_Z, 9);
	printf("\",\n\n");
		
	for (row = ROWS - 50; row > -50; row --) {
		imageRow = ROWS - (row + 50);
		shouldPrint = 0;
		/* printf("\t\twait until done = '1'; row <= \"");
		PrintIntInBinary(row - 1, 9);
		printf("\";\n");*/
		if (imageRow % 10 == 0 && imageRow != 0) {
			shouldPrint = 1;
		}
		for (col = - COLS / 2; col < COLS / 2; col ++) {
			imageColumn = col + COLS / 2;
			SetIntersection(CAMERA_X, CAMERA_Y, CAMERA_Z, CAMERA_X_SLOPE, col, row);
			currentRed = red / 2; currentGreen = green / 2; currentBlue = blue / 2;
			// better saturation
			// currentRed = 3 * red / 4; currentGreen = 3 * green / 4; currentBlue = 3 * blue / 4;
			// 1 reflection
			SetIntersection(intersectX, intersectY, intersectZ, reflectX, reflectY, reflectZ);
			currentRed += red / 4; currentGreen += green / 4; currentBlue += blue / 4;
			currentRed = 4 * (currentRed / 4);
			currentGreen = 4 * (currentGreen / 4);
			currentBlue = 4 * (currentBlue / 4);
			setPixelColor(&newImg, imageRow, imageColumn, currentRed, currentGreen, currentBlue);
			/* if (shouldPrint == 1) {
				printf("\t\tscreen_col_to_read <= \"");
				PrintIntInBinary(imageColumn, 9);
				printf("\"; wait for 40 ns;\n");
				if (imageColumn > 0) {
					printf("\t\tassert screen_col_color = \"");
					PrintIntInBinary(currentRed / 4, 6);
					PrintIntInBinary(currentGreen / 4, 6);
					PrintIntInBinary(currentBlue / 4, 6);
					printf("\" report \"row %d, col %d r,g,b = %d, %d, %d\" severity error;\n",
						row, col, currentRed / 4, currentGreen / 4, currentBlue / 4);
					
				}
			}*/
		}
	}
	printf("\t\twait until done = '1';\n");
	writeImageColor(&newImg, "RayTrace.pmg");
	
	for (i = 0; i < 30; i ++) {
		printf("\t\telsif screen_col < %d then\n\t\t\tread_c_o_temp := screen_col - %d;\n\t\t\tcolor := read_%d;\n", (i + 1) * 11, i * 11, i);
	}
	
	for (i = 0; i < 30; i ++) {
		printf("col_base%d <= \"", i);
		PrintIntInBinary(i * 11 - 160, 9);
		printf("\"; ", i);
	}
	return 0;
}
