private Edge getEdge(long fromNodeId, long toNodeId, int distance) {
   Edge edge = null;
   for (Long outEdge : graph.getOutEdges(fromNodeId)) {
     edge = graph.getEdge(outEdge);
     if ((int) edge.getToNode() == toNodeId && edge.getDistance() == distance) {
       break;
     }
   }
   return edge;
 }
  public void expandVertex(
      Node target,
      TimeEntry removed,
      HashMap<Long, Integer> wasTraversed,
      PriorityQueue<TimeEntry> queue,
      HashMap<Long, RouteEntry> parents) {

    Long2IntMap neig = graph.accessNeighborhood(graph.getNode(removed.getId()));

    for (long vid : neig.keySet()) {

      int arrivalTime = graph.getArrival(removed.getArrivalTime(), neig.get(vid));
      int travelTime = removed.getTravelTime() + neig.get(vid);
      TimeEntry newEntry = new TimeEntry(vid, travelTime, arrivalTime, removed.getId());

      Edge edge = null;
      int distance = -1;

      if (!wasTraversed.containsKey(vid)) {

        queue.offer(newEntry);
        wasTraversed.put(newEntry.getId(), newEntry.getTravelTime());

        distance = neig.get(vid);
        edge = getEdge(removed.getId(), vid, distance);
        parents.put(
            vid, new RouteEntry(removed.getId(), distance / 17, edge.getId(), edge.getLabel()));
      } else {

        int cost = wasTraversed.get(vid);

        if (cost != wasRemoved && cost > newEntry.getTravelTime()) {
          queue.remove(newEntry);
          queue.offer(newEntry);
          wasTraversed.remove(newEntry.getId());
          wasTraversed.put(newEntry.getId(), newEntry.getTravelTime());

          parents.remove(vid);
          distance = neig.get(vid);
          edge = getEdge(removed.getId(), vid, distance);
          parents.put(
              vid, new RouteEntry(removed.getId(), distance / 17, edge.getId(), edge.getLabel()));
        }
      }
    }
  }