[Script-WIP] Listra 2D Map Graph & Pathfinding Modules

#define dijkstra
/*
 Dijkstra's Algorithm


@> Function
 * To find a shortest path from starting vertex (s) to target vertex (t)
  and return path distance from s to t.

@> Required Data
 * n: number of grid vertices
 * s: index of source vertex
 * t: index of target vertex
 * global.nb[u,i]: i-th neighbor of grid vertex u, whose distance from u
                  is 1 (as vertex index)
 * global.tc[u,i]: terrain cost from u to global.nb[u,i]
                  (equal to 1 normally)
 * global.neighbors: maximum number of each vertices (usually 4 for
                    4-directional movement or 8 for 8-directional movement)

@> Required Scripts
 * remove(x): Removes x from array q (as priority queue)
 * add(x): Inserts x into array q (as priority queue)
 * u_nearest(): Returns index u of array q, where vertices_dist[u]
                is smallest

@> Usage Guide
 * For graph initialization, build a passage graph with script
  buildGraph().
 * Then use this script on a game character. Set its variables s as index of
  the target's position vertex and t as index of the character's
  position vertex.
 * After that, you can trace the character's path by using vertices_prev[t].
 * You can also use vertices_dist[t] or value returned by this script to
  show the character's distance to the target.

@> Credits
 * Bunga Tepi Jalan (bungatepijalan.wordpress.com)
 * Wikipedia

@> Notes
 * If you want to learn more about Dijkstra's algorithm, see the article at:
  http://en.wikipedia.org/wiki/Dijkstra's_algorithm
 * The path constructed by this algorithm is always the shortest.
 * This script is made based on pseudocode at that article, with
  improvement on efficiency.
*/

// Initializations
nq=0        // Create open list q (as priority queue)
add(s)      // Add s into open list q
for(i=1;i<=n;i+=1){
    vertices_dist[i]=9999 // Unknown distance function from source to v
    vertices_prev[i]=0    // Previous node in optimal path from source
}
vertices_dist[s]=0      // Distance from source to source
while(nq>0){            // The main loop
    //Finds vertex u with least value of path distance
    u_nearest()
    dist_u=vertices_dist[u]
    if(u==t){
        //If u is the target, search completed and returns its distance.
        //You can add statements here, usually to read/construct path
        return dist_u
    }
    remove(u)
    for(i=1;i<=global.neighbors;i+=1){
        v=global.nb[u,i]    // where v has not yet been removed from Q.
        if(v!=0){
            alt=dist_u+global.tc[u,i]
            if(alt<vertices_dist[v]){  // Relax (u,v)
                add(v)
                vertices_dist[v]=alt
                vertices_prev[v]=u
            }
        }
    }
}
//Statements below this will be executed if all remaining vertices
//are inaccessible from source. This means that target vertex is isolated
//from source vertex. Also means that vertices_dist[t]==9999.
//You can add them here, usually to alert that no path is found.

#define BFS
/*
 Best-first Search (BFS)


@> Function
 * To find a path from starting vertex (s) to target vertex (t).

@> Required Data
 * n: number of grid vertices
 * s: index of source vertex
 * t: index of target vertex
 * global.nb[u,i]: i-th neighbor of grid vertex u, whose distance from u
                  is 1 (as vertex index)
 * global.tc[u,i]: terrain cost from u to global.nb[u,i]
                  (equal to 1 normally)
 * global.neighbors: maximum number of each vertices (usually 4 for
                    4-directional movement or 8 for 8-directional movement)

@> Required Scripts
 * remove(x): Removes x from array q (as priority queue)
 * add(x): Inserts x into array q (as priority queue)
 * u_nearest3(): Returns index u of array q, where h_score[u]
                is smallest
 * findHeuristic(x,y): Returns heuristic (estimated) distance from
                      vertex x to y

@> Usage Guide
 * For graph initialization, build a passage graph with script
  buildGraph().
 * Then use this script on a game character. Set its variables s as index of
  the target's position vertex and t as index of the character's
  position vertex.
 * After that, you can trace the character's path by using vertices_prev[t].
 * You can also use vertices_dist[t] or value returned by this script to
  show the character's distance to the target.

@> Credits
 * Bunga Tepi Jalan (bungatepijalan.wordpress.com)
 * Wikipedia

@> Notes
 * If you want to learn more about Best-first Search, see the article at:
  http://en.wikipedia.org/wiki/Best-first_search
 * The constructed path isn't always the shortest path. If you want to find
  the shortest path, don't use this.
*/

