//draw the pathfinder grid
void drawGrid(){
  stroke(0);
  fill(0, 0, 255);
  strokeWeight(1);
  for(int i = 0; i < pathfinder.wide; i++){
    for(int j = 0; j < pathfinder.high; j++){
      if(pathfinder.grid[i][j] < 0){
        rect(i * xstep, j * ystep, xstep, ystep);
      }
    }
  }
  for(int i = 1; i < pathfinder.wide; i++){
    line(0, i * xstep, width, i * xstep);
    line(i * ystep, 0, i * ystep, height);
  }
}
//draw the closed and open list of nodes, put cost of reaching target in top left corner
//and sequence number of closed list nodes in bottom left corner
void drawLists(){
  strokeWeight(3);
  ellipseMode(CENTER);
  for(int i = 0; i < pathfinder.openlist.size(); i++){
    Pathfinder.Node temp = (Pathfinder.Node)pathfinder.openlist.get(i);
    stroke(10, 200, 10);
    noFill();
    ellipse((xstep>>1) + temp.n.x * xstep, (ystep>>1) + temp.n.y * ystep, 10, 10);
    float theta = atan2(temp.p.y - temp.n.y, temp.p.x - temp.n.x);
    float x = ((xstep>>1) + temp.n.x * xstep) + cos(theta) * 20;
    float y = ((ystep>>1) + temp.n.y * ystep) + sin(theta) * 20;
    line((xstep>>1) + temp.n.x * xstep, (ystep>>1) + temp.n.y * ystep, x, y);
    fill(0);
    text(temp.cost, temp.n.x * xstep, 10 + temp.n.y * ystep);
  }
  for(int i = 1; i < pathfinder.closedlist.size(); i++){
    Pathfinder.Node temp = (Pathfinder.Node)pathfinder.closedlist.get(i);
    stroke(200, 200, 10);
    noFill();
    ellipse((xstep>>1) + temp.n.x * xstep, (ystep>>1) + temp.n.y * ystep, 10, 10);
    float theta = atan2(temp.p.y - temp.n.y, temp.p.x - temp.n.x);
    float x = ((xstep>>1) + temp.n.x * xstep) + cos(theta) * 20;
    float y = ((ystep>>1) + temp.n.y * ystep) + sin(theta) * 20;
    line((xstep>>1) + temp.n.x * xstep, (ystep>>1) + temp.n.y * ystep, x, y);
    fill(0);
    text(temp.cost, temp.n.x * xstep, 10 + temp.n.y * ystep);
    text(i, temp.n.x * xstep, (ystep - 10) + temp.n.y * ystep);
  }
}
//draw pathfinder bots
void drawBots(){
  noFill();
  strokeWeight(3);
  stroke(0, 255, 0);
  for(int i = 0; i < pathfinder.bot.length; i++){
    ellipse((xstep>>1) + pathfinder.bot[i].x * xstep, (ystep>>1) + pathfinder.bot[i].y * ystep, xstep - 5, ystep - 5);
  }
}
//draw pathfinder targets
void drawTargets(){
  noFill();
  strokeWeight(3);
  stroke(255, 0, 0);
  for(int i = 0; i < pathfinder.target.length; i++){
    ellipse((xstep>>1) + pathfinder.target[i].x * xstep, (ystep>>1) + pathfinder.target[i].y * ystep, xstep - 5, ystep - 5);
  }
}
//draw bestVec2
void drawBestVec2(Vec2 temp){
  noFill();
  strokeWeight(4);
  stroke(0, 0, 255);
  ellipse((xstep>>1) + temp.x * xstep, (ystep>>1) + temp.y * ystep, xstep - 15, ystep - 15);
}
//drawPathway
void drawPath(){
  Vector pathway = pathfinder.logicalPath(0, 0);
  stroke(255, 255, 0);
  strokeWeight(1);
  beginShape(LINE_STRIP);
  for(int i = 0; i < pathway.size(); i++){
    Vec2 temp = (Vec2)pathway.get(i);
    vertex((xstep>>1) + temp.x * xstep, (ystep>>1) + temp.y * ystep);
  }
  endShape();
}