// Pathfinding Elements
// Worthless Bums 2012.


// Heuristic for estimating cost in A* pathfinding algorithim.
function heuristic_cost_estimate (%startNode, %goalNode)
{//echo("heuristic_cost_estimate()");
	// Nothing here yet!
}
// Returns the node in the open set with the lowest f_score.
function getLowestFScoreNodeInOpenSet ()
{//echo("getLowestFScoreNodeInOpenSet()");

}
// Returns true iff the specified node is in the closed set.
function isNodeInClosedSet (%node)
{//echo("isNodeInClosedSet()");

}
// Returns true iff the specified node is in the open set.
function isNodeInOpenSet (%node)
{//echo("isNodeInOpenSet()");

}
// Returns a path to the target, if any.
// Note: %start and %goal should be tile indice pairs.
function getAStarPath (%start, %goal)
{//echo("getAStarPath()");
	// Sets of nodes.
	// NOTE: This should all be initialized at startup to save time!
	$closedSet[$mazeDimension, $mazeDimension] = 1;			// The set of nodes already evaluated.
	$closedSetSize = 0;
	
	$openSet[$mazeDimension, $mazeDimension] = 1;			// The set of tentative nodes to be evaluated, initially containing the start node.
	$openSet[getWord(%start, 0), getWord(%start, 1)] = 0;	// Add the start node to the open set.
	$openSetSize = 1;
	
    $came_from[$mazeDimension, $mazeDimension] = 1;	   		// The map of navigated nodes.
	$came_fromSize = 0;
	
	// Cost from start along best known path.
    $g_score[$mazeDimension, $mazeDimension] = 1;
	$g_score[getWord(%start, 0), getWord(%start, 1)] = 0;
	// Heuristic cost estimae.
    $h_score[$mazeDimension, $mazeDimension] = 1;
	$h_score[getWord(%start, 0), getWord(%start, 1)] = heuristic_cost_estimate(%start, %goal);
	// Estimated total cost from start to goal through y.
	$f_score[$mazeDimension, $mazeDimension] = 1;
	$f_score[getWord(%start, 0), getWord(%start, 1)] = %g_score[getWord(%start, 0), getWord(%start, 1)] + %h_score[getWord(%start, 0), getWord(%start, 1)];
    //%f_score[%start] = g_score[%start] + h_score[%start];
	
    while ($openSetSize > 0)
	{
        %current = getLowestFScoreNodeInOpenSet();//current := the node in openset having the lowest f_score[] value
        if (%current == %goal)		// Both should be index pairs!
		{
            //return reconstruct_path($came_from, $came_from[getWord(%goal, 0), getWord(%goal, 1)]);
			
			// Just return the next step!
			return $came_from[$mazeDimension, $mazeDimension];
		}
 
        $openSet[getWord(%current, 0), getWord(%current, 1)] = 1;//remove current from openset
        $closedSet[getWord(%current, 0), getWord(%current, 1)] = 0;//add current to closedset
		
		// Check NSEW cardinal directions from the current node.
        //for each neighbor in neighbor_nodes(current)
		if (true)
		{
			// Construct north neighbor.
			if (getWord(%current, 1) - 1 >= 0)
			{
				%neighbor = getWord(%current, 0) SPC getWord(%current, 1) - 1;
				
				/*if neighbor in closedset
				{
					continue
				}*/
				
				// If neighbor is NOT in closedSet then keep going.
				if (!isNodeInClosedSet(%neighbor))
				{
					%tentative_g_score = $g_score[getWord(%current, 0), getWord(%current, 1)] + VectorDist(%current, %neighbor);//dist_between(%current, %neighbor)
					
					if (!isNodeInOpenSet())//if neighbor not in openset
					{
						$closedSet[getWord(%neighbor, 0), getWord(%neighbor, 1)] = 0;//add neighbor to openset
						$h_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = heuristic_cost_estimate(%neighbor, %goal);//h_score[neighbor] := heuristic_cost_estimate(neighbor, goal)
						%tentative_is_better = true;//tentative_is_better := true
					}
					else if (%tentative_g_score < $g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)])//tentative_g_score < g_score[neighbor]
					{
						%tentative_is_better = true;
					}
					else
					{
						%tentative_is_better = false;
					}
		 
					if (%tentative_is_better == true)//tentative_is_better = true
					{
						$came_from[getWord(%neighbor, 0), getWord(%neighbor, 1)] = %current;//came_from[neighbor] := current
						$g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = %tentative_g_score;//g_score[neighbor] := %tentative_g_score
						$f_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = $g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] + $h_score[getWord(%neighbor, 0), getWord(%neighbor, 1)];//f_score[neighbor] := g_score[neighbor] + h_score[neighbor]
					}
				}
			}
			
			// Construct south neighbor.
			if (getWord(%current, 1) - 1 >= 0)
			{
				%neighbor = getWord(%current, 0) SPC getWord(%current, 1) + 1;
				
				/*if neighbor in closedset
				{
					continue
				}*/
				
				// If neighbor is NOT in closedSet then keep going.
				if (!isNodeInClosedSet(%neighbor))
				{
					%tentative_g_score = $g_score[getWord(%current, 0), getWord(%current, 1)] + VectorDist(%current, %neighbor);//dist_between(%current, %neighbor)
					
					if (!isNodeInOpenSet())//if neighbor not in openset
					{
						$closedSet[getWord(%neighbor, 0), getWord(%neighbor, 1)] = 0;//add neighbor to openset
						$h_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = heuristic_cost_estimate(%neighbor, %goal);//h_score[neighbor] := heuristic_cost_estimate(neighbor, goal)
						%tentative_is_better = true;//tentative_is_better := true
					}
					else if (%tentative_g_score < $g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)])//tentative_g_score < g_score[neighbor]
					{
						%tentative_is_better = true;
					}
					else
					{
						%tentative_is_better = false;
					}
		 
					if (%tentative_is_better == true)//tentative_is_better = true
					{
						$came_from[getWord(%neighbor, 0), getWord(%neighbor, 1)] = %current;//came_from[neighbor] := current
						$g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = %tentative_g_score;//g_score[neighbor] := %tentative_g_score
						$f_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = $g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] + $h_score[getWord(%neighbor, 0), getWord(%neighbor, 1)];//f_score[neighbor] := g_score[neighbor] + h_score[neighbor]
					}
				}
			}
			
			// Construct east neighbor.
			if (getWord(%current, 1) - 1 >= 0)
			{
				%neighbor = getWord(%current, 0) + 1 SPC getWord(%current, 1);
				
				/*if neighbor in closedset
				{
					continue
				}*/
				
				// If neighbor is NOT in closedSet then keep going.
				if (!isNodeInClosedSet(%neighbor))
				{
					%tentative_g_score = $g_score[getWord(%current, 0), getWord(%current, 1)] + VectorDist(%current, %neighbor);//dist_between(%current, %neighbor)
					
					if (!isNodeInOpenSet())//if neighbor not in openset
					{
						$closedSet[getWord(%neighbor, 0), getWord(%neighbor, 1)] = 0;//add neighbor to openset
						$h_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = heuristic_cost_estimate(%neighbor, %goal);//h_score[neighbor] := heuristic_cost_estimate(neighbor, goal)
						%tentative_is_better = true;//tentative_is_better := true
					}
					else if (%tentative_g_score < $g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)])//tentative_g_score < g_score[neighbor]
					{
						%tentative_is_better = true;
					}
					else
					{
						%tentative_is_better = false;
					}
		 
					if (%tentative_is_better == true)//tentative_is_better = true
					{
						$came_from[getWord(%neighbor, 0), getWord(%neighbor, 1)] = %current;//came_from[neighbor] := current
						$g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = %tentative_g_score;//g_score[neighbor] := %tentative_g_score
						$f_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = $g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] + $h_score[getWord(%neighbor, 0), getWord(%neighbor, 1)];//f_score[neighbor] := g_score[neighbor] + h_score[neighbor]
					}
				}
			}
			
			// Construct west neighbor.
			if (getWord(%current, 1) - 1 >= 0)
			{
				%neighbor = getWord(%current, 0) - 1 SPC getWord(%current, 1);
				
				/*if neighbor in closedset
				{
					continue
				}*/
				
				// If neighbor is NOT in closedSet then keep going.
				if (!isNodeInClosedSet(%neighbor))
				{
					%tentative_g_score = $g_score[getWord(%current, 0), getWord(%current, 1)] + VectorDist(%current, %neighbor);//dist_between(%current, %neighbor)
					
					if (!isNodeInOpenSet())//if neighbor not in openset
					{
						$closedSet[getWord(%neighbor, 0), getWord(%neighbor, 1)] = 0;//add neighbor to openset
						$h_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = heuristic_cost_estimate(%neighbor, %goal);//h_score[neighbor] := heuristic_cost_estimate(neighbor, goal)
						%tentative_is_better = true;//tentative_is_better := true
					}
					else if (%tentative_g_score < $g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)])//tentative_g_score < g_score[neighbor]
					{
						%tentative_is_better = true;
					}
					else
					{
						%tentative_is_better = false;
					}
		 
					if (%tentative_is_better == true)//tentative_is_better = true
					{
						$came_from[getWord(%neighbor, 0), getWord(%neighbor, 1)] = %current;//came_from[neighbor] := current
						$g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = %tentative_g_score;//g_score[neighbor] := %tentative_g_score
						$f_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] = $g_score[getWord(%neighbor, 0), getWord(%neighbor, 1)] + $h_score[getWord(%neighbor, 0), getWord(%neighbor, 1)];//f_score[neighbor] := g_score[neighbor] + h_score[neighbor]
					}
				}
			}
		}
	}
	
	// Failed to find a path from start to goal!
	
    return false;
}
// Reconstructs the path from the specified nodes.
function reconstruct_path (%came_from, %current_node)
{//echo("reconstruct_path()");
    //if came_from[current_node] is set
	if (true)
	{
        //p := reconstruct_path(came_from, came_from[current_node])
		%p = reconstruct_path(%came_from, %came_from[getWord(%current_node, 0), getWord(%current_node, 1)]);
		
        //return (p + current_node)
		%pX = getWord(%p, 0) + getWord(%current_node, 0);
		%pY = getWord(%p, 1) + getWord(%current_node, 1);
		return %pX SPC %pY;
	}
    else
	{
        return %current_node;
	}
}