// Initializations
nq=0        // Create open list q (as priority queue)
add(s)      // Add s into open list q
for(i=1;i<=n;i+=1){
    q1[i]=0    // Create closed list q1, The list of nodes already evaluated.
    vertices_prev[i]=0    // The map of navigated nodes.
}
vertices_dist[s]=0
h_score[s]=findHeuristic(s,t)          // Heuristic distance
while(nq>0){    // while the open list is not empty
    u_nearest3()
    if(u==t){
        //If u is the target, search completed and returns its distance.
        //You can add statements here, usually to read/construct path
        return 0
    }
    remove(u)
    q1[u]=1    // move current node from open list to the closed list
    for(i=1;i<=global.neighbors;i+=1){
        v=global.nb[u,i]    // successor of u
        if(v!=0){
        if(!q1[v]){
            add(v)
            vertices_dist[v]=vertices_dist[u]+global.tc[u,i]
            h_score[v]=findHeuristic(v,t)
            vertices_prev[v]=u
        }else{
            if(vertices_dist[v]>vertices_dist[u]+global.tc[u,i]){
                vertices_dist[v]=vertices_dist[u]+global.tc[u,i]
                vertices_prev[v]=u
            }
        }
        }
    }
}
//Statements below this will be executed if all remaining vertices
//are inaccessible from source. This means that target vertex is isolated
//from source vertex.
//You can add them here, usually to alert that no path is found.

#define AStar
/*
 A* Algorithm


@> Function
 * To find a shortest path from starting vertex (s) to target vertex (t)
  and return path distance from s to t.

@> Required Data
 * n: number of grid vertices
 * s: index of source vertex
 * t: index of target vertex
 * global.nb[u,i]: i-th neighbor of grid vertex u, whose distance from u
                  is 1 (as vertex index)
 * global.tc[u,i]: terrain cost from u to global.nb[u,i]
                  (equal to 1 normally)
 * global.neighbors: maximum number of each vertices (usually 4 for
                    4-directional movement or 8 for 8-directional movement)

@> Required Scripts
 * remove(x): Removes x from array q (as priority queue)
 * add(x): Inserts x into array q (as priority queue)
 * found(x): Returns whether x is in array q
 * u_nearest2(): Returns index u of array q, where f_score[u]
                is smallest
 * findHeuristic(x,y): Returns heuristic (estimated) distance from
                      vertex x to y

@> Usage Guide
 * For graph initialization, build a passage graph with script
  buildGraph().
 * Then use this script on a game character. Set its variables s as index of
  the target's position vertex and t as index of the character's
  position vertex.
 * After that, you can trace the character's path by using vertices_prev[t].
 * You can also use vertices_dist[t] or value returned by this script to
  show the character's distance to the target.

@> Credits
 * Bunga Tepi Jalan (bungatepijalan.wordpress.com)
 * Wikipedia
 * Amit's A* Pages (http://www-cs-students.stanford.edu/~amitp/gameprog.html)

@> Notes
 * If you want to learn more about A* Algorithm, see the article at:
  http://en.wikipedia.org/wiki/A*_search_algorithm
 * A* is the best choice for pathfinding. Recommended to use this on
  most purposes.
*/

