//Pathfind.js
//Written by Kamatsu
//MIT License


function PFPoint (x,y) {
   return {x: x, y: y};
}

function AStarPathFind(fromCoords, toCoords,isObstructed, distance) {

   
   var pq = new PriorityQueue(fromCoords);
   
   var visited = new Array();
   

   var visitNode = function(p,distanceTravelled,isObstructed) {
      //some dodgyness with hashtables but easiest way to retrace the path
      a = p.value;
      var psf = p.pathSoFar;
      
      if (visited[a.x + 'x' + a.y] == true) return false;
      
      visited[a.x + 'x' + a.y] = true;
      
      if (a.x == toCoords.x && a.y == toCoords.y) {      
          return true;
      }
      
      if (!isObstructed(PFPoint(a.x+1,a.y))
         && !visited[a.x+1 + 'x' + a.y]) {
         psf.push(KEY_RIGHT);

         pq.enqueue(PFPoint(a.x+1,a.y),
                  -(distance(PFPoint(a.x+1,a.y),toCoords) + distanceTravelled),psf);
         psf.pop();   
      }
      if (!isObstructed(PFPoint(a.x-1,a.y))      
         && !visited[a.x-1 + 'x' + a.y]) {
         psf.push(KEY_LEFT);

         pq.enqueue(PFPoint(a.x-1,a.y),
                  -(distance(PFPoint(a.x-1,a.y),toCoords) + distanceTravelled),psf);
         psf.pop();     
      }
      if (!isObstructed(PFPoint(a.x,a.y+1))
         && !visited[a.x + 'x' + a.y+1]) {
         psf.push(KEY_DOWN);

         pq.enqueue(PFPoint(a.x,a.y+1),
                  -(distance(PFPoint(a.x,a.y+1),toCoords) + distanceTravelled),psf);
         psf.pop();   
      }
      if (!isObstructed(PFPoint(a.x,a.y-1))
         && !visited[a.x + 'x' + a.y-1]) {
         psf.push(KEY_UP);

         pq.enqueue(PFPoint(a.x,a.y-1),
                  -(distance(PFPoint(a.x,a.y-1),toCoords) + distanceTravelled),psf);
         psf.pop();   
      }
      return false;
   }

   
   
   var distanceTravelled = 0;
   while (!pq.is_empty()) {
      var item;

      distanceTravelled++;
      item = pq.dequeue();
      if (visitNode(item,distanceTravelled,isObstructed)) {
         return item.pathSoFar;
         break;
      };
   }
   return null;

}

   function PQSwap (a,b) {
      var t_v = a.value;
      var t_p = a.priority;
      var t_psf = a.pathSoFar;
      a.value = b.value;
      a.priority = b.priority;
      a.pathSoFar = b.pathSoFar;
      b.value = t_v;
      b.priority = t_p;
      b.pathSoFar = t_psf;
   }
   
   var PQNode = function(value,priority,psf) {
      this.value = value;
      this.priority = priority;
      this.weight = 0; 
      this.firstChild = null;
      this.pathSoFar = psf.concat(new Array());
      this.secondChild = null;
      this.insertNode = function (value,priority,psf) {
         this.weight++;
         if (this.firstChild == null)
            this.firstChild = new PQNode(value,priority,psf);
         else if (this.secondChild == null)
            this.secondChild = new PQNode(value,priority,psf);
         else if (this.firstChild.weight > this.secondChild.weight) {
            this.secondChild.insertNode(value,priority,psf);
            if (this.secondChild.priority > this.priority) 
               PQSwap(this,this.secondChild);
         } else {
            this.firstChild.insertNode(value,priority,psf);
            if (this.firstChild.priority > this.priority) 
               PQSwap(this,this.firstChild);               
         } 
      }
   }
   
function PriorityQueue (startValue) {
   


   var check = function (item) {
      if (item == null) return;
      if (item.firstChild == null && item.secondChild == null) return;
      check(item.firstChild); 
      check(item.secondChild);      
      if (item.secondChild != null) {
         if (((item.firstChild == null) ||
              item.firstChild.priority < item.secondChild.priority)     
          &&  item.secondChild.priority > item.priority)       
               PQSwap(item,item.secondChild);
      }
      else if (item.firstChild != null) {
         if (((item.secondChild == null) ||
                item.secondChild.priority < item.firstChild.priority)
            &&  item.firstChild.priority > item.priority)
            PQSwap(item,item.firstChild); 
      }
   }   
   var isLeaf = function (node) {
      return (node.firstChild == null && node.secondChild == null);
   }
   var pullDeepestRightValues = function (node) {   
      if (node.secondChild == null
       || (node.firstChild != null && node.firstChild.weight > node.secondChild.weight))
         if (isLeaf(node.firstChild)) {
            var ret = {value:node.firstChild.value,priority:node.firstChild.priority,psf:node.firstChild.pathSoFar};
            node.firstChild = null;
            return ret;
         } else return pullDeepestRightValues(node.firstChild);
   
      else
         if (isLeaf(node.secondChild)) {
            var ret = {value:node.secondChild.value,priority:node.secondChild.priority,psf:node.secondChild.pathSoFar};
            node.secondChild = null;
            return ret;
         } else return pullDeepestRightValues(node.secondChild);
   }
   this.root = new PQNode(startValue,0, new Array())

   this.dequeue = function() {        
      
      var ret = {value:this.root.value,priority:this.root.priority,pathSoFar:this.root.pathSoFar};
      if (isLeaf(this.root)) {
         var dr = this.root;
         this.root = null;
      }
      else  {
         var dr = pullDeepestRightValues(this.root);
         this.root.value = dr.value;
         this.root.priority = dr.priority;
         this.root.pathSoFar = dr.psf;
         check(this.root);
      }
      return ret;
   }  

   this.is_empty = function() {
      return (this.root == null);
   }

   this.enqueue = function(value,priority,psf) {
      if (this.root == null) this.root = new PQNode(value,priority, psf);
      else this.root.insertNode(value,priority,psf);
   }

}


