/**
   * Adds an {@link UndirectedIntWeightedEdge} with given weight from given source to target {@link
   * Node} to {@link #graph}. If the graph contains this edge already, the weight of this edge is
   * increased by given weight.
   *
   * @param source The source node of the edge to add.
   * @param target The target node of the edge to add.
   * @param weight The weight of the edge to add or -if the edge already exists- the weight added to
   *     the current weight of the existing edge.
   */
  private void addUndirectedEdge(Node source, Node target, int weight) {
    final UndirectedWeightedEdge edge =
        (UndirectedWeightedEdge) this.gds.newEdgeInstance(source, target);
    final IntWeight newWeight = new IntWeight(this.calculcateEdgeWeight(edge, weight));

    if (this.graph.containsEdge(edge))
      ((UndirectedWeightedEdge) this.graph.getEdge(source, target)).setWeight(newWeight);
    else {
      edge.setWeight(newWeight);
      this.graph.addEdge(edge);
    }
  }
  private boolean undirectedAdd(UndirectedWeightedEdge e, UndirectedNode s) {
    HashMap<Node, Node> parent = parents.get(s);
    HashMap<Node, Integer> height = heights.get(s);

    IntWeight eWeight = (IntWeight) e.getWeight();

    UndirectedNode n1 = e.getNode1();
    UndirectedNode n2 = e.getNode2();

    if (height.get(n1) > height.get(n2)) {
      n1 = e.getNode2();
      n2 = e.getNode1();
    }

    if (height.get(n1) + (int) eWeight.getWeight() >= height.get(n2)
        || height.get(n1) + (int) eWeight.getWeight() < 0) {
      return true;
    }
    if (height.get(n2) != Integer.MAX_VALUE) apsp.decr(height.get(n2));
    apsp.incr(height.get(n1) + (int) eWeight.getWeight());
    height.put(n2, height.get(n1) + (int) eWeight.getWeight());
    parent.put(n2, n1);
    PriorityQueue<Node> q = new PriorityQueue<>();
    q.add(n2);
    while (!q.isEmpty()) {
      Node current = q.poll();

      if (height.get(current) == Integer.MAX_VALUE) {
        break;
      }

      for (IElement edge : current.getEdges()) {
        UndirectedWeightedEdge d = (UndirectedWeightedEdge) edge;

        Node neighbor = d.getDifferingNode(current);
        IntWeight dWeight = (IntWeight) d.getWeight();

        int alt = height.get(current) + (int) dWeight.getWeight();

        if (alt < height.get(neighbor)) {
          if (height.get(neighbor) != Integer.MAX_VALUE) apsp.decr(height.get(neighbor));
          apsp.incr(alt);
          height.put(neighbor, alt);
          parent.put(neighbor, current);
          if (q.contains(neighbor)) {
            q.remove(neighbor);
          }
          q.add(neighbor);
        }
      }
    }
    return true;
  }
  private boolean applyAfterUndirectedEdgeWeight(Update u) {
    UndirectedWeightedEdge e = (UndirectedWeightedEdge) ((EdgeWeight) u).getEdge();
    UndirectedNode n1 = e.getNode1();
    UndirectedNode n2 = e.getNode2();

    IntWeight eWeight = (IntWeight) e.getWeight();

    for (IElement ie : g.getNodes()) {
      UndirectedNode s = (UndirectedNode) ie;
      HashMap<Node, Node> parent = parents.get(s);
      HashMap<Node, Integer> height = heights.get(s);

      UndirectedNode src;
      UndirectedNode dst;
      if (height.get(n1) > height.get(n2)) {
        src = n2;
        dst = n1;
      } else {
        src = n1;
        dst = n2;
      }

      if (!parent.containsKey(dst)
          || height.get(src) + (int) eWeight.getWeight() == height.get(dst)) {
        continue;
      }

      if (height.get(src) + (int) eWeight.getWeight() > height.get(dst)
          || height.get(src) + (int) eWeight.getWeight() < 0) {
        if (parent.get(dst).equals(src)) {
          undirectedDelete(s, e);
        }
      } else {
        undirectedAdd(e, s);
      }
      apsp.truncate();
    }
    return true;
  }
  @Override
  public boolean compute() {

    for (IElement ie : g.getNodes()) {
      Node s = (Node) ie;

      HashMap<Node, Node> parent = new HashMap<Node, Node>();
      HashMap<Node, Integer> height = new HashMap<Node, Integer>();

      for (IElement iNode : g.getNodes()) {
        Node t = (Node) iNode;
        if (t.equals(s)) {
          height.put(s, 0);
        } else {
          height.put(t, Integer.MAX_VALUE);
        }
      }
      if (DirectedNode.class.isAssignableFrom(this.g.getGraphDatastructures().getNodeType())) {
        PriorityQueue<QueueElement<DirectedNode>> q =
            new PriorityQueue<QueueElement<DirectedNode>>();
        q.add(new QueueElement((DirectedNode) s, height.get(s)));
        while (!q.isEmpty()) {
          QueueElement<DirectedNode> c = q.poll();
          DirectedNode current = c.e;
          if (height.get(current) == Integer.MAX_VALUE) {
            break;
          }

          for (IElement iEdge : current.getOutgoingEdges()) {
            DirectedWeightedEdge d = (DirectedWeightedEdge) iEdge;
            IntWeight dWeight = (IntWeight) d.getWeight();

            DirectedNode neighbor = d.getDst();

            int alt = height.get(current) + (int) dWeight.getWeight();
            if (alt < 0) {
              continue;
            }
            if (alt < height.get(neighbor)) {
              height.put(neighbor, alt);
              parent.put(neighbor, current);
              QueueElement<DirectedNode> temp =
                  new QueueElement<DirectedNode>(neighbor, height.get(neighbor));
              if (q.contains(temp)) {
                q.remove(temp);
              }
              q.add(temp);
            }
          }
        }
      } else {
        PriorityQueue<QueueElement<Node>> q = new PriorityQueue<QueueElement<Node>>();
        q.add(new QueueElement((Node) s, height.get(s)));
        while (!q.isEmpty()) {
          QueueElement<Node> c = q.poll();
          Node current = c.e;

          if (height.get(current) == Integer.MAX_VALUE) {
            break;
          }

          for (IElement iEdge : current.getEdges()) {
            UndirectedWeightedEdge d = (UndirectedWeightedEdge) iEdge;
            IntWeight dWeight = (IntWeight) d.getWeight();

            Node neighbor = d.getDifferingNode(current);

            int alt = height.get(current) + (int) dWeight.getWeight();
            if (alt < 0) {
              continue;
            }
            if (alt < height.get(neighbor)) {
              height.put(neighbor, alt);
              parent.put(neighbor, current);
              QueueElement<Node> temp = new QueueElement<Node>(neighbor, height.get(neighbor));
              if (q.contains(temp)) {
                q.remove(temp);
              }
              q.add(temp);
            }
          }
        }
      }
      for (int i : height.values()) {
        if (i != Integer.MAX_VALUE && i != 0) {
          apsp.incr(i);
        }
      }
      apsp.truncate();
      parents.put(s, parent);
      heights.put(s, height);
    }

    return true;
  }
  private void undirectedDelete(Node r, UndirectedEdge e) {
    HashMap<Node, Node> parent = this.parents.get(r);
    HashMap<Node, Integer> height = this.heights.get(r);
    Node n1 = e.getNode1();
    Node n2 = e.getNode2();
    Node src;
    Node dst;

    if (height.get(n1) > height.get(n2)) {
      src = n2;
      dst = n1;
    } else {
      src = n1;
      dst = n2;
    }

    // if the source or dst or edge is not in tree do nothing
    if (height.get(src) == Integer.MAX_VALUE
        || height.get(dst) == Integer.MAX_VALUE
        || height.get(dst) == 0
        || parent.get(dst) != src) {
      return;
    }

    // Queues and data structure for tree change
    HashSet<Node> uncertain = new HashSet<Node>();
    HashSet<Node> changed = new HashSet<Node>();

    PriorityQueue<QueueElement<Node>> q = new PriorityQueue<QueueElement<Node>>();

    q.add(new QueueElement<Node>(dst, height.get(dst)));

    uncertain.add(dst);
    parent.remove(dst);

    while (!q.isEmpty()) {
      QueueElement<Node> qE = q.poll();
      Node w = qE.e;

      int key = qE.distance;

      // find the new shortest path
      int dist = Integer.MAX_VALUE;

      ArrayList<Node> minSettled = new ArrayList<Node>();
      ArrayList<Node> min = new ArrayList<Node>();
      for (IElement iEdge : w.getEdges()) {
        UndirectedWeightedEdge edge = (UndirectedWeightedEdge) iEdge;
        IntWeight eWeight = (IntWeight) edge.getWeight();
        Node z = edge.getDifferingNode(w);
        if (changed.contains(z) || height.get(z) == Integer.MAX_VALUE) {
          continue;
        }
        if (height.get(z) + (int) eWeight.getWeight() < dist) {
          min.clear();
          minSettled.clear();
          min.add(z);
          if (!uncertain.contains(z)) minSettled.add(z);
          dist = height.get(z) + (int) eWeight.getWeight();
          continue;
        }
        if (height.get(z) + (int) eWeight.getWeight() == dist) {
          min.add(z);
          if (!uncertain.contains(z)) minSettled.add(z);
          continue;
        }
      }
      boolean noPossibleNeighbour =
          (key >= breakLoop && dist > breakLoop)
              || (min.isEmpty() && (!uncertain.contains(w) || (key == dist)));

      // no neighbour found
      if (noPossibleNeighbour) {
        if (height.get(w) != Integer.MAX_VALUE) apsp.decr(height.get(w));
        height.put(w, Integer.MAX_VALUE);
        parent.remove(w);
        continue;
      }
      if (uncertain.contains(w)) {
        if (key == dist) {
          if (minSettled.isEmpty()) {
            parent.put(w, min.get(0));
          } else {
            parent.put(w, minSettled.get(0));
          }
          if (height.get(w) != Integer.MAX_VALUE) apsp.decr(height.get(w));
          apsp.incr(dist);
          height.put(w, dist);
          for (IElement iEdge : w.getEdges()) {
            UndirectedWeightedEdge ed = (UndirectedWeightedEdge) iEdge;
            IntWeight edWeight = (IntWeight) ed.getWeight();
            Node z = ed.getDifferingNode(w);
            if (height.get(z) > dist + (int) edWeight.getWeight()) {
              q.remove(new QueueElement<Node>(z, dist + (int) edWeight.getWeight()));
              q.add(new QueueElement<Node>(z, dist + (int) edWeight.getWeight()));
            }
          }
        } else {
          changed.add(w);
          q.add(new QueueElement<Node>(w, dist));
          uncertain.remove(w);
          for (IElement iEdge : w.getEdges()) {
            UndirectedEdge ed = (UndirectedEdge) iEdge;
            Node z = ed.getDifferingNode(w);
            if (parent.get(z) == w) {
              parent.remove(z);
              uncertain.add(z);

              q.add(new QueueElement<Node>(z, height.get(z)));
            }
          }
        }
        continue;
      }
      if (dist > key) {
        q.add(new QueueElement<Node>(w, dist));
        continue;
      }
      if (minSettled.isEmpty()) {
        parent.put(w, min.get(0));
      } else {
        parent.put(w, minSettled.get(0));
      }
      changed.remove(w);
      if (height.get(w) != Integer.MAX_VALUE) apsp.decr(height.get(w));
      apsp.incr(dist);
      height.put(w, dist);
      for (IElement iEdge : w.getEdges()) {
        UndirectedWeightedEdge ed = (UndirectedWeightedEdge) iEdge;
        IntWeight edWeight = (IntWeight) ed.getWeight();
        Node z = ed.getDifferingNode(w);
        if (height.get(z) > dist + (int) edWeight.getWeight()) {
          q.remove(new QueueElement<Node>(z, dist + (int) edWeight.getWeight()));
          q.add(new QueueElement<Node>(z, dist + (int) edWeight.getWeight()));
        }
      }
    }
  }