/**
   * Executes an A* search in a graph. It needs a graph, the start and end vertex as well as a
   * heuristic distance measurer.
   */
  public WeightedEdgeContainer<VERTEX_ID> startAStarSearch(
      Graph<VERTEX_ID, VERTEX_VALUE, Integer> g,
      VERTEX_ID start,
      VERTEX_ID goal,
      DistanceMeasurer<VERTEX_ID, VERTEX_VALUE, Integer> measurer) {

    HashSet<VERTEX_ID> closedSet = new HashSet<>();
    HashSet<VERTEX_ID> openSet = new HashSet<>();
    HashMap<VERTEX_ID, VERTEX_ID> cameFrom = new HashMap<>();
    // distance from the start from start along optimal path
    HashMap<VERTEX_ID, Integer> g_score = new HashMap<>();
    // distance from start to goal plus heuristic estimate
    HashMap<VERTEX_ID, Double> f_score = new HashMap<>();
    // heuristic score
    HashMap<VERTEX_ID, Double> h_score = new HashMap<>();
    g_score.put(start, 0);
    h_score.put(start, measurer.measureDistance(g, start, goal));
    f_score.put(start, h_score.get(start));

    // create a deep copy
    for (Vertex<VERTEX_ID, VERTEX_VALUE> v : g.getVertexSet()) openSet.add(v.getVertexId());

    while (!openSet.isEmpty()) {
      VERTEX_ID v = findLowest(openSet, f_score);
      if (v.equals(goal)) {
        return new WeightedEdgeContainer<>(g_score, cameFrom);
      } else {
        openSet.remove(v);
        closedSet.add(v);
        for (Edge<VERTEX_ID, Integer> y : g.getEdges(v)) {
          boolean tentativeIsBetter = false;
          if (closedSet.contains(y.getDestinationVertexID())) continue;

          int tentativeGScore = g_score.get(v) + y.getValue();

          Integer gScore = g_score.get(y);
          if (!openSet.contains(y.getDestinationVertexID())) {
            openSet.add(y.getDestinationVertexID());
            tentativeIsBetter = true;
          } else if (gScore == null || tentativeGScore < gScore) {
            tentativeIsBetter = true;
          } else {
            tentativeIsBetter = false;
          }

          if (tentativeIsBetter) {
            cameFrom.put(y.getDestinationVertexID(), v);

            g_score.put(y.getDestinationVertexID(), tentativeGScore);
            double dist = measurer.measureDistance(g, y.getDestinationVertexID(), goal);
            h_score.put(y.getDestinationVertexID(), dist);
            f_score.put(y.getDestinationVertexID(), tentativeGScore + dist);
          }
        }
      }
    }
    return new WeightedEdgeContainer<>(g_score, cameFrom);
  }
 private double averageEdgeWeights(Graph<VERTEX_ID, VERTEX_VALUE, Integer> g, VERTEX_ID vertex) {
   int sum = 0;
   int count = 0;
   Set<Edge<VERTEX_ID, Integer>> adjacents = g.getEdges(vertex);
   if (adjacents == null) return 0;
   for (Edge<VERTEX_ID, Integer> e : adjacents) {
     sum += e.getValue();
     count++;
   }
   if (count == 0) return 0;
   else return sum / count;
 }