// Initializations
nq=0        // Create open list q (as priority queue)
add(s)      // Add s into open list q
for(i=1;i<=n;i+=1){
    q1[i]=0    // Create closed list q1, The list of nodes already evaluated.
    vertices_prev[i]=0    // The map of navigated nodes.
}
vertices_dist[s]=0                    // Distance from start along optimal path.
h_score[s]=findHeuristic(s,t)  // Heuristic distance
f_score[s]=h_score[s]  // Estimated total distance from start to goal.
while(nq>0){    // while the open list is not empty
    u_nearest2()
    if(u==t){
        //If u is the target, search completed and returns its distance.
        //You can add statements here, usually to read/construct path
        return dist_u
    }
    remove(u)
    q1[u]=1    // move current node from open list to the closed list
    for(i=1;i<=global.neighbors;i+=1){
        v=global.nb[u,i]
        if(v!=0){
        if(!q1[v]){
            tentative_g_score=vertices_dist[u]+global.tc[u,i]
            if(!found(v)){
                add(v)
                tentative_is_better=true
            }else if(tentative_g_score<vertices_dist[v]){
                tentative_is_better=true
            }else{
                tentative_is_better=false
            }
            if(tentative_is_better){
                if(vertices_prev[v]!=0){
                    if(f_score[u]<f_score[vertices_prev[v]]){
                        vertices_prev[v]=u
                    }
                }else{
                    vertices_prev[v]=u
                }
                vertices_dist[v]=tentative_g_score
                h_score[v]=findHeuristic(v,t)
                f_score[v]=vertices_dist[v]+h_score[v]
            }
        }
        }
    }
}
//Statements below this will be executed if all remaining vertices
//are inaccessible from source. This means that target vertex is isolated
//from source vertex.
//You can add them here, usually to alert that no path is found.

#define remove
//remove(ii)
//Removes an element with value ii from priority queue q
ii=argument0
iii=1
while(iii<=nq && q[iii]!=ii){
  iii+=1
}
iv=iii;
if(iv<=nq){
  if(iv<nq){
  for(iii=iv;iii<=nq-1;iii+=1){
    q[iii]=q[iii+1];
  }
  }
  nq=nq-1;
}

#define add
//add(ii)
//Inserts an element with value ii into priority queue q
ii=argument0
if(nq==0){
    q[1]=ii
}else{
    iii=1
    while(iii<=nq && q[iii]<ii){
        iii+=1
    }
    iv=iii
    if(iv<=nq){
        for(iii=nq;iii>=iv;iii-=1){
            q[iii+1]=q[iii]
        }
    }
    q[iv]=ii
}
nq+=1

#define found
//found(ii)
//Returns whether element ii is in priority queue q
ii=argument0
ff=false;
if(nq>0){
    for(iii=1;iii<=nq;iii+=1){
        if(q[iii]==ii){
            ff=true
            break
        }
    }
}
return ff;

#define u_nearest
//u_nearest()
//Returns index u of array q, where vertices_dist[u] is smallest
d=vertices_dist[q[1]]
  u=q[1]
  if(nq>1){
  for(ii=2;ii<=nq;ii+=1){
  if(vertices_dist[q[ii]]<d){
    d=vertices_dist[q[ii]];
    u=q[ii];
  }
  }
  }

#define u_nearest2
//u_nearest2()
//Returns index u of array q, where f_score[u] is smallest
if(nq>0){
  d=f_score[q[1]]
  u=q[1]
  if(nq>1){
  for(ii=2;ii<=nq;ii+=1){
  if(f_score[q[ii]]<d){
    d=f_score[q[ii]];
    u=q[ii];
  }
  }
  }
}

#define u_nearest3
//u_nearest3()
//Returns index u of array q, where h_score[u] is smallest
if(nq>0){
  d=h_score[q[1]]
  u=q[1]
  if(nq>1){
  for(ii=2;ii<=nq;ii+=1){
  if(h_score[q[ii]]<d){
    d=h_score[q[ii]];
    u=q[ii];
  }
  }
  }
}

#define findHeuristic
//findHeuristic(x,y)
//Calculates heuristic distance between vertex x and vertex y
dx=((argument1-1) div global.w)-((argument0-1) div global.w)
dy=((argument1-1) mod global.w)-((argument0-1) mod global.w)
//Uncomment one of two statements below to select a heuristic formula
//you want to use
return sqrt(dx*dx+dy*dy)        //Euclidean distance heuristic
//return (dx+dy)                //Manhattan distance heuristic

