AStarSearch(startWp, goalWp)
{
  pQOpen = [];
  pQSize = 0;
  closedList = [];
  listSize = 0;
  s = spawnstruct();
  s.g = 0; //start node
  s.h = distance(level.waypoints[startWp].origin, level.waypoints[goalWp].origin);
  s.f = s.g + s.h;
  s.wpIdx = startWp;
  s.parent = spawnstruct();
  s.parent.wpIdx = -1;

  PQPush(pQOpen, s, pQSize); //push s on Open
  pQSize++;

  //while Open is not empty
  while(!PQIsEmpty(pQOpen, pQSize))
  {
    //pop node n from Open  // n has the lowest f
    n = PQPop(pQOpen, pQSize);
    pQSize--;

    //if n is a goal node; construct path, return success
    if(n.wpIdx == goalWp)
    {

      x = n;
      for(z = 0; z < 100; z++)
      {
        parent = x.parent;
        if(parent.wpIdx == -1)
        {
          return;
        }
        line(level.waypoints[x.wpIdx].origin, level.waypoints[parent.wpIdx].origin, (0,1,0));
        x = parent;
      }

      return;
    }

    //for each successor nc of n
    for(i = 0; i < level.waypoints[n.wpIdx].childCount; i++)
    {
      //newg = n.g + cost(n,nc)
      newg = n.g + distance(level.waypoints[n.wpIdx].origin, level.waypoints[level.waypoints[n.wpIdx].children[i]].origin);

      //if nc is in Open or Closed, and nc.g <= newg then skip
      if(PQExists(pQOpen, level.waypoints[n.wpIdx].children[i], pQSize))
      {
        nc = PQGet(pQOpen, level.waypoints[n.wpIdx].children[i], pQSize);

        if(nc.g <= newg)
        {
          continue;
        }
      }
      else
      if(ListExists(closedList, level.waypoints[n.wpIdx].children[i], listSize))
      {
        nc = ListGet(closedList, level.waypoints[n.wpIdx].children[i], listSize);

        if(nc.g <= newg)
        {
          continue;
        }
      }

//      nc.parent = n
//      nc.g = newg
//      nc.h = GoalDistEstimate( nc )
//      nc.f = nc.g + nc.h

      nc = spawnstruct();
      nc.parent = spawnstruct();
      nc.parent = n;
Rendflex: ... yeah