/*
example: 
	var map = [ [0,0,0,0,0,1,1,0],
		    [0,0,0,0,1,0,0,0],
		    [0,1,1,0,1,0,0,0],
		    [0,0,0,0,1,0,1,0],
		    [0,1,1,0,0,0,1,0],
		    [0,0,0,0,0,0,1,0] ];
	var start = {x:0, y:0};
	var goal  = {x:7, y:4};

	returns an array of {x: , y: } to goal
*/

function aStar (map, start, goal) {
	var closedSet = [];
	var openSet   = [];	
	var mh = map.length;
	var mw = map[0].length;
	var cameFrom  = new Array2d(mh, mw, null );
	var gScore    = new Array2d(mh, mw, 0);
	var fScore    = new Array2d(mh, mw, 0);
	openSet.push(start);

	gScore[start.y][start.x] = 0;
	fScore[start.y][start.x] = h(start, goal);

	var current   = {};
	var neighbors = [];
	while (openSet.length > 0) {
		current = openSet[0]; // the first is the one with the lowest fScore

		if (current.x == goal.x && current.y == goal.y)
			return reconstructPath(cameFrom, goal, start);

		closedSet.push( openSet.shift() );
		neighbors = getNeighbors ( current , map, mh, mw);
		var tentativeGScore = 0;

		for (var i=0 ; i< neighbors.length; i++){
			var n = neighbors[i];
			if (  isIn(n, closedSet) ) continue;

			tentativeGScore = gScore[current.y][current.x] + 1;
			
			if ( !isIn(n,openSet) || tentativeGScore < gScore[n.y][n.x]){
				cameFrom[n.y][n.x] = current;
				gScore[n.y][n.x]   = tentativeGScore;
				fScore[n.y][n.x]   = tentativeGScore + h(n, goal);

				if ( !isIn(n, openSet) )
					insertOrdered (n, openSet, gScore);
			}
		}
	}
	return null;
}

function insertOrdered (elem, arr, score) {
	var i = 0;
	while ( i < arr.length && score[elem.y][elem.x] > score[arr[i].y][arr[i].x] )
		i++;
	arr.splice(i, 0, elem);
}

function getNeighbors (n, map, h, w){

	var nx = n.x;
	var ny = n.y;

	var r = [];

	if (nx > 0 && map[ny][nx-1] == 0)
		r.push({x: nx-1, y:ny}); //left
	
	if (nx < w-1 && map[ny][nx+1] == 0)
		r.push({x: nx+1, y:ny}); //right
	
	if (ny > 0 && map[ny-1][nx] == 0)
		r.push({x: nx, y:ny-1}); //up
	
	if (ny < h-1 && map[ny+1][nx] == 0)
		r.push({x:nx, y:ny+1}); //down
	
	return r;
}

function reconstructPath (cameFrom, current) {
	var path = [];
	while ( cameFrom[current.y][current.x] != null ) {
		path.push(current);
		current = cameFrom[current.y][current.x];
	}
	return path.reverse();
}

function h (start, goal) {
	return Math.abs(start.x-goal.x) + Math.abs(start.y-goal.y);
}


function Array2d (firstDim, secondDim, val) {
	var arr = [];
	for (var i = 0; i < firstDim; i++) {
		arr[i] = [];
		if ( typeof val !== 'undefined')
			for (var j = 0; j < secondDim; j++) {
				arr[i][j] = val;
			};
	};
	return arr;
}

function isIn (point, list) {
	for (var i = 0; i < list.length; i++) {
		if (point.x == list[i].x && point.y == list[i].y)
			return true;
	};
	return false;
}