#define buildGraph
/*
 buildGraph()


@> Function
 * To generate a passage graph of given 2-dimensional map.

@> Required Data
 * global.M[i,j]: Terrain type of cell of map matrix at row i and column j.
                  Obstacles have terrain type 0.
 * global.passable[i]: The value is 0 if i is type of obstacle. Otherwise,
                      determines terrain cost of terrain type i (normally 1).
 * h: number of rows of the map matrix
 * w: number of columns of the map matrix

@> Outputs
 * global.nb[u,i]: i-th neighbor of grid vertex u, whose distance from u
                  is 1 (as vertex index)
 * global.tc[u,i]: terrain cost from u to global.nb[u,i]
                  (equal to 1 normally)

@> Usage Guide
 * From a terrain map, build 2-dimensional map matrix global.M, and then use
  this script to generate a passage graph.

@> Credits
 * Bunga Tepi Jalan (bungatepijalan.wordpress.com)
*/
global.neighbors=4
//Maximum number of neighbors of each vertices.
//You can change the value to: 4 for 4-directional movement, or 8 for
//8-directional movement
for(i=0;i<h;i+=1){
    for(j=0;j<w;j+=1){
        if(global.passable[global.M[i,j]]>0){
            node=i*w+j
            for(k=1;k<=4;k+=1){
                global.nb[node+1,k]=0
            }
            //West
            if(j>0){
                if(global.passable[global.M[i,j-1]]>0){
                    global.nb[node+1,1]=node
                    global.tc[node+1,1]=global.passable[global.M[i,j]]
                }
            }
            //East
            if(j<w){
                if(global.passable[global.M[i,j+1]]>0){
                    global.nb[node+1,2]=node+2
                    global.tc[node+1,2]=global.passable[global.M[i,j]]
                }
            }
            //North
            if(i>0){
                if(global.passable[global.M[i-1,j]]>0){
                    global.nb[node+1,3]=node-w+1
                    global.tc[node+1,3]=global.passable[global.M[i,j]]
                }
            }
            //South
            if(i<h){
                if(global.passable[global.M[i+1,j]]>0){
                    global.nb[node+1,4]=node+w+1
                    global.tc[node+1,4]=global.passable[global.M[i,j]]
                }
            }
            if(global.neighbors==8){
                //Northwest
                if(i>0 && j>0){
                    if(global.passable[global.M[i-1,j-1]]>0){
                        global.nb[node+1,1]=node-w
                        global.tc[node+1,1]=global.passable[global.M[i,j]]*sqrt(2)
                    }
                }
                //Northeast
                if(i>0 && j<w){
                    if(global.passable[global.M[i-1,j+1]]>0){
                        global.nb[node+1,1]=node-w+2
                        global.tc[node+1,1]=global.passable[global.M[i,j]]*sqrt(2)
                    }
                }
                //Southwest
                if(i<h && j>0){
                    if(global.passable[global.M[i+1,j-1]]>0){
                        global.nb[node+1,1]=node+w
                        global.tc[node+1,1]=global.passable[global.M[i,j]]*sqrt(2)
                    }
                }
                //Southeast
                if(i<h && j<w){
                    if(global.passable[global.M[i+1,j+1]]>0){
                        global.nb[node+1,1]=node+w+2
                        global.tc[node+1,1]=global.passable[global.M[i,j]]*sqrt(2)
                    }
                }
            }
        }
    }
}

#define DFSBacktracker
/*
 Recursive Backtracker (using Depth-first Search)

Required data
 * s: current cell
 * global.nb[u,i]: i-th neighbor of cell u (1<=i<=4), whose distance
                  from u is 1
*/

