public static int getIdOf(Graph g, double latitude, double longitude) {
   int s = g.getNodes();
   NodeAccess na = g.getNodeAccess();
   for (int i = 0; i < s; i++) {
     if (Math.abs(na.getLatitude(i) - latitude) < 1e-4
         && Math.abs(na.getLongitude(i) - longitude) < 1e-4) {
       return i;
     }
   }
   return -1;
 }
  void queryIndex(Graph g, LocationIndex idx, double lat, double lon, double expectedDist) {
    QueryResult res = idx.findClosest(lat, lon, EdgeFilter.ALL_EDGES);
    if (!res.isValid()) {
      errors.add("node not found for " + lat + "," + lon);
      return;
    }

    GHPoint found = res.getSnappedPoint();
    double dist = distCalc.calcDist(lat, lon, found.lat, found.lon);
    if (Math.abs(dist - expectedDist) > .1) {
      errors.add(
          "queried lat,lon="
              + (float) lat
              + ","
              + (float) lon
              + " (found: "
              + (float) found.lat
              + ","
              + (float) found.lon
              + ")"
              + "\n   expected distance:"
              + expectedDist
              + ", but was:"
              + dist);
    }
  }
 public boolean containsLatitude(Graph g, EdgeIterator iter, double latitude) {
   NodeAccess na = g.getNodeAccess();
   while (iter.next()) {
     if (Math.abs(na.getLatitude(iter.getAdjNode()) - latitude) < 1e-4) return true;
   }
   return false;
 }
  public TestAlgoCollector assertDistance(
      AlgoHelperEntry algoEntry, List<QueryResult> queryList, OneRun oneRun) {
    List<Path> altPaths = new ArrayList<Path>();
    QueryGraph queryGraph = new QueryGraph(algoEntry.getQueryGraph());
    queryGraph.lookup(queryList);
    AlgorithmOptions opts = algoEntry.opts;
    FlagEncoder encoder = opts.getFlagEncoder();
    if (encoder.supports(TurnWeighting.class))
      algoEntry.setAlgorithmOptions(
          AlgorithmOptions.start(opts)
              .weighting(
                  new TurnWeighting(
                      opts.getWeighting(),
                      opts.getFlagEncoder(),
                      (TurnCostExtension) queryGraph.getExtension()))
              .build());

    for (int i = 0; i < queryList.size() - 1; i++) {
      RoutingAlgorithm algo = algoEntry.createAlgo(queryGraph);
      Path path =
          algo.calcPath(queryList.get(i).getClosestNode(), queryList.get(i + 1).getClosestNode());
      // System.out.println(path.calcInstructions().createGPX("temp", 0, "GMT"));
      altPaths.add(path);
    }

    PathMerger pathMerger =
        new PathMerger().setCalcPoints(true).setSimplifyResponse(false).setEnableInstructions(true);
    AltResponse rsp = new AltResponse();
    pathMerger.doWork(rsp, altPaths, trMap.getWithFallBack(Locale.US));

    if (rsp.hasErrors()) {
      errors.add(
          algoEntry
              + " response contains errors. Expected distance: "
              + rsp.getDistance()
              + ", expected points: "
              + oneRun
              + ". "
              + queryList
              + ", errors:"
              + rsp.getErrors());
      return this;
    }

    PointList pointList = rsp.getPoints();
    double tmpDist = pointList.calcDistance(distCalc);
    if (Math.abs(rsp.getDistance() - tmpDist) > 2) {
      errors.add(
          algoEntry
              + " path.getDistance was  "
              + rsp.getDistance()
              + "\t pointList.calcDistance was "
              + tmpDist
              + "\t (expected points "
              + oneRun.getLocs()
              + ", expected distance "
              + oneRun.getDistance()
              + ") "
              + queryList);
    }

    if (Math.abs(rsp.getDistance() - oneRun.getDistance()) > 2) {
      errors.add(
          algoEntry
              + " returns path not matching the expected distance of "
              + oneRun.getDistance()
              + "\t Returned was "
              + rsp.getDistance()
              + "\t (expected points "
              + oneRun.getLocs()
              + ", was "
              + pointList.getSize()
              + ") "
              + queryList);
    }

    // There are real world instances where A-B-C is identical to A-C (in meter precision).
    if (Math.abs(pointList.getSize() - oneRun.getLocs()) > 1) {
      errors.add(
          algoEntry
              + " returns path not matching the expected points of "
              + oneRun.getLocs()
              + "\t Returned was "
              + pointList.getSize()
              + "\t (expected distance "
              + oneRun.getDistance()
              + ", was "
              + rsp.getDistance()
              + ") "
              + queryList);
    }
    return this;
  }
  void contractNodes() {
    meanDegree = g.getAllEdges().getMaxId() / g.getNodes();
    int level = 1;
    counter = 0;
    int initSize = sortedNodes.getSize();
    int logSize =
        (int) Math.round(Math.max(10, sortedNodes.getSize() / 100 * logMessagesPercentage));
    if (logMessagesPercentage == 0) logSize = Integer.MAX_VALUE;

    // preparation takes longer but queries are slightly faster with preparation
    // => enable it but call not so often
    boolean periodicUpdate = true;
    StopWatch periodSW = new StopWatch();
    int updateCounter = 0;
    int periodicUpdatesCount =
        Math.max(10, sortedNodes.getSize() / 100 * periodicUpdatesPercentage);
    if (periodicUpdatesPercentage == 0) periodicUpdate = false;

    // disable as preparation is slower and query time does not benefit
    int lastNodesLazyUpdates =
        lastNodesLazyUpdatePercentage == 0
            ? 0
            : sortedNodes.getSize() / 100 * lastNodesLazyUpdatePercentage;
    StopWatch lazySW = new StopWatch();

    // Recompute priority of uncontracted neighbors.
    // Without neighborupdates preparation is faster but we need them
    // to slightly improve query time. Also if not applied too often it decreases the shortcut
    // number.
    boolean neighborUpdate = true;
    if (neighborUpdatePercentage == 0) neighborUpdate = false;

    StopWatch neighborSW = new StopWatch();
    LevelGraphStorage lg = ((LevelGraphStorage) g);
    while (!sortedNodes.isEmpty()) {
      // periodically update priorities of ALL nodes
      if (periodicUpdate && counter > 0 && counter % periodicUpdatesCount == 0) {
        periodSW.start();
        sortedNodes.clear();
        int len = g.getNodes();
        for (int node = 0; node < len; node++) {
          if (g.getLevel(node) != 0) continue;

          int priority = oldPriorities[node] = calculatePriority(node);
          sortedNodes.insert(node, priority);
        }
        periodSW.stop();
        updateCounter++;
      }

      if (counter % logSize == 0) {
        // TODO necessary?
        System.gc();
        logger.info(
            Helper.nf(counter)
                + ", updates:"
                + updateCounter
                + ", nodes: "
                + Helper.nf(sortedNodes.getSize())
                + ", shortcuts:"
                + Helper.nf(newShortcuts)
                + ", dijkstras:"
                + Helper.nf(dijkstraCount)
                + ", t(dijk):"
                + (int) dijkstraSW.getSeconds()
                + ", t(period):"
                + (int) periodSW.getSeconds()
                + ", t(lazy):"
                + (int) lazySW.getSeconds()
                + ", t(neighbor):"
                + (int) neighborSW.getSeconds()
                + ", meanDegree:"
                + (long) meanDegree
                + ", algo:"
                + algo.getMemoryUsageAsString()
                + ", "
                + Helper.getMemInfo());
        dijkstraSW = new StopWatch();
        periodSW = new StopWatch();
        lazySW = new StopWatch();
        neighborSW = new StopWatch();
      }

      counter++;
      int polledNode = sortedNodes.pollKey();
      if (sortedNodes.getSize() < lastNodesLazyUpdates) {
        lazySW.start();
        int priority = oldPriorities[polledNode] = calculatePriority(polledNode);
        if (!sortedNodes.isEmpty() && priority > sortedNodes.peekValue()) {
          // current node got more important => insert as new value and contract it later
          sortedNodes.insert(polledNode, priority);
          lazySW.stop();
          continue;
        }
        lazySW.stop();
      }

      // contract!
      newShortcuts += addShortcuts(polledNode);
      g.setLevel(polledNode, level);
      level++;

      EdgeSkipIterator iter = vehicleAllExplorer.setBaseNode(polledNode);
      while (iter.next()) {
        int nn = iter.getAdjNode();
        if (g.getLevel(nn) != 0)
          // already contracted no update necessary
          continue;

        if (neighborUpdate && rand.nextInt(100) < neighborUpdatePercentage) {
          neighborSW.start();
          int oldPrio = oldPriorities[nn];
          int priority = oldPriorities[nn] = calculatePriority(nn);
          if (priority != oldPrio) sortedNodes.update(nn, oldPrio, priority);

          neighborSW.stop();
        }

        if (removesHigher2LowerEdges) lg.disconnect(vehicleAllTmpExplorer, iter);
      }
    }

    // Preparation works only once so we can release temporary data.
    // The preparation object itself has to be intact to create the algorithm.
    close();
    logger.info(
        "took:"
            + (int) allSW.stop().getSeconds()
            + ", new shortcuts: "
            + newShortcuts
            + ", "
            + prepareWeighting
            + ", "
            + prepareEncoder
            + ", removeHigher2LowerEdges:"
            + removesHigher2LowerEdges
            + ", dijkstras:"
            + dijkstraCount
            + ", t(dijk):"
            + (int) dijkstraSW.getSeconds()
            + ", t(period):"
            + (int) periodSW.getSeconds()
            + ", t(lazy):"
            + (int) lazySW.getSeconds()
            + ", t(neighbor):"
            + (int) neighborSW.getSeconds()
            + ", meanDegree:"
            + (long) meanDegree
            + ", initSize:"
            + initSize
            + ", periodic:"
            + periodicUpdatesPercentage
            + ", lazy:"
            + lastNodesLazyUpdatePercentage
            + ", neighbor:"
            + neighborUpdatePercentage);
  }