void setup() {
  size(1500, 1500);
}

void draw_arrow(float x1, float y1, float x2, float y2) {
  line(x1, y1, x2, y2);
  float triangle_size = dist(x1, y1, x2, y2) / 50;
  translate(x2, y2);  //changes (0, 0)
  rotate(atan2(y2 - y1, x2 - x1)); 
  triangle( -triangle_size * 2, - triangle_size, 0, 0, -triangle_size * 2, triangle_size);
}

void draw_points(ArrayList<ArrayList<Integer>> coords) {
  strokeWeight(20);
   stroke(#FF0000);
   print(coords.size());
  for (int i = 0; i < coords.size(); i++) {
    print(i);
      point(coords.get(i).get(0), coords.get(i).get(1));
      //text(i+1, coords.get(i).get(0), coords.get(i).get(1));
  }
}

void draw_vis(ArrayList<ArrayList<Integer>> coords, boolean[] vis) {
  strokeWeight(20);
  stroke(#555555);
  for (int i = 0; i < coords.size(); i++) {
      if (!vis[i]) continue;
      point(coords.get(i).get(0), coords.get(i).get(1));
  }
}

void draw_graph(ArrayList<ArrayList<Integer>> adj, ArrayList<ArrayList<Integer>> coords) {

  draw_points(coords);
  stroke(#550000);
  strokeWeight(10);
  for (int i = 0; i < adj.size(); i++) {
     for (int idx = 0; idx < adj.get(i).size(); idx++) {
       int e = adj.get(i).get(idx);
        print(adj.get(i).size());
       draw_arrow(coords.get(i).get(0), coords.get(i).get(1), coords.get(e).get(0), coords.get(e).get(1));
     }
  }
}

void bfs(ArrayList<ArrayList<Integer>> adj, ArrayList<ArrayList<Integer>> coords, boolean[] vis, int start, int end) {
    ArrayList<Integer> q = new ArrayList<Integer>();
    q.add(start);
    while (q.size() != 0) {
        int next = q.get(0);
        q.remove(0);
        vis[next] = true;
        for (int neighbor: adj.get(next)) {
          if (vis[neighbor]) continue;
          q.add(neighbor);
          print("start");
          draw_graph(adj, coords);
          print("start");
          draw_vis(coords, vis);
          print("start");
          stroke(#00FF00);
          draw_arrow(coords.get(next).get(0), coords.get(next).get(1), coords.get(neighbor).get(0), coords.get(neighbor).get(1));
          if (neighbor == end) return;
          delay(1000);
        }
    }
}

void test() {
  int size = 5;
  ArrayList<Integer> empty = new ArrayList<Integer>();
  ArrayList<ArrayList<Integer>> adj = new ArrayList<ArrayList<Integer>>();
  ArrayList<ArrayList<Integer>> coords = new ArrayList<ArrayList<Integer>>();
  for (int i = 0; i < size; i++) {
    adj.add(empty);
    coords.add(empty);
  }
  adj.get(0).add(2);
  adj.get(1).add(4);
  adj.get(1).add(3);
  adj.get(2).add(0);
  coords.get(0).add(width/10); coords.get(0).add(height-100);
  coords.get(1).add(width/5); coords.get(1).add(height-100);
  coords.get(3).add(width/2); coords.get(3).add(height-100);
  coords.get(4).add(width/2); coords.get(4).add(height/2);
  coords.get(2).add(width/5); coords.get(2).add(height/2);
  boolean[] vis = new boolean[size];
  for (int i = 0; i < size; i++) {
    vis[i] = false; 
  }
  bfs(adj, coords, vis, 0, 4);
  
}

void draw() {
  background(0);
  print("a");
  test();
  //strokeWeight(20);
  //stroke(#550000);
  //draw_arrow(width/2, height/2, width/3, height/2);
  
}