// Initializations
nq=global.n
for(i=1;i<=global.n;i+=1){
    q[i]=0
    prev[i]=0
}
u=s
while(nq>=0){
    if(!q[u]){
        q[u]=1          //Mark the current cell as 'Visited'
        nq-=1
    }
    avail=false
    for(i=1;i<=4;i+=1){
        if(global.nb1[u,i]!=0){
            avail=(avail || !q[global.nb1[u,i]])
        }
    }
    //If the current cell has any neighbours which have not been visited
    if(avail){
        //Choose randomly one of the unvisited neighbours
        do{
            j=1+irandom(3)
            v=global.nb1[u,j]
        }until(!q[v])
        prev[v]=u
        //remove the wall between the current cell and the chosen cell
        global.nb[u,j]=v
        if(j==1){
            j=2
        }else if(j==2){
            j=1
        }else if(j==3){
            j=4
        }else if(j==4){
            j=3
        }
        global.nb[v,j]=u
        //Make the chosen cell the current cell, and
        //repeat the search until all cells visited
        u=v
    }else{
        //Make previously chosen cell the current cell (backtrack)
        if(prev[u]!=0){
            u=prev[u]
        }
    }
}

#define HuntNKill
/*
 Hunt and Kill

Required data
 * s: current cell
 * global.nb[u,i]: i-th neighbor of cell u (1<=i<=4), whose distance
                  from u is 1
*/

// Initializations
nq=global.n
for(i=1;i<=global.n;i+=1){
    q[i]=0
}
u=s
while(nq>=0){
    if(!q[u]){
        q[u]=1          //Mark the current cell as 'Visited'
        nq-=1
    }
    avail=false
    for(i=1;i<=4;i+=1){
        if(global.nb1[u,i]!=0){
            avail=(avail || !q[global.nb1[u,i]])
        }
    }
    //If the current cell has any neighbours which have not been visited
    if(avail){
        //Choose randomly one of the unvisited neighbours
        do{
            j=1+irandom(3)
            v=global.nb1[u,j]
        }until(!q[v])
        //remove the wall between the current cell and the chosen cell
        global.nb[u,j]=v
        if(j==1){
            j=2
        }else if(j==2){
            j=1
        }else if(j==3){
            j=4
        }else if(j==4){
            j=3
        }
        global.nb[v,j]=u
        //Make the chosen cell the current cell, and
        //repeat the search until all cells visited
        u=v
    }else{
        //Hunt!
        do{
            u=1+irandom(global.n-1)
        }until(q[u])
    }
}

#define Prim
/*
 Prim's Algorithm

Required data
 * s: index of source vertex
 * global.nb[u,i]: i-th neighbor of grid vertex u (1<=i<=4), whose distance
                  from u is 1
*/

// Initializations
nq=0
for(i=1;i<=global.n;i+=1){
    q1[i]=0
}
u=1+irandom(global.n-1)
q1[u]=1
add(u)
while(nq<=global.n){
    avail=false
    for(i=1;i<=4;i+=1){
        if(global.nb1[u,i]!=0){
            avail=(avail || !q1[global.nb1[u,i]])
        }
    }
    //If the current cell has any neighbours which have not been visited
    if(avail){
        //Choose randomly one of the unvisited neighbours
        do{
            j=1+irandom(3)
            v=global.nb1[u,j]
        }until(!q1[v])
        q1[v]=1
        add(v)
        //remove the wall between the current cell and the chosen cell
        global.nb[u,j]=v
        if(j==1){
            j=2
        }else if(j==2){
            j=1
        }else if(j==3){
            j=4
        }else if(j==4){
            j=3
        }
        global.nb[v,j]=u
    }
    i=1+irandom(nq-1)
    u=q[i]
}

/*
*==============================================================================
* ## MGraph (Map Graph)
*------------------------------------------------------------------------------
*  This class generates a passage graph of given 2-dimensional map and uses it
*  for pathfinding analysis with Dijkstra's algorithm and A*.
*------------------------------------------------------------------------------
*  Usage Guide
*------------------------------------------------------------------------------
*  Build a 2-dimensional map matrix global.M, and then use
*  the constructor to generate a passage graph.
*------------------------------------------------------------------------------
*  Credits
*------------------------------------------------------------------------------
*  Bunga Tepi Jalan (bungatepijalan.wordpress.com)
*  Wikipedia
*==============================================================================
*/
import PQueue;
class MGraph {
   private var n, h, w, neighbors:Number;
   private var nb, tc, passable, vertices_dist, vertices_next:Array;
   function MGraph(M, pass:Array, nh:Number) {
      h = M.length;
      w = M[0].length;
      n = h*w;
      neighbors = nh;
      passable = pass;
      nb = new Array(n+1);
      tc = new Array(n+1);
      var node;
      for (var i = 0; i<h; i += 1) {
         for (var j = 0; j<w; j += 1) {
            if (passable[M[i][j]]>0) {
               node = i*w+j;
               nb[node+1] = new Array();
               tc[node+1] = new Array();
               for (var k = 1; k<=neighbors+1; k += 1) {
                  nb[node+1].push(0);
                  tc[node+1].push(0);
               }
               //West
               if (j>0) {
                  if (passable[M[i][j-1]]>0) {
                     nb[node+1][1] = node;
                     tc[node+1][1] = passable[M[i][j]];
                  }
               }
               //East                                                             
               if (j<w) {
                  if (passable[M[i][j+1]]>0) {
                     nb[node+1][2] = node+2;
                     tc[node+1][2] = passable[M[i][j]];
                  }
               }
               //North                                                             
               if (i>0) {
                  if (passable[M[i-1][j]]>0) {
                     nb[node+1][3] = node-w+1;
                     tc[node+1][3] = passable[M[i][j]];
                  }
               }
               //South                                                             
               if (i<h) {
                  if (passable[M[i+1][j]]>0) {
                     nb[node+1][4] = node+w+1;
                     tc[node+1][4] = passable[M[i][j]];
                  }
               }
               if (neighbors == 8) {
                  //Northwest
                  if (i>0 && j>0) {
                     if (passable[M[i-1][j-1]]>0) {
                        nb[node+1][5] = node-w;
                        tc[node+1][5] = passable[M[i][j]]*Math.sqrt(2);
                     }
                  }
                  //Northeast                                                             
                  if (i>0 && j<w) {
                     if (passable[M[i-1][j+1]]>0) {
                        nb[node+1][6] = node-w+2;
                        tc[node+1][6] = passable[M[i][j]]*Math.sqrt(2);
                     }
                  }
                  //Southwest                                                             
                  if (i<h && j>0) {
                     if (passable[M[i+1][j-1]]>0) {
                        nb[node+1][7] = node+w;
                        tc[node+1][7] = passable[M[i][j]]*Math.sqrt(2);
                     }
                  }
                  //Southeast                                                             
                  if (i<h && j<w) {
                     if (passable[M[i+1][j+1]]>0) {
                        nb[node+1][8] = node+w+2;
                        tc[node+1][8] = passable[M[i][j]]*Math.sqrt(2);
                     }
                  }
               }
            }
         }
      }
   }
   function findHeuristic(x1, y1, x2, y2:Number) {
      return Math.sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
   }
   public function dijkstra(x1, y1, x2, y2:Number) {
      // Initializations
      var s = y1*w+x1+1;
      var t = y2*w+x2+1;
      var q = [0];// Create open list q (as priority queue)
      q = PQueue.add(q, s);// Add s into open list q
      vertices_dist = new Array();
      var vertices_prev = new Array();
      vertices_next = new Array();
      for (var i = 0; i<=n; i++) {
         vertices_dist.push(9999);// Unknown distance function from source to v
         vertices_prev.push(0);// Previous node in optimal path from source
         vertices_next.push(0);
      }
      vertices_dist[s] = 0;// Distance from source to source
      var u, v, alt;
      while (q.length>1) {// The main loop
         //Finds vertex u with least value of path distance
         var d = vertices_dist[q[1]];
         u = q[1];
         if (q.length>1) {
            for (var ii = 2; ii<=q.length; ii++) {
               if (vertices_dist[q[ii]]<d) {
                  d = vertices_dist[q[ii]];
                  u = q[ii];
               }
            }
         }
         if (u == t) {
            //If u is the target, search completed and returns its distance.
            //You can add statements here, usually to read/construct path
            while (vertices_prev[u] != s && vertices_prev[u] != 0) {
               vertices_next[vertices_prev[u]] = u;
               u = vertices_prev[u];
            }
            vertices_next[s] = u;
            return vertices_dist[t];
         }
         q = PQueue.remove(q, u);
         for (var i = 1; i<=neighbors; i++) {
            v = nb[u][i];// where v has not yet been removed from Q.
            if (v != 0) {
               alt = vertices_dist[u]+tc[u][i];
               if (alt<vertices_dist[v]) {// Relax (u,v)
                  q = PQueue.add(q, v);
                  vertices_dist[v] = alt;
                  vertices_prev[v] = u;
               }
            }
         }
      }
      //Statements below this will be executed if all remaining vertices
      //are inaccessible from source. This means that target vertex is isolated
      //from source vertex. Also means that vertices_dist[t]==9999.
      //You can add them here, usually to alert that no path is found.
   }
   public function AStar(x1, y1, x2, y2:Number) {
      // Initializations
      var s = y1*w+x1+1;
      var t = y2*w+x2+1;
      var q = [0];// Create open list q (as priority queue)
      var q1 = new Array();// Create closed list q1, The list of nodes already evaluated.
      q = PQueue.add(q, s);// Add s into open list q
      vertices_dist = new Array();
      var vertices_prev = new Array();
      vertices_next = new Array();
      var h_score = new Array();
      var f_score = new Array();
      for (var i = 0; i<=n; i++) {
         q1.push(0);
         vertices_dist.push(9999);// Unknown distance function from source to v
         h_score.push(0);
         f_score.push(0);
         vertices_prev.push(0);// Previous node in optimal path from source
         vertices_next.push(0);
      }
      vertices_dist[s] = 0;// Distance from start along optimal path.
      h_score[s] = findHeuristic(x1, y1, x2, y2);// Heuristic distance
      f_score[s] = h_score[s];// Estimated total distance from start to goal.
      var u, v, alt, tentative_g_score, tentative_is_better;
      while (q.length>1) {// The main loop
         //Finds vertex u with least value of path distance
         var d = f_score[q[1]];
         u = q[1];
         if (q.length>1) {
            for (var ii = 2; ii<=q.length; ii++) {
               if (f_score[q[ii]]<d) {
                  d = f_score[q[ii]];
                  u = q[ii];
               }
            }
         }
         if (u == t) {
            //If u is the target, search completed and returns its distance.
            //You can add statements here, usually to read/construct path
            while (vertices_prev[u] != s && vertices_prev[u] != 0) {
               vertices_next[vertices_prev[u]] = u;
               u = vertices_prev[u];
            }
            vertices_next[s] = u;
            return vertices_dist[t];
         }
         q = PQueue.remove(q, u);
         q1[u] = 1;
         for (var i = 1; i<=neighbors; i++) {
            v = nb[u][i];// where v has not yet been removed from Q.
            if (v != 0) {
               if (!q1[v]) {
                  tentative_g_score = vertices_dist[u]+tc[u][i];
                  if (!PQueue.found(q, v)) {
                     q = PQueue.add(q, v);
                     tentative_is_better = true;
                  } else if (tentative_g_score<vertices_dist[v]) {
                     tentative_is_better = true;
                  } else {
                     tentative_is_better = false;
                  }
                  if (tentative_is_better) {
                     if (vertices_prev[v] != 0) {
                        if (f_score[u]<f_score[vertices_prev[v]]) {
                           vertices_prev[v] = u;
                        }
                     } else {
                        vertices_prev[v] = u;
                     }
                     vertices_dist[v] = tentative_g_score;
                     h_score[v] = findHeuristic(Math.floor((v-1)/w), (v-1)%w, x2, y2);
                     f_score[v] = vertices_dist[v]+h_score[v];
                  }
               }
            }
         }
      }
      //Statements below this will be executed if all remaining vertices
      //are inaccessible from source. This means that target vertex is isolated
      //from source vertex. Also means that vertices_dist[t]==9999.
      //You can add them here, usually to alert that no path is found.
   }
   public function get _h() {
      return h;
   }
   public function get _w() {
      return w;
   }
   public function nextp(x:Number) {
      return vertices_next[x];
   }
}