import java.awt.Point;
import java.util.ArrayList;

public class AStar {
	public static Point[] findPath(double[][] map, Point start, Point finish) {
		// These are for running A*.
		Point current = null;
		ArrayList  candidates = new ArrayList();
		Point[][] parent = new Point[map.length][map[0].length];
		double[][] cost = new double[map.length][map[0].length]; // All costs start at infinity, but Java inits them to zero.
		double[][] costToGoal = new double[map.length][map[0].length];
		
		// Set initial costs to infinity.
		for(int i=0; i < map.length; i++) {
			for(int j=0; j < map[i].length; j++) {
				cost[i][j] = Float.POSITIVE_INFINITY;
				costToGoal[i][j] = Float.POSITIVE_INFINITY;
			}
		}
		
		// We make start its own parent so that we know it's visited.  
		// This may have problems when reversing, but we can safely do so because we have the start point as an argument.
		parent[start.x][start.y] = start;
		cost[start.x][start.y] = 0;
		costToGoal[start.x][start.y] = heuristic(map, start.x, start.y, finish.x, finish.y);
		
		candidates.add(start);
		while(!candidates.isEmpty()) {
			// Do an O(n) search for the smallest candidate.  In a densely connected graph, a minheap may drive the cost to O(n^2 log n).
			// Since this isn't really densely connected, we could switch for a performance boost.
			Point minCostCandidate = null;
			double minCost = Float.POSITIVE_INFINITY;
			for(Point p : candidates) {
				if(cost[p.x][p.y] + costToGoal[p.x][p.y] < minCost) {
					minCostCandidate = p;
					minCost = cost[p.x][p.y] + costToGoal[p.x][p.y];
				}
			}
			candidates.remove(minCostCandidate);
			current = minCostCandidate;
			
			if(current.equals(finish)) {
				break;
			}
			
			// We've got the shortest path so far.  Now let's figure out the cost of the eight items around this candidate.
			// If a neighbor has a _higher_ cost, make this node the parent and decrease the cost.
			for(int dy=-1; dy<2; dy++) {
				for(int dx=-1; dx<2; dx++) {
					// Short-hand for our neighbor's location.
					int nbrx = current.x+dx;
					int nbry = current.y+dy;
					
					// Make sure it's a valid spot on the map.
					if(dx==0 && dy==0) { continue; } // Skip the current point.  Can't be your own parent.
					if(!isInsideMap(map, nbrx, nbry)) { continue; } // Don't plot outside the map.
					
					// Distance from the start to here plus the distance from here to the neighbor.
					double distToNeighbor = cost[current.x][current.y] + distanceSquared(map, current.x, current.y, nbrx, nbry);
					
					// If we have a faster way to get here, update it!
					if(distToNeighbor < cost[nbrx][nbry]) {
						cost[nbrx][nbry] = distToNeighbor;
						costToGoal[nbrx][nbry] = heuristic(map, nbrx, nbry, finish.x, finish.y);
						parent[nbrx][nbry] = current;
						candidates.add(new Point(nbrx, nbry));
					}
				}
			}
		}
		
		if(!current.equals(finish)) {
			return null; // No path to goal.
		}
		
		ArrayList  prepath = new ArrayList();
		while(!current.equals(start)) {
			prepath.add(current);
			current = parent[current.x][current.y];
		}
		
		Point[] path = new Point[prepath.size()]; // Reverse the path before returning it.
		for(int i=0; i < path.length; i++) {
			path[i] = prepath.get(prepath.size()-1-i);
		}
		return path;
	}
	
	public static boolean isInsideMap(double[][] map, int x, int y) {
		return x >= 0 && x < map.length && y >= 0 && y < map[0].length; 
	}
	
	public static double heuristic(double[][] map, int ax, int ay, int goalx, int goaly) {
		double dx = ax - goalx;
		double dy = ay - goaly;
		double dz = map[ax][ay] - map[goalx][goaly];
		return dx*dx + dy*dy + dz*dz;
	}
	
	public static double distanceSquared(double[][] map, int ax, int ay, int bx, int by) {
		final double UPHILL_COST_SCALAR = 1.0; // Going up hill costs 3x more than going down hill.
		final double DOWNHILL_COST_SCALAR = 1.0; //  Going down hill costs just as much as going flat.
		
		double deltaDistance = (ax-bx)*(ax-bx) + (ay-by)*(ay-by);
		double deltaHeight = map[ax][ay] - map[bx][by];
		if(deltaHeight < 0) {
			deltaHeight = Math.max(0, deltaHeight*DOWNHILL_COST_SCALAR); // We are assuming that moving downhill has a non-negative cost. 
		} else if(deltaHeight > 0) {
			deltaHeight *= UPHILL_COST_SCALAR;
		} else {
			// No change.
		}
		
		return deltaDistance + deltaHeight*deltaHeight;
	}
}

	public static void main(String[] args) {
		final int XSIZE = 40;
		final int YSIZE = 40;
		
		// Run this to test.
		double[][] map = new double[XSIZE][YSIZE];
		Random rand = new Random();
		
		for(int i=0; i < XSIZE; i++) {
			for(int j=0; j < YSIZE; j++) {
				map[i][j] = rand.nextInt(2);
			}
		}
		Point start = new Point(rand.nextInt(XSIZE), rand.nextInt(YSIZE));
		Point end = new Point(rand.nextInt(XSIZE), rand.nextInt(YSIZE));
		
		Point[] path = findPath(map, start, end);
		
		// Draw the map
		char[][] output = new char[XSIZE][YSIZE];
		for(int i=0; i < XSIZE; i++) {
			for(int j=0; j < YSIZE; j++) {
				if(map[i][j] != 0) {
					output[i][j] = '#';
				} else {
					output[i][j] = ' ';
				}
			}
		}
		
		// Draw the path on the map.
		if(path != null) {
			for(Point p : path) {
				output[p.x][p.y] = '.';
			}
		}
		
		// Draw the start and end.
		output[start.x][start.y] = 'S';
		output[end.x][end.y] = 'E';
		
		// Print everything.
		for(int j=0; j < YSIZE; j++) {
			for(int i=0; i < XSIZE; i++) {
				System.out.print(output[i][j]);
			}
			System.out.println();
		}
	}