//Pathfinding class: uses A* algorithm with a patch for unfinishable paths
class Pathfinder{
  int [][][] grid;                                  //terrain costs, -1 for a wall positive numbers indicate difficult ground
  boolean [][][] onList;                            //is a grid coord on the closed or open lists?
  Vec3 [] bot;                                      //chasing bots
  Vec3 [] target;                                   //targets
  int wide, high, deep;                             //size of world
  Vector openlist, closedlist;
  Pathfinder(int wide, int high, int deep, int numBots, int numTargets){
    this.wide = wide;
    this.high = high;
    this.deep = deep;
    grid = new int[wide][high][deep];
    for(int i = 0; i < wide; i++){
      for(int j = 0; j < high; j++){
        for(int k = 0; k < high; k++){
          grid[i][j][k] = 0;
        }
      }
    }
    onList = new boolean[wide][high][deep];
    bot = new Vec3[numBots];
    target = new Vec3[numTargets];
    openlist = new Vector();
    closedlist = new Vector();
  }
  //init or relocate bot
  void setBot(int id, int x, int y, int z){
    bot[id] = new Vec3(x, y, z);
  }
  //init or relocate target
  void setTarget(int id, int x, int y, int z){
    target[id] = new Vec3(x, y, z);
  }
  //set terrain costs or a wall
  void setGrid(int x, int y, int z, int value){
    grid[x][y][z] = value;
  }
  //A* pathfinding algorithm
  void findPath(int botID, int targetID, boolean move){
    int x = bot[botID].x;
    int y = bot[botID].y;
    int z = bot[botID].z;
    int targetX = target[targetID].x;
    int targetY = target[targetID].y;
    int targetZ = target[targetID].z;
    //cost is defined by F = G + H
    //G = grid movement cost (all equal in this version but stated for further development)
    //H = heuristic distance (derived from Manhattan formula: width + height from target)
    int G = 10;
    int H = 10 * (abs(x - targetX) + abs(y - targetY) + abs(z - targetZ));
    //if heuristic cost is zero, pathfinding is unnecessary
    if(H == 0){
      return;
    }
    //clear lists
    onList = new boolean[wide][high][deep];
    openlist.clear();
    closedlist.clear();
    //add current locale
    closedlist.add(new Node(null, bot[botID], G + H));
    onList[x][y][z] = true;
    while(x != targetX || y != targetY || z != targetZ){
      //add possible nodes to open list
      //straight line nodes
      //0
      if(gridLegal(x + 1, y, z) && !onList[x + 1][y][z]){
        G = 10;
        H = 10 * (abs(x + 1 - targetX) + abs(y - targetY) + abs(z - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x + 1, y, z), G + H + grid[x + 1][y][z]));
        onList[x + 1][y][z] = true;
      }
      //1
      if(gridLegal(x + 1, y + 1, z) && !onList[x + 1][y + 1][z]){
        G = 14;
        H = 10 * (abs(x + 1 - targetX) + abs(y + 1 - targetY) + abs(z - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x + 1, y + 1, z), G + H + grid[x + 1][y + 1][z]));
        onList[x + 1][y + 1][z] = true;
      }
      //2
      if(gridLegal(x + 1, y + 1, z + 1) && !onList[x + 1][y + 1][z + 1]){
        G = 17;
        H = 10 * (abs(x + 1 - targetX) + abs(y + 1 - targetY) + abs(z + 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x + 1, y + 1, z + 1), G + H + grid[x + 1][y + 1][z + 1]));
        onList[x + 1][y + 1][z + 1] = true;
      }
      //3
      if(gridLegal(x + 1, y, z + 1) && !onList[x + 1][y][z + 1]){
        G = 14;
        H = 10 * (abs(x + 1 - targetX) + abs(y - targetY) + abs(z + 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x + 1, y, z + 1), G + H + grid[x + 1][y][z + 1]));
        onList[x + 1][y][z + 1] = true;
      }
      //4
      if(gridLegal(x + 1, y - 1, z + 1) && !onList[x + 1][y - 1][z + 1]){
        G = 17;
        H = 10 * (abs(x + 1 - targetX) + abs(y - 1 - targetY) + abs(z + 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x + 1, y - 1, z + 1), G + H + grid[x + 1][y - 1][z + 1]));
        onList[x + 1][y - 1][z + 1] = true;
      }
      //5
      if(gridLegal(x + 1, y - 1, z) && !onList[x + 1][y - 1][z]){
        G = 14;
        H = 10 * (abs(x + 1 - targetX) + abs(y - 1 - targetY) + abs(z - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x + 1, y - 1, z), G + H + grid[x + 1][y - 1][z]));
        onList[x + 1][y - 1][z] = true;
      }
      //6
      if(gridLegal(x + 1, y - 1, z - 1) && !onList[x + 1][y - 1][z - 1]){
        G = 17;
        H = 10 * (abs(x + 1 - targetX) + abs(y - 1 - targetY) + abs(z - 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x + 1, y - 1, z - 1), G + H + grid[x + 1][y - 1][z - 1]));
        onList[x + 1][y - 1][z - 1] = true;
      }
      //7
      if(gridLegal(x + 1, y, z - 1) && !onList[x + 1][y][z - 1]){
        G = 14;
        H = 10 * (abs(x + 1 - targetX) + abs(y - targetY) + abs(z - 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x + 1, y, z - 1), G + H + grid[x + 1][y][z - 1]));
        onList[x + 1][y][z - 1] = true;
      }
      //8
      if(gridLegal(x + 1, y + 1, z - 1) && !onList[x + 1][y + 1][z - 1]){
        G = 17;
        H = 10 * (abs(x + 1 - targetX) + abs(y + 1 - targetY) + abs(z - 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x + 1, y + 1, z - 1), G + H + grid[x + 1][y + 1][z - 1]));
        onList[x + 1][y + 1][z - 1] = true;
      }
      //9
      if(gridLegal(x, y + 1, z) && !onList[x][y + 1][z]){
        G = 10;
        H = 10 * (abs(x - targetX) + abs(y + 1 - targetY) + abs(z - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x, y + 1, z), G + H + grid[x][y + 1][z]));
        onList[x][y + 1][z] = true;
      }
      //10
      if(gridLegal(x, y + 1, z + 1) && !onList[x][y + 1][z + 1]){
        G = 14;
        H = 10 * (abs(x - targetX) + abs(y + 1 - targetY) + abs(z + 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x, y + 1, z + 1), G + H + grid[x][y + 1][z + 1]));
        onList[x][y + 1][z + 1] = true;
      }
      //11
      if(gridLegal(x, y, z + 1) && !onList[x][y][z + 1]){
        G = 10;
        H = 10 * (abs(x - targetX) + abs(y - targetY) + abs(z + 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x, y, z + 1), G + H + grid[x][y][z + 1]));
        onList[x][y][z + 1] = true;
      }
      //12
      if(gridLegal(x, y - 1, z + 1) && !onList[x][y - 1][z + 1]){
        G = 14;
        H = 10 * (abs(x - targetX) + abs(y - 1 - targetY) + abs(z + 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x, y - 1, z + 1), G + H + grid[x][y - 1][z + 1]));
        onList[x][y - 1][z + 1] = true;
      }
      //13
      if(gridLegal(x, y - 1, z) && !onList[x][y - 1][z]){
        G = 10;
        H = 10 * (abs(x - targetX) + abs(y - 1 - targetY) + abs(z - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x, y - 1, z), G + H + grid[x][y - 1][z]));
        onList[x][y - 1][z] = true;
      }
      //14
      if(gridLegal(x, y - 1, z - 1) && !onList[x][y - 1][z - 1]){
        G = 14;
        H = 10 * (abs(x - targetX) + abs(y - 1 - targetY) + abs(z - 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x, y - 1, z - 1), G + H + grid[x][y - 1][z - 1]));
        onList[x][y - 1][z - 1] = true;
      }
      //15
      if(gridLegal(x, y, z - 1) && !onList[x][y][z - 1]){
        G = 10;
        H = 10 * (abs(x - targetX) + abs(y - targetY) + abs(z - 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x, y, z - 1), G + H + grid[x][y][z - 1]));
        onList[x][y][z - 1] = true;
      }
      //16
      if(gridLegal(x, y + 1, z - 1) && !onList[x][y + 1][z - 1]){
        G = 14;
        H = 10 * (abs(x - targetX) + abs(y + 1 - targetY) + abs(z - 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x, y + 1, z - 1), G + H + grid[x][y + 1][z - 1]));
        onList[x][y + 1][z - 1] = true;
      }
      //17
      if(gridLegal(x - 1, y + 1, z) && !onList[x - 1][y + 1][z]){
        G = 14;
        H = 10 * (abs(x - 1 - targetX) + abs(y + 1 - targetY) + abs(z - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x - 1, y + 1, z), G + H + grid[x - 1][y + 1][z]));
        onList[x - 1][y + 1][z] = true;
      }
      //18
      if(gridLegal(x - 1, y + 1, z + 1) && !onList[x - 1][y + 1][z + 1]){
        G = 17;
        H = 10 * (abs(x - 1 - targetX) + abs(y + 1 - targetY) + abs(z + 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x - 1, y + 1, z + 1), G + H + grid[x - 1][y + 1][z + 1]));
        onList[x - 1][y + 1][z + 1] = true;
      }
      //19
      if(gridLegal(x - 1, y, z + 1) && !onList[x - 1][y][z + 1]){
        G = 14;
        H = 10 * (abs(x - 1 - targetX) + abs(y - targetY) + abs(z + 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x - 1, y, z + 1), G + H + grid[x - 1][y][z + 1]));
        onList[x - 1][y][z + 1] = true;
      }
      //20
      if(gridLegal(x - 1, y - 1, z + 1) && !onList[x - 1][y - 1][z + 1]){
        G = 17;
        H = 10 * (abs(x - 1 - targetX) + abs(y - 1 - targetY) + abs(z + 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x - 1, y - 1, z + 1), G + H + grid[x - 1][y - 1][z + 1]));
        onList[x - 1][y - 1][z + 1] = true;
      }
      //21
      if(gridLegal(x - 1, y - 1, z) && !onList[x - 1][y - 1][z]){
        G = 14;
        H = 10 * (abs(x - 1 - targetX) + abs(y - 1 - targetY) + abs(z - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x - 1, y - 1, z), G + H + grid[x - 1][y - 1][z]));
        onList[x - 1][y - 1][z] = true;
      }
      //22
      if(gridLegal(x - 1, y - 1, z - 1) && !onList[x - 1][y - 1][z - 1]){
        G = 17;
        H = 10 * (abs(x - 1 - targetX) + abs(y - 1 - targetY) + abs(z - 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x - 1, y - 1, z - 1), G + H + grid[x - 1][y - 1][z - 1]));
        onList[x - 1][y - 1][z - 1] = true;
      }
      //23
      if(gridLegal(x - 1, y, z - 1) && !onList[x - 1][y][z - 1]){
        G = 14;
        H = 10 * (abs(x - 1 - targetX) + abs(y - targetY) + abs(z - 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x - 1, y, z - 1), G + H + grid[x - 1][y][z - 1]));
        onList[x - 1][y][z - 1] = true;
      }
      //24
      if(gridLegal(x - 1, y + 1, z - 1) && !onList[x - 1][y + 1][z - 1]){
        G = 17;
        H = 10 * (abs(x - 1 - targetX) + abs(y + 1 - targetY) + abs(z - 1 - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x - 1, y + 1, z - 1), G + H + grid[x - 1][y + 1][z - 1]));
        onList[x - 1][y + 1][z - 1] = true;
      }
      //25
      if(gridLegal(x - 1, y, z) && !onList[x - 1][y][z]){
        G = 10;
        H = 10 * (abs(x - 1 - targetX) + abs(y - targetY) + abs(z - targetZ));
        openlist.add(new Node(new Vec3(x,y,z), new Vec3(x - 1, y, z), G + H + grid[x - 1][y][z]));
        onList[x - 1][y][z] = true;
      }
      //find lowest costing node and add to closed list
      int lowestCost = 10000;
      int lowestCostNode = -1;
      for(int i = 0; i < openlist.size(); i++){
        Node temp = (Node)openlist.get(i);
        if(temp.cost < lowestCost){
          lowestCost = temp.cost;
          lowestCostNode = i;
        }
      }
      //escape node search if the target cannot be found
      if(lowestCostNode == -1){
        break;
      }
      Node temp = (Node)openlist.remove(lowestCostNode);
      closedlist.add(temp);
      x = temp.n.x;
      y = temp.n.y;
      z = temp.n.z;
    }
    if(move){
      //create logical path to target
      if(closedlist.size() > 1){
        Vector pathway = logicalPath(botID, targetID);
        Vec3 temp = (Vec3)pathway.get(pathway.size() - 2);
        bot[botID].x = temp.x;
        bot[botID].y = temp.y;
        bot[botID].z = temp.z;
      }
    }
  }
  //creates a path to the target from the closed list
  Vector logicalPath(int botID, int targetID){
    Vector pathway = new Vector();
    pathway.add(target[targetID]);
    int x = target[targetID].x;
    int y = target[targetID].y;
    int z = target[targetID].z;
    //check if path to target is complete
    //if not, generate path to lowest cost node
    Node test = (Node)closedlist.get(closedlist.size() - 1);
    if(test.n.x != x && test.n.y != y && test.n.z != z){
      pathway.clear();
      int G = 10;
      int H = 10 * (abs(bot[botID].x - x) + abs(bot[botID].y - y) + abs(bot[botID].z - z));
      int lowestCostNode = G + H;
      for(int i = 0; i < closedlist.size(); i++){
        Node temp = (Node)closedlist.get(i);
        if(temp.cost < lowestCostNode){
          x = temp.n.x;
          y = temp.n.y;
          z = temp.n.z;
          lowestCostNode = temp.cost;
        }
      }
      //workaround for being already at the lowest cost node
      if(lowestCostNode == G + H){
        x = bot[botID].x;
        y = bot[botID].y;
        z = bot[botID].z;
        pathway.add(new Vec3(x, y, z));
      }
      pathway.add(new Vec3(x, y, z));
    }
    for(int i = closedlist.size() - 1; i > 0; i--){
      Node temp = (Node)closedlist.get(i);
      if(temp.n.x == x && temp.n.y == y && temp.n.z == z){
        pathway.add(temp.p);
        x = temp.p.x;
        y = temp.p.y;
        z = temp.p.z;
      }
    }
    return pathway;
  }
  //is a grid square filled or no?
  boolean gridLegal(int x, int y, int z){
    if(x < 0 || x > wide - 1 || y < 0 || y > high - 1 || z < 0 || z > deep - 1 || grid[x][y][z] < 0){
      return false;
    }
    return true;
  }
  //pathfinding node class
  class Node{
    Vec3 n, p;            //n = node's location, p = parent's location
    int cost;             //cost of reaching target
    Node(Vec3 p, Vec3 n, int cost){
      this.n = n;
      this.p = p;
      this.cost = cost;
    }
  }
}
//2D vector class
class Vec3{
  int x,y,z;
  Vec3(int x, int y, int z){
    this.x = x;
    this.y = y;
    this.z = z;
  }
  boolean equals(Vec3 other){
    if(x == other.x && y == other.y && z == other.z){
      return true;
    }
    return false;
  }
}