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(int point_weight) {
    strokeWeight(point_weight);
    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(int graph_weight, int point_weight) {
    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), point_weight, graph_weight);
      }
    }
  }

  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 setup() {
  size(1000, 1000);
  frameRate(60);
  background(255);
  test();
  edges = bfs(0, 6);
}


//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 weight) {
  if (x1 == x2 && y1 == y2) return;
  float angle = atan2(y2-y1, x2 - x1);
  pushMatrix();
  translate(x1, y1);
  PVector v = PVector.fromAngle(angle);
  v.setMag(dist(x1, y1, x2, y2) - min(offset, weight) - offset/2);
  line(0, 0, v.x, v.y);
  pushMatrix();
  translate(v.x, v.y);
  rotate(angle);
  float triangle_size = dist(x1, y1, x2, y2) / 50;
  triangle(-triangle_size * 2, - triangle_size, 0, 0, -triangle_size * 2, triangle_size);
  popMatrix();
  popMatrix();
}


ArrayList<int[]> bfs(int start, int end) {
  ArrayList<Integer> q = new ArrayList<Integer>();
  ArrayList<int[]> res = new ArrayList<int[]>();
  q.add(start);
  int[] 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);
    for (int neighbor : g.adj.get(next)) {
      if (vis[neighbor]) continue;
      q.add(neighbor);
      edge = new int[]{next, neighbor};
      res.add(edge);
      q_size++;
      if (neighbor == 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.add_edge(0, 2);
  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() {
  int edge_weight = 20;
  int point_weight = 30;
  int graph_weight = 10;
  if (frameCount % 60 == 0) {
    g.draw_graph(graph_weight, point_weight);
    g.draw_points(point_weight);
    stroke(#00FF00);
    if (!edges.isEmpty()) {
      int[] edge = edges.get(0);
      edges.remove(0);
      strokeWeight(edge_weight);
      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), point_weight, edge_weight);
    }
  }
}







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