/******************************************************************************************
*                                         A* algorithm - Wikipedia modified
*******************************************************************************************/
var closedset = '';	  				// the empty set    - The set of nodes already evaluated.
var openset = '';						//set containing the initial node % The set of tentative nodes to be evaluated.
var path ='';
  			          	
function findPath(xg,yg){
     var x = 0, y = 0 ;
     var g_score = new Array(avenues*streets);
     var h_score = new Array(avenues*streets);
     var f_score = new Array(avenues*streets);
     var came_from = new Array(avenues*streets);
     for (i=0; i< avenues*streets; i++) came_from[i] = -1;
     start = karelY*avenues+karelX;
     goal = yg*avenues+xg;
     closedset = '';	  				// the empty set    - The set of nodes already evaluated.
     openset = ','+start+',';				//set containing the initial node % The set of tentative nodes to be evaluated.
     g_score[start] = 0;                     	//Distance from start along optimal path.
     h_score[start] = heuristic_estimate_of_distance(start, goal)
     f_score[start] = h_score[start];		//Estimated total distance from start to goal through y.
     while (openset != ',') {				//openset is not empty
         x = lowest_f_score_node(goal); 		// the node in openset having the lowest f_score[] value
         if (x == goal){
             reconstruct_path(start,goal,came_from);
             return "Path found";
         }
         remove_node_from_openset(x);
         add_node_to_closedset(x);
         //foreach y in neighbor_nodes(x) {
         for (i=0; i < 4; i++) {
             switch(i) {
               case 1: y = x + avenues; facing = "north"; break;
               case 3: y = x - avenues; facing = "south"; break;
               case 0: if(x % avenues < avenues -1) y = x + 1; 
                       else y = -1;
                       facing = "east"; 
                       break;
               case 2: if(x % avenues > 0) y = x - 1;
                       else y = -1;
                       facing = "west"
			     break;
                       break;
               default: break;
             }
             
             if(y < 0 || y > avenues*streets-1) {     // not inside Karel's world
		    continue;
             }
             if(!is_neighbor(x,facing)) // is y reachable from x
                 continue;
             if (node_in_closedset(y))
                 continue;
             tentative_g_score = g_score[x] + dist_between(x,y);
             tentative_is_better = false;
             if (node_not_in_openset(y)) {
                 add_node_to_openset(y);
                 h_score[y] = heuristic_estimate_of_distance(y, goal)
                 tentative_is_better = true
             }
             else if (tentative_g_score < g_score[y]) {
                 tentative_is_better = true
             }
             if (tentative_is_better == true) {
                 came_from[y] = x
                 g_score[y] = tentative_g_score
                 f_score[y] = g_score[y] + h_score[y]
             }
          }
      }
      path = '';
      return("No path found");
 }

 function is_neighbor(n,facing){ 
  var side  = facing;
  var i = n % avenues; 
  var j = Math.floor(n/avenues);
  if(facing == "east") {side="west"; i++;}
  if(facing == "north") {side="south"; j++;}
  var wall = "wall:("+(i+1)+","+(j+1)+")"+side;
  if (worldMap.indexOf(wall) != -1) return false;
  else return true;
 }

 function add_node_to_openset(node){
   openset+=node+',';
 }
 function add_node_to_closedset(node){
   closedset+=node+',';
 }
 function remove_node_from_openset(node){
   openset = openset.replace(node+',',''); 
 }
 function node_not_in_openset(node){
   if (openset.indexOf(','+node+',') == -1) return true;
   else return false;
 }
 function node_in_closedset(node){
   if (closedset.indexOf(','+node+',') == -1) return false;
   else return true;
 }
 function dist_between(node1,node2){ 
    return Math.abs(Math.floor(node1/avenues)-Math.floor(node2/avenues))+Math.abs((node1 % avenues)-(node2 % avenues));
 }
 function lowest_f_score_node(goal){
  nodes = openset.split(',');
  k = parseInt(nodes[1]);
  min = Math.abs(Math.floor(k/avenues)-Math.floor(goal/avenues))+Math.abs((k % avenues)-(goal % avenues));
  index = 1;
  for(i=2;i<nodes.length-1;i++){  
     k = parseInt(nodes[1]);
     d = Math.abs(Math.floor(k/avenues)-Math.floor(goal/avenues))+Math.abs((k % avenues)-(goal % avenues));
     if(d < min) {
       min = d; index = i;
     }
  }
  return parseInt(nodes[index]);
 }
 function heuristic_estimate_of_distance(node1,node2){
    // Manhatten distance abs(x2-x1)+abs(y2-y1)
    return Math.abs(Math.floor(node1/avenues)-Math.floor(node2/avenues))+Math.abs((node1 % avenues)-(node2 % avenues));
    //var x1 = node1/avenues, x2= node2/avenues;
    //var y1 = node1 % avenues, y2 = node2 % avenues;
    //return (x1-x2)*(x1-x2)+(y1-y2)*(y1-y2); 
 }
 function reconstruct_path(from,to,came_from){
     next = to;
     path=to;
     while(next != from){
       next = came_from[next]
       path = next+','+path;
     }
   //drawPath();
   //followBeepers(to);
   //catchBeepers();
 }
 function drawPath(){
    if(path == '') return;
    sq = path.split(',');
    saveX = karelX; saveY = karelY;
    for (i=0; i< sq.length; i++) {
       karelY = Math.floor(parseInt(sq[i])/avenues);
       karelX = parseInt(sq[i]) % avenues;
       mode="manual"
       putBeeperM();
       mode = "auto"
    }
    karelX=saveX;
    karelY=saveY;
 } 
function searchBeepers(xg,yg){
 while(karelX!=xg || karelY!=yg){
   if(beepersPresent()) pickBeeper();
   if(!beeperFoundFront())
     turnLeft();
 }
 if(beepersPresent()) pickBeeper();
}
function beeperFoundFront(){
  if(frontIsBlocked()) return false;
  move();
  if(beepersPresent()) {
    return true;
  }
  turnAround();
  move();
  turnAround();
  return false;
}
function beepersOnSide(side){
  saveX=karelX;
  saveY=karelY;
  ret = false;
  switch(side){
    case "north": karelY++; break;
    case "south": karelY--; break;
    case "east":  karelX++; break;
    case "west":  karelX--; break;
    default: break;
  }
  if(karelX < 0 || karelX > avenues-1) ret = false;
  if(karelY < 0 || karelX > streets-1) ret = false;
  if(beepersPresent()) ret = true;
  karelX=saveX; karelY=saveY;
  return ret;
}

function followBeepers(){
  more = true;
  while(more) {
    if(beepersPresent())pickBeeper();
    if(beepersOnSide("north")){turnNorth();if(frontIsClear()){move();continue;}}
    if(beepersOnSide("south")){turnSouth();if(frontIsClear()){move();continue;}}
    if(beepersOnSide("east")){turnEast();if(frontIsClear()){move();continue;}}
    if(beepersOnSide("west")){turnWest();if(frontIsClear()){move();continue;}}
    more = false;
  }
}
function turnNorth(){
  switch(karelFacing){
    case "north": break;
    case "south": turnAround();break;
    case "east": turnLeft(); break;
    case "west": turnRight();break;
    default: break;
  }
}
function turnSouth(){
  switch(karelFacing){
    case "south": break;
    case "north": turnAround();break;
    case "east": turnRight(); break;
    case "west": turnLeft();break;
    default: break;
  }
}
function turnEast(){
  switch(karelFacing){
    case "east": break;
    case "west": turnAround();break;
    case "south": turnLeft(); break;
    case "north": turnRight();break;
    default: break;
  }
}
function turnWest(){
  switch(karelFacing){
    case "west": break;
    case "east": turnAround();break;
    case "north": turnLeft(); break;
    case "south": turnRight();break;
    default: break;
  }
}

function followPath(){
    if(path == '') return;
    sq = path.split(',');
    for (i=0; i< sq.length; i++) {
       y = Math.floor(parseInt(sq[i])/avenues);
       x = parseInt(sq[i]) % avenues;
       if(x==karelX && y==karelY) continue;
       if(karelX > x) {turnWest();move();continue;}
       if(karelX < x) turnEast();
       if(karelY > y) turnSouth();
       if(karelY < y) turnNorth();
       move();
    }
 }