class Delay {
  int limit;

  Delay (int l) {
    limit = millis() + l;
  }

  boolean expired () {
    return millis() > limit;
  }
}

class Graph {
  ArrayList<ArrayList<Integer>> adj;
  ArrayList<ArrayList<Integer>> coords;
  int V;
  Graph(int v) {
    V = v;
    adj = new ArrayList<ArrayList<Integer>>(V);
    coords = new ArrayList<ArrayList<Integer>>(V);
    for (int i = 0; i < V; i++) {
      adj.add(new ArrayList<Integer>());
      coords.add(new ArrayList<Integer>());
    }
  }

  void draw_vis(boolean[] vis) {
    strokeWeight(20);
    stroke(255);
    for (int i = 0; i < coords.size(); i++) {
      if (!vis[i]) continue;
      point(coords.get(i).get(0), coords.get(i).get(1));
      text(i+1, coords.get(i).get(0)-4, coords.get(i).get(1)+5);
    }
  }

  void draw_points() {
    strokeWeight(20);
    stroke(#FF0000);
    for (int i = 0; i < coords.size(); i++) {
      point(coords.get(i).get(0), coords.get(i).get(1));
      text(i+1, coords.get(i).get(0)-4, coords.get(i).get(1)+5);
    }
  }

  void draw_graph() {
    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);
        draw_arrow(coords.get(i).get(0), coords.get(i).get(1), coords.get(e).get(0), coords.get(e).get(1), 5);
      }
    }
  }

  void set_coords(int idx, int x, int y) {
    coords.get(idx).add(x);
    coords.get(idx).add(y);
  }

  void add_edge(int u, int v) {
    adj.get(u).add(v);
  }
}

Graph g;
ArrayList<int[]> edges;
boolean[] vis;

void draw_arrow_2(float x1, float y1, float x2, float y2, float offset) {
  float triangle_size = dist(x1, y1, x2, y2) / 50;
  pushMatrix();
  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);
  popMatrix();
  line(x1, y1, x2, y2);
}

void draw_arrow(float x1, float y1, float x2, float y2, float offset) {
  float angle = atan2(y2-y1, x2 - x1);
  float offset_x = offset;
  float offset_y = offset;
  //if (x2 < x1) offset_x *= -1;
  //if (y2 < y1) offset_y *= -1;
  line(x1 + offset_x * sqrt(1-sin(angle)), y1 + offset_y * sin(angle), 
  x2 - offset_x * sqrt(1-sin(angle)), y2 - offset_y * sin(angle)); 
}


void setup() {
  size(1000, 1000);
  frameRate(60);
  background(255);
  test();
  edges = bfs(0, 6);
}



ArrayList<int[]> bfs(int start, int end) {
  ArrayList<Integer> q = new ArrayList<Integer>();
  ArrayList<int[]> res = new ArrayList<int[]>();
  q.add(start);
  int[] edge = new int[] {0, 0};
  res.add(edge);
  int q_size = 1;
  while (q_size > 0) {
    int next = q.get(0);
    q.remove(0);
    q_size--;
    vis[next] = true;
    g.add_edge(0, 0);
    print(g.adj.size());
    for (int neighbor : g.adj.get(next)) {
      print(neighbor);
      if (vis[neighbor]) continue;
      q.add(neighbor);
      edge = new int[]{next, neighbor};
      res.add(edge);
      q_size++;
      if (neighbor == end) return res;
    }
    //if (q_size == 0) {
    //  print("end");
    //  return res;
    //}
  }
  return res;
}

void test() {
  int size = 7;
  g = new Graph(size);
  g.add_edge(0, 1);
  g.add_edge(1, 3);
  g.add_edge(1, 4);
  g.add_edge(2, 0);
  g.add_edge(4, 5);
  g.add_edge(5, 6);
  g.set_coords(0, 100, 500);
  g.set_coords(1, 300, 500);
  g.set_coords(2, 300, 750);
  g.set_coords(3, 500, 500);
  g.set_coords(4, 300, 250);
  g.set_coords(5, 500, 250);
  g.set_coords(6, 700, 250);
  vis = new boolean[]{false, false, false, false, false, false, false};
}

void draw() {
  if (frameCount % 60 == 0) {
    g.draw_graph();
    g.draw_points();
    stroke(#00FF00);
    if (!edges.isEmpty()) {
      int[] edge = edges.get(0);
      edges.remove(0);
      strokeWeight(20);
      draw_arrow(g.coords.get(edge[0]).get(0), g.coords.get(edge[0]).get(1), g.coords.get(edge[1]).get(0), g.coords.get(edge[1]).get(1), 5);
    }
  }
}







// TODO: So umstrukturieren, dass man einen Algorithmus hat, der einem die nächste Kante gibt, die man dann im draw malen muss.