/****************************************************************
* Title: A* Pathfinding
*
*
* Description: This program utilizes the A* pathfinding algorithm
* to search a user generated maze, via 2D arrays.  Some numbers to
* be familiar with are:
*      1 = WALL
*      2 = START NODE
*      5 = PATH
*      9 = END NODE
*****************************************************************/

// USER: Define WIDTH and HEIGHT for 2D Maze
#define HEIGHT 10
#define WIDTH  10

#include <iostream>
#include <string>

using namespace std;

void findHCost();
void findLowestCost(int x, int y);
void findSolution(int caseSelection);

void updateCostRadius(int x, int y);

void addToOpenList(int x, int y);
void addToClosedList(int x, int y);

void printHCost();
void printFCost();
void printOpenList();
void printClosedList();

// USER: Create maze based off of the defines
int map[HEIGHT][WIDTH] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1,				//    1 = WALL 
                          1, 2, 1, 0, 0, 0, 0, 0, 0, 1,                         //    2 = Current Location     
			  1, 0, 1, 0, 1, 1, 1, 1, 0, 1,                         //    5 = PATH (PARENT)
			  1, 0, 1, 0, 1, 9, 1, 1, 0, 1,				//    9 = END POSITION  
			  1, 0, 1, 0, 1, 0, 0, 1, 0, 1,
			  1, 0, 1, 0, 1, 1, 0, 1, 0, 1,
			  1, 0, 1, 0, 0, 0, 0, 1, 0, 1,
			  1, 0, 1, 1, 1, 1, 1, 1, 0, 1,
			  1, 0, 0, 0, 0, 0, 0, 0, 0, 1,
			  1, 1, 1, 1, 1, 1, 1, 1, 1, 1 };

// Used to store the cost for the heuristic
int HCost[HEIGHT][WIDTH];

// Used to store the total cost of movement (F = G + H)
// I don't calculate G independently due to it only being involved with the radius of the current node
int FCost[HEIGHT][WIDTH];

// Used to store the movement track from the Start node to the End
int closedList[HEIGHT][WIDTH];

// Used to store the radius cost of the surrounding nodes
int openList[HEIGHT][WIDTH];

bool solved = false;

// Coords of the current parent
int parentNodeX;
int parentNodeY;

// Case Selection
int caseSelection;

// Error Checking
int stuckCount;
int verifyStartNode;
int verifyEndNode;

/****************************************************************
* Function Name: main(void)
*
* Description: The glorious main() function! Gateway to happiness!
*****************************************************************/
int main()
{
	bool menu = true;
	int selection = 0;

	cout << "===================================================" << endl;
	cout << "Developed by David McGraw"                           << endl;
	cout << "===================================================" << endl;
	cout << "The Current Map (modify map in main.cpp):"   << endl << endl;

	// Print-Verify Map
	for(int x = 0; x < HEIGHT; x++)
	{
		for(int y = 0; y < WIDTH; y++)
		{
			if(map[x][y] == 9)
			{
				verifyEndNode += 1;
			}
			if(map[x][y] == 2)
			{
				verifyStartNode += 1;
			}

			cout << map[x][y] << " ";
		}
		cout << endl;
	}
	cout << endl;

	// Verify Start and End nodes exist
	if(verifyEndNode == 0 && verifyStartNode == 0)
	{
		cout << "You are MISSING the end node, and start node. Please Fix, and restart this application." << endl;
		return 0;
	}
	if(verifyEndNode == 0)
	{
		cout << "You are MISSING the end node. Please Fix, and restart this application." << endl;
		return 0;
	}
	if(verifyStartNode == 0)
	{
		cout << "You are MISSING the start node. Please Fix, and restart this application." << endl;
		return 0;
	}


	while(menu)
	{
		        if(solved == true) 
			{ 
				cout << "Please Restart to Play Again!" << endl;
				return 0; 
			}

			cout << endl << "Menu Options:" << endl;
			cout << "1. Find Solution (Quick)" << endl;
			cout << "2. Find Solution (Detailed)" << endl;
			cout << "3. Exit" << endl;
			cout << endl << "Enter Selection: ";
			cin >> selection;

			if(selection > 2)
				cout << "Please enter a valid selection" << endl;
			else
			{
				switch(selection)
				{
				case 1:
					caseSelection = 1;
					findSolution(1);
					break;
				case 2:
					caseSelection = 2;
					findSolution(2);
					break;
				case 3:
					menu = false;
					break;				
				}
			}
	}

	return 0;
}

/****************************************************************
* Function Name: findSolution(int)
*
* Description: Let's find us a solution!
*
* @int caseSelection - Used to print a detailed view or quick view
*****************************************************************/
void findSolution(int caseSelection)
{
	// Find the cost for the Heuristic
	findHCost();

	// Find the Start Node
	for(int x = 0; x < HEIGHT; x++)
	{
		for(int y = 0; y < WIDTH; y++)
		{
			if(map[x][y] == 2)
			{
				addToClosedList(x, y);
				parentNodeX = x;
				parentNodeY = y;
				break;
			}
		}
	}

	while(!solved)
	{
		// Init Stuck Count
		stuckCount = 0;

		// Update Cost Radius
		updateCostRadius(parentNodeX, parentNodeY);

		// Search for the Lowest Movement Cost
		findLowestCost(parentNodeX, parentNodeY);

		if(caseSelection == 2)
		{
			cout << "------------" << endl;
			printClosedList();
			system("PAUSE");
		}

		if(stuckCount == 8)
		{
			cout << "------------------------------" << endl;
			cout << "Failed To Find Path... Exiting" << endl;
			cout << "------------------------------" << endl;
			solved = true;
		}
	}

	cout << endl << "========" << endl;
	cout << "Solution" << endl;
	cout << "========" << endl;
	printClosedList();

	system("PAUSE");
}

/****************************************************************
* Function Name: findLowestCost(int, int)
*
* Description: Searches around the current node to locate the lowest
* movement cost. Once found, that node is then added to the closed
* list, and thus becoming the new parent node.
*
* @int x - x coord
* @int y - y coord
*****************************************************************/
void findLowestCost(int x, int y)
{
	int lowestCost = 0;
	int lowestX = 0;
	int lowestY = 0;

	// Look around to see if the end is near us
	if(FCost[x-1][y-1] == 9 || FCost[x-1][y] == 9 || FCost[x-1][y+1] == 9||
		FCost[x][y-1] == 9 || FCost[x][y+1] == 9 || 
		FCost[x+1][y-1] == 9 || FCost[x+1][y] == 9 || FCost[x+1][y+1] == 9)
	{
		addToClosedList(x, y);
		cout << "==============" << endl;
		cout << "Solution Found" << endl;
		cout << "==============" << endl;
		solved = true;
	}

	// Look at the top left
	if(FCost[x-1][y-1] != 1)
	{
		// if Lowest Cost hasn't been set
		if(lowestCost == 0 && closedList[x-1][y-1] != 5)
		{
			lowestCost = FCost[x-1][y-1];
			lowestX = x-1;
			lowestY = y-1;
		}

		if(lowestCost > FCost[x-1][y-1])
		{
			if(closedList[x-1][y-1] != 5)
			{
				lowestCost = FCost[x-1][y-1]; lowestX = x-1; lowestY = y-1;
			}
		}
		else if(closedList[x-1][y-1] == 5)
		   stuckCount += 1;
	}
	else
		stuckCount += 1;

	// Look at the top middle... Bigger than the top left? Hmm, Next.. Otherwise, we've found the new smallest
	if(FCost[x-1][y] != 1)
	{
		// if Lowest Cost hasn't been set
		if(lowestCost == 0 && closedList[x-1][y] != 5)
		{
			lowestCost = FCost[x-1][y];
			lowestX = x-1;
			lowestY = y;
		}

		if(lowestCost > FCost[x-1][y])
		{
			// Is this position on the closed list already?
			if(closedList[x-1][y] != 5)
			{
				lowestCost = FCost[x-1][y]; lowestX = x-1; lowestY = y;
			}
			// else, it is, so ignore it
		}
		else if(closedList[x-1][y] == 5)
		   stuckCount += 1;
		// else, it is, so ignore it
	}
	else
		stuckCount += 1;

	// Look at the top right... Bigger than the smallest? ..
	if(FCost[x-1][y+1] != 1 && closedList[x-1][y+1] != 5)
	{
		// if Lowest Cost hasn't been set
		if(lowestCost == 0)
		{
			lowestCost = FCost[x-1][y+1];
			lowestX = x-1;
			lowestY = y+1;
		}

		if(lowestCost > FCost[x-1][y+1])
		{
			if(closedList[x-1][y+1] != 5)
			{
				lowestCost = FCost[x-1][y+1]; lowestX = x-1; lowestY = y+1;
			}
		}
		else if(closedList[x-1][y+1] == 5)
		   stuckCount += 1;
	}
	else
		stuckCount += 1;

	if(FCost[x][y-1] != 1)
	{
		// if Lowest Cost hasn't been set
		if(lowestCost == 0  && closedList[x][y-1] != 5)
		{
			lowestCost = FCost[x][y-1];
			lowestX = x;
			lowestY = y-1;
		}

		if(lowestCost > FCost[x][y-1])
		{
			if(closedList[x][y-1] != 5)
			{
			lowestCost = FCost[x][y-1]; lowestX = x; lowestY = y-1;
			}
		}
		else if(closedList[x][y-1] == 5)
		   stuckCount += 1;
	}
	else
		stuckCount += 1;

	if(FCost[x][y+1] != 1)
	{
		// if Lowest Cost hasn't been set
		if(lowestCost == 0 && closedList[x][y+1] != 5)
		{
			lowestCost = FCost[x][y+1];
			lowestX = x;
			lowestY = y+1;
		}

		if(lowestCost > FCost[x][y+1])
		{
			if(closedList[x][y+1] != 5)
			{
				lowestCost = FCost[x][y+1]; lowestX = x; lowestY = y+1;
			}
		}
		else if(closedList[x][y+1] == 5)
		   stuckCount += 1;
	}
	else
		stuckCount += 1;

	if(FCost[x+1][y-1] != 1)
	{
		// if Lowest Cost hasn't been set
		if(lowestCost == 0 && closedList[x+1][y-1] != 5)
		{
			lowestCost = FCost[x+1][y-1];
			lowestX = x+1;
			lowestY = y-1;
		}

		if(lowestCost > FCost[x+1][y-1]) 
		{
			if(closedList[x+1][y-1] != 5)
			{
				lowestCost = FCost[x+1][y-1]; lowestX = x+1; lowestY = y-1;
			}
		}
		else if(closedList[x+1][y-1] == 5)
		   stuckCount += 1;
	}
	else
		stuckCount += 1;

	if(FCost[x+1][y] != 1)
	{
		// if Lowest Cost hasn't been set
		if(lowestCost == 0 && closedList[x+1][y] != 5)
		{
			lowestCost = FCost[x+1][y];
			lowestX = x+1;
			lowestY = y;
		}

		if(lowestCost > FCost[x+1][y])
		{
			if(closedList[x+1][y] != 5)
			{
				lowestCost = FCost[x+1][y]; lowestX = x+1; lowestY = y;
			}
		}
		else if(closedList[x+1][y] == 5)
		   stuckCount += 1;
	}
	else
		stuckCount += 1;

	if(FCost[x+1][y+1] != 1)
	{
		// if Lowest Cost hasn't been set
		if(lowestCost == 0 && closedList[x+1][y+1] != 5)
		{
			lowestCost = FCost[x+1][y+1];
			lowestX = x+1;
			lowestY = y+1;
		}

		if(lowestCost > FCost[x+1][y+1])
		{
			if(closedList[x+1][y+1] != 5)
			{
				lowestCost = FCost[x+1][y+1]; lowestX = x+1; lowestY = y+1;
			}
		}
	    else if(closedList[x-1][y+1] == 5)
		   stuckCount += 1;
	}
	else
		stuckCount += 1;

	// Add lowest cost to the closed list (this is our path)
	addToClosedList(lowestX, lowestY);
}

/****************************************************************
* Function Name: updateCostRadius(int, int)
*
* Description: Update the cost around the current node. Add 10 to H for 
* positions left, right, up, and down.  Add 15 to H for diagonal positions
*
* @int x - x coord
* @int y - y coord
*****************************************************************/
void updateCostRadius(int x, int y)
{
	if(caseSelection == 2)
	{
		cout << endl;
		cout << "Location of NODE: X = " << x << " : Y = " << y << endl; 
		cout << "Printing COST Around NODE" << endl;
	}

	// Add G to H, LEFT RIGHT UP DOWN cost of 10
	if(map[x-1][y] != 9) 
	{ 
		if(map[x-1][y] == 1)
		{
			FCost[x-1][y] = 1;
		}
		else 
		{
			FCost[x-1][y] = 10 + HCost[x-1][y]; 
			addToOpenList(x-1, y); 
		}
	} else { 
		if (map[x-1][y] == 9) { FCost[x-1][y] = 9; } 
	}																					
	// UP
	if(map[x][y-1] != 9) 
	{
		if(map[x][y-1] == 1)
		{
			FCost[x][y-1] = 1;
		}
		else
		{
			FCost[x][y-1] = 10 + HCost[x][y-1]; 
			addToOpenList(x, y-1);
		}
	} else { 
		if(map[x][y-1] == 9) { FCost[x][y-1] = 9; }  
	}																				
	// LEFT
	if(map[x+1][y] != 9) 
	{ 
		if(map[x+1][y] == 1)
		{
			FCost[x+1][y] = 1;
		}
		else
		{
			FCost[x+1][y] = 10 + HCost[x+1][y]; 
			addToOpenList(x+1, y);
		}
	} else { 
		if(map[x+1][y] == 9) { FCost[x+1][y] = 9; } 
	}																				   
	// DOWN
	if(map[x][y+1] != 9) 
	{ 
		if(map[x][y+1] == 1)
		{
			FCost[x][y+1] = 1;
		}
		else
		{
			FCost[x][y+1] = 10 + HCost[x][y+1]; 
			addToOpenList(x, y+1);
		}
	} else {
		if(map[x][y+1] == 9) { FCost[x][y+1] = 9; }  
	}																				   
	// RIGHT
	// Add G to H, Diag Movement cost of 15
	if(map[x-1][y-1] != 9) 
	{ 
		if(map[x-1][y-1] == 1)
		{
			FCost[x-1][y-1] = 1;
		}
		else
		{
			FCost[x-1][y-1] = 15 + HCost[x-1][y-1]; 
			addToOpenList(x-1, y-1);
		}
	} else { 
		if(map[x-1][y-1] == 9){ FCost[x-1][y-1] = 9; } 
	}																				  
	// Top Left
	if(map[x-1][y+1] != 9) 
	{ 
		if(map[x-1][y+1] == 1)
		{
			FCost[x-1][y+1] = 1;
		}
		else
		{
			FCost[x-1][y+1] = 15 + HCost[x-1][y+1]; 
			addToOpenList(x-1, y+1);
		}
	} else { 
		if(map[x-1][y+1] == 9) { FCost[x-1][y+1] = 9; } 
	}																				  
	// Top Right
	if(map[x+1][y+1] != 9) 
	{ 
		if(map[x+1][y+1] == 1)
		{
			FCost[x+1][y+1] = 1;
		}
		else
		{
			FCost[x+1][y+1] = 15 + HCost[x+1][y+1]; 
			addToOpenList(x+1, y+1);
		}
	} else { 
		if(map[x+1][y+1] == 9) { FCost[x+1][y+1] = 9; }
	}																				 
	// Bottom Right
	if(map[x+1][y-1] != 9) 
	{ 
		if(map[x+1][y-1] == 1)
		{
			FCost[x+1][y-1] = 1;
		}
		else
		{
			FCost[x+1][y-1] = 15 + HCost[x+1][y-1]; 
			addToOpenList(x+1, y-1);
		}
	} else { 
		if(map[x+1][y-1] == 9) { FCost[x+1][y-1] = 9; } 
	}																				
	// Bottom Left
	// Print Detailed
	if(caseSelection == 2)
	{
		// Print Radius 
		cout << FCost[x-1][y-1] << " " << FCost[x-1][y] << " " << FCost[x-1][y+1] << endl;
		cout << FCost[x][y-1] << " " << "N  " << FCost[x][y+1] << endl;
		cout << FCost[x+1][y-1] << " " << FCost[x+1][y] << " " << FCost[x+1][y+1] << endl;
	}

}

/****************************************************************
* Function Name: addToClosedList(int, int)
*
* Description: Adds position to the Closed List.  This list is used
* to determine the corrent path.
* 
* @int x - x coord
* @int y - y coord
*****************************************************************/
void addToClosedList(int x, int y)
{
	if(closedList[x][y] != 5)
	{
		closedList[x][y] = 5;
		parentNodeX = x;
		parentNodeY = y;
	}
	// else skip
}

/****************************************************************
* Function Name: addToOpenList(int, int)
*
* Description: Adds position to the Open List.  This List is used
* to determine active nodes around the current (parent) node.
* 
* @int x - x coord
* @int y - y coord
*****************************************************************/
void addToOpenList(int x, int y)
{
	openList[x][y] = FCost[x][y];
}

/****************************************************************
* Function Name: findHCost(void)
*
* Description: H is the Heuristic 'Manhattan Method', which calculates 
* the movement cost from the end node, through the entire map. Each 
* movement is n*10.  This method ignores all obstacle's, so even walls
* will be given a H cost from the end node.
*****************************************************************/
void findHCost()
{
	int curLoc = 0;
	int endNodeX = 0;
	int endNodeY = 0;
	int yCount = 0;

	for(int x = 0; x < HEIGHT; x++)
	{
		for(int y = 0; y < WIDTH; y++)
		{
			if(map[x][y] == 9) // We're at the end node
			{
				// Temp Variables
				endNodeX = x;
				endNodeY = y;
				yCount = y;

				// Look Up
				for(int a = x; a != 0; a--)
				{
					endNodeX--;
					HCost[endNodeX][endNodeY] = (HCost[endNodeX+1][endNodeY] + 10);

					// Fill Left
					for(int b = y; b != 0; b--)
					{
						endNodeY--;
						HCost[endNodeX][endNodeY] = (HCost[endNodeX][endNodeY+1] + 10);
					}

					endNodeY = y;

					// Fill Right
					for(int b = y; b != WIDTH-1; b++)
					{
						endNodeY++;
						HCost[endNodeX][endNodeY] = (HCost[endNodeX][endNodeY-1] + 10);
					}
					endNodeY = y;
				}

				endNodeX = x;
				endNodeY = y;

				// Look Down
				for(int a = x; a != HEIGHT-1; a++)
				{
					endNodeX++;
					HCost[endNodeX][endNodeY] = (HCost[endNodeX-1][endNodeY] + 10);

					// Fill Left
					for(int b = y; b != 0; b--)
					{
						endNodeY--;
						HCost[endNodeX][endNodeY] = (HCost[endNodeX][endNodeY+1] + 10);
					}

					endNodeY = y;

					// Fill Right
					for(int b = y; b != WIDTH-1; b++)
					{
						endNodeY++;
						HCost[endNodeX][endNodeY] = (HCost[endNodeX][endNodeY-1] + 10);
					}
					endNodeY = y;
				}

				endNodeX = x;
				endNodeY = y;

				// Fill Horizontal Line that is on the End Node
				// Fill Left
				for(int b = y; b != 0; b--)
				{
					endNodeY--;
					HCost[endNodeX][endNodeY] = (HCost[endNodeX][endNodeY+1] + 10);
				}

				endNodeY = y;

				// Fill Right
				for(int b = y; b != WIDTH-1; b++)
				{
					endNodeY++;
					HCost[endNodeX][endNodeY] = (HCost[endNodeX][endNodeY-1] + 10);
				}
				endNodeY = y;

			break;
			}

		}
	}
}

/****************************************************************
* Function Name: printHCost(void)
*
* Description: Print Cost for H  (F = G + H)
*****************************************************************/
void printHCost()
{
	for(int x = 0; x < HEIGHT; x++)
	{
		for(int y = 0; y < WIDTH; y++)
		{
			cout << HCost[x][y] << " ";
		}
		cout << endl;
	}
}

/****************************************************************
* Function Name: printFCost(void)
*
* Description: Print Cost for F  (F = G + H)
*****************************************************************/
void printFCost()
{
	for(int x = 0; x < HEIGHT; x++)
	{
		for(int y = 0; y < WIDTH; y++)
		{
			cout << FCost[x][y] << " ";
		}
		cout << endl;
	}
}

/****************************************************************
* Function Name: printOpenList(void)
*
* Description: Print Open List
*****************************************************************/
void printOpenList()
{
	for(int x = 0; x < HEIGHT; x++)
	{
		for(int y = 0; y < WIDTH; y++)
		{
			cout << openList[x][y] << " ";
		}
		cout << endl;
	}
}

/****************************************************************
* Function Name: printClosedList(void)
*
* Description: Print Closed List
*****************************************************************/
void printClosedList()
{
	for(int x = 0; x < HEIGHT; x++)
	{
		for(int y = 0; y < WIDTH; y++)
		{
			cout << closedList[x][y] << " ";
		}
		cout << endl;
	}

}