Repository: graphhopper/graphhopper
Pull request #1341 (2018-04-14 06:28:27)

Refactors bidirectional routing algorithms.
This is a follow up of #1336 and it should be less code to review after this is merged.
The goal of this PR is to reduce some code duplication between bidirectional Dijkstra and AStar algorithms and to make it possible to reuse the code for turn-cost CH as well (#1247).

Both DijkstraBidirectionRef and AStarBidirection now subclass a new class GenericDijkstraBidirection that has a generic parameter for the type of shortest path entries.

I tried to make the refactoring somewhat easy to follow in the single commits, no real logic has changed it mostly moving code around and some renaming, with one exception: In the initFrom/To methods there were some checks for null that were needed because a) it was not enforced in which order these methods were called and b) LandmarkExplorer used these methods to initialize a fwd/bwd-only search. I have disentangled the code in these methods and changed LandmarkExplorer accordingly such that these null-checks should be no longer necessary.


Patch:
diff --git a/core/src/main/java/com/graphhopper/GraphHopper.java b/core/src/main/java/com/graphhopper/GraphHopper.java
index 51f17bd9a..8f4ce86af 100644
--- a/core/src/main/java/com/graphhopper/GraphHopper.java
+++ b/core/src/main/java/com/graphhopper/GraphHopper.java
@@ -1066,6 +1066,7 @@ public class GraphHopper implements GraphHopperAPI {
                         hints(hints).
                         build();
 
+                // do the actual route calculation !
                 altPaths = routingTemplate.calcPaths(queryGraph, tmpAlgoFactory, algoOpts);
 
                 boolean tmpEnableInstructions = hints.getBool(Routing.INSTRUCTIONS, enableInstructions);
@@ -1219,9 +1220,9 @@ public class GraphHopper implements GraphHopperAPI {
         preparation.setMinOneWayNetworkSize(minOneWayNetworkSize);
         preparation.doWork();
         int currNodeCount = ghStorage.getNodes();
-        logger.info("edges: " + ghStorage.getAllEdges().length() + ", nodes " + currNodeCount
-                + ", there were " + preparation.getMaxSubnetworks()
-                + " subnetworks. removed them => " + (prevNodeCount - currNodeCount)
+        logger.info("edges: " + Helper.nf(ghStorage.getAllEdges().length()) + ", nodes " + Helper.nf(currNodeCount)
+                + ", there were " + Helper.nf(preparation.getMaxSubnetworks())
+                + " subnetworks. removed them => " + Helper.nf(prevNodeCount - currNodeCount)
                 + " less nodes");
     }
 
diff --git a/core/src/main/java/com/graphhopper/routing/AStar.java b/core/src/main/java/com/graphhopper/routing/AStar.java
index 63b2de066..21aed2150 100644
--- a/core/src/main/java/com/graphhopper/routing/AStar.java
+++ b/core/src/main/java/com/graphhopper/routing/AStar.java
@@ -24,10 +24,7 @@ import com.graphhopper.routing.weighting.WeightApproximator;
 import com.graphhopper.routing.weighting.Weighting;
 import com.graphhopper.storage.Graph;
 import com.graphhopper.storage.SPTEntry;
-import com.graphhopper.util.EdgeExplorer;
-import com.graphhopper.util.EdgeIterator;
-import com.graphhopper.util.Helper;
-import com.graphhopper.util.Parameters;
+import com.graphhopper.util.*;
 
 import java.util.PriorityQueue;
 
@@ -150,11 +147,6 @@ public class AStar extends AbstractRoutingAlgorithm {
                 setWeight(currEdge.weight).setSPTEntry(currEdge).extract();
     }
 
-    @Override
-    protected SPTEntry createSPTEntry(int node, double weight) {
-        throw new IllegalStateException("use AStarEdge constructor directly");
-    }
-
     @Override
     protected boolean finished() {
         return currEdge.adjNode == to1;
@@ -165,6 +157,9 @@ public class AStar extends AbstractRoutingAlgorithm {
         return visitedCount;
     }
 
+    protected void updateBestPath(EdgeIteratorState edgeState, SPTEntry bestSPTEntry, int traversalId) {
+    }
+
     public static class AStarEntry extends SPTEntry {
         double weightOfVisitedPath;
 
@@ -177,6 +172,11 @@ public class AStar extends AbstractRoutingAlgorithm {
         public final double getWeightOfVisitedPath() {
             return weightOfVisitedPath;
         }
+
+        @Override
+        public AStarEntry getParent() {
+            return (AStarEntry) parent;
+        }
     }
 
     @Override
diff --git a/core/src/main/java/com/graphhopper/routing/AStarBidirection.java b/core/src/main/java/com/graphhopper/routing/AStarBidirection.java
index b9a2eebce..b01d70f4a 100644
--- a/core/src/main/java/com/graphhopper/routing/AStarBidirection.java
+++ b/core/src/main/java/com/graphhopper/routing/AStarBidirection.java
@@ -1,14 +1,14 @@
 /*
  *  Licensed to GraphHopper GmbH under one or more contributor
- *  license agreements. See the NOTICE file distributed with this work for 
+ *  license agreements. See the NOTICE file distributed with this work for
  *  additional information regarding copyright ownership.
- * 
- *  GraphHopper GmbH licenses this file to you under the Apache License, 
- *  Version 2.0 (the "License"); you may not use this file except in 
+ *
+ *  GraphHopper GmbH licenses this file to you under the Apache License,
+ *  Version 2.0 (the "License"); you may not use this file except in
  *  compliance with the License. You may obtain a copy of the License at
- * 
+ *
  *       http://www.apache.org/licenses/LICENSE-2.0
- * 
+ *
  *  Unless required by applicable law or agreed to in writing, software
  *  distributed under the License is distributed on an "AS IS" BASIS,
  *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
@@ -17,10 +17,6 @@
  */
 package com.graphhopper.routing;
 
-import com.carrotsearch.hppc.IntHashSet;
-import com.carrotsearch.hppc.IntObjectMap;
-import com.carrotsearch.hppc.predicates.IntObjectPredicate;
-import com.graphhopper.coll.GHIntObjectHashMap;
 import com.graphhopper.routing.AStar.AStarEntry;
 import com.graphhopper.routing.util.TraversalMode;
 import com.graphhopper.routing.weighting.BeelineWeightApproximator;
@@ -29,11 +25,10 @@ import com.graphhopper.routing.weighting.WeightApproximator;
 import com.graphhopper.routing.weighting.Weighting;
 import com.graphhopper.storage.Graph;
 import com.graphhopper.storage.SPTEntry;
-import com.graphhopper.util.*;
-
-import java.util.ArrayList;
-import java.util.List;
-import java.util.PriorityQueue;
+import com.graphhopper.util.EdgeIterator;
+import com.graphhopper.util.EdgeIteratorState;
+import com.graphhopper.util.Helper;
+import com.graphhopper.util.Parameters;
 
 /**
  * This class implements a bidirectional A* algorithm. It is interesting to note that a
@@ -62,271 +57,72 @@ import java.util.PriorityQueue;
  * @author jansoe
  */
 public class AStarBidirection extends AbstractBidirAlgo implements RecalculationHook {
-    protected AStarEntry currFrom;
-    protected AStarEntry currTo;
-    protected PathBidirRef bestPath;
-    protected IntObjectMap<AStarEntry> bestWeightMapFrom;
-    protected IntObjectMap<AStarEntry> bestWeightMapTo;
-    private IntObjectMap<AStarEntry> bestWeightMapOther;
     private ConsistentWeightApproximator weightApprox;
-    private PriorityQueue<AStarEntry> pqOpenSetFrom;
-    private PriorityQueue<AStarEntry> pqOpenSetTo;
-    private IntHashSet ignoreExplorationFrom = new IntHashSet();
-    private IntHashSet ignoreExplorationTo = new IntHashSet();
-    private boolean updateBestPath = true;
 
     public AStarBidirection(Graph graph, Weighting weighting, TraversalMode tMode) {
         super(graph, weighting, tMode);
-        int size = Math.min(Math.max(200, graph.getNodes() / 10), 150_000);
-        initCollections(size);
         BeelineWeightApproximator defaultApprox = new BeelineWeightApproximator(nodeAccess, weighting);
         defaultApprox.setDistanceCalc(Helper.DIST_PLANE);
         setApproximation(defaultApprox);
     }
 
-    protected void initCollections(int size) {
-        pqOpenSetFrom = new PriorityQueue<AStarEntry>(size);
-        bestWeightMapFrom = new GHIntObjectHashMap<AStarEntry>(size);
-
-        pqOpenSetTo = new PriorityQueue<AStarEntry>(size);
-        bestWeightMapTo = new GHIntObjectHashMap<AStarEntry>(size);
-    }
-
-    /**
-     * @param approx if true it enables approximate distance calculation from lat,lon values
-     */
-    public AStarBidirection setApproximation(WeightApproximator approx) {
-        weightApprox = new ConsistentWeightApproximator(approx);
-        return this;
-    }
-
-    public WeightApproximator getApproximation() {
-        return weightApprox.getApproximation();
-    }
-
     @Override
-    protected SPTEntry createSPTEntry(int node, double weight) {
-        throw new IllegalStateException("use AStarEdge constructor directly");
-    }
-
-    @Override
-    public void initFrom(int from, double weight) {
-        currFrom = new AStarEntry(EdgeIterator.NO_EDGE, from, weight, weight);
+    void init(int from, double fromWeight, int to, double toWeight) {
         weightApprox.setFrom(from);
-        pqOpenSetFrom.add(currFrom);
-
-        if (currTo != null) {
-            currFrom.weight += weightApprox.approximate(currFrom.adjNode, false);
-            currTo.weight += weightApprox.approximate(currTo.adjNode, true);
-        }
-
-        if (!traversalMode.isEdgeBased()) {
-            bestWeightMapFrom.put(from, currFrom);
-            if (currTo != null) {
-                bestWeightMapOther = bestWeightMapTo;
-                updateBestPath(GHUtility.getEdge(graph, from, currTo.adjNode), currTo, from);
-            }
-        } else if (currTo != null && currTo.adjNode == from) {
-            // special case of identical start and end
-            bestPath.sptEntry = currFrom;
-            bestPath.edgeTo = currTo;
-            finishedFrom = true;
-            finishedTo = true;
-        }
-    }
-
-    @Override
-    public void initTo(int to, double weight) {
-        currTo = new AStarEntry(EdgeIterator.NO_EDGE, to, weight, weight);
         weightApprox.setTo(to);
-        pqOpenSetTo.add(currTo);
-
-        if (currFrom != null) {
-            currFrom.weight += weightApprox.approximate(currFrom.adjNode, false);
-            currTo.weight += weightApprox.approximate(currTo.adjNode, true);
-        }
-
-        if (!traversalMode.isEdgeBased()) {
-            bestWeightMapTo.put(to, currTo);
-            if (currFrom != null) {
-                bestWeightMapOther = bestWeightMapFrom;
-                updateBestPath(GHUtility.getEdge(graph, currFrom.adjNode, to), currFrom, to);
-            }
-        } else if (currFrom != null && currFrom.adjNode == to) {
-            // special case of identical start and end
-            bestPath.sptEntry = currFrom;
-            bestPath.edgeTo = currTo;
-            finishedFrom = true;
-            finishedTo = true;
-        }
-    }
-
-    @Override
-    protected Path createAndInitPath() {
-        bestPath = new PathBidirRef(graph, weighting);
-        return bestPath;
+        super.init(from, fromWeight, to, toWeight);
     }
 
     @Override
-    protected Path extractPath() {
-        if (finished())
-            return bestPath.extract();
-
-        return bestPath;
-    }
-
-    @Override
-    protected double getCurrentFromWeight() {
-        return currFrom.weight;
-    }
-
-    @Override
-    protected double getCurrentToWeight() {
-        return currTo.weight;
+    protected AStarEntry createStartEntry(int node, double weight, boolean reverse) {
+        double heapWeight = weight + weightApprox.approximate(node, reverse);
+        return new AStarEntry(EdgeIterator.NO_EDGE, node, heapWeight, weight);
     }
 
     @Override
-    protected boolean finished() {
-        if (finishedFrom || finishedTo)
-            return true;
-
-        // using 'weight' is important and correct here e.g. approximation can get negative and smaller than 'weightOfVisitedPath'
-        return currFrom.weight + currTo.weight >= bestPath.getWeight();
+    protected SPTEntry createEntry(EdgeIteratorState edge, double weight, SPTEntry parent, boolean reverse) {
+        int neighborNode = edge.getAdjNode();
+        double heapWeight = weight + weightApprox.approximate(neighborNode, reverse);
+        AStarEntry entry = new AStarEntry(edge.getEdge(), neighborNode, heapWeight, weight);
+        entry.parent = parent;
+        return entry;
     }
 
     @Override
-    boolean fillEdgesFrom() {
-        if (pqOpenSetFrom.isEmpty())
-            return false;
-
-        currFrom = pqOpenSetFrom.poll();
-        bestWeightMapOther = bestWeightMapTo;
-        fillEdges(currFrom, pqOpenSetFrom, bestWeightMapFrom, ignoreExplorationFrom, outEdgeExplorer, false);
-        visitedCountFrom++;
-        return true;
+    protected void updateEntry(SPTEntry entry, EdgeIteratorState edge, double weight, SPTEntry parent, boolean reverse) {
+        entry.edge = edge.getEdge();
+        entry.weight = weight + weightApprox.approximate(edge.getAdjNode(), reverse);
+        ((AStarEntry) entry).weightOfVisitedPath = weight;
+        entry.parent = parent;
     }
 
     @Override
-    boolean fillEdgesTo() {
-        if (pqOpenSetTo.isEmpty())
-            return false;
-
-        currTo = pqOpenSetTo.poll();
-        bestWeightMapOther = bestWeightMapFrom;
-        fillEdges(currTo, pqOpenSetTo, bestWeightMapTo, ignoreExplorationTo, inEdgeExplorer, true);
-        visitedCountTo++;
-        return true;
-    }
-
-    private void fillEdges(AStarEntry currEdge, PriorityQueue<AStarEntry> prioQueueOpenSet,
-                           IntObjectMap<AStarEntry> bestWeightMap, IntHashSet ignoreExploration,
-                           EdgeExplorer explorer, boolean reverse) {
-
-        int currNode = currEdge.adjNode;
-        EdgeIterator iter = explorer.setBaseNode(currNode);
-        while (iter.next()) {
-            if (!accept(iter, currEdge.edge))
-                continue;
-
-            int neighborNode = iter.getAdjNode();
-            int traversalId = traversalMode.createTraversalId(iter, reverse);
-            if (ignoreExploration.contains(traversalId))
-                continue;
-
-            // TODO performance: check if the node is already existent in the opposite direction
-            // then we could avoid the approximation as we already know the exact complete path!
-            double alreadyVisitedWeight = weighting.calcWeight(iter, reverse, currEdge.edge)
-                    + currEdge.getWeightOfVisitedPath();
-            if (Double.isInfinite(alreadyVisitedWeight))
-                continue;
-
-            AStarEntry ase = bestWeightMap.get(traversalId);
-            if (ase == null || ase.getWeightOfVisitedPath() > alreadyVisitedWeight) {
-                double currWeightToGoal = weightApprox.approximate(neighborNode, reverse);
-                double estimationFullWeight = alreadyVisitedWeight + currWeightToGoal;
-                if (ase == null) {
-                    ase = new AStarEntry(iter.getEdge(), neighborNode, estimationFullWeight, alreadyVisitedWeight);
-                    bestWeightMap.put(traversalId, ase);
-                } else {
-//                    assert (ase.weight > 0.999999 * estimationFullWeight) : "Inconsistent distance estimate "
-//                            + ase.weight + " vs " + estimationFullWeight + " (" + ase.weight / estimationFullWeight + "), and:"
-//                            + ase.getWeightOfVisitedPath() + " vs " + alreadyVisitedWeight + " (" + ase.getWeightOfVisitedPath() / alreadyVisitedWeight + ")";
-                    prioQueueOpenSet.remove(ase);
-                    ase.edge = iter.getEdge();
-                    ase.weight = estimationFullWeight;
-                    ase.weightOfVisitedPath = alreadyVisitedWeight;
-                }
-
-                ase.parent = currEdge;
-                prioQueueOpenSet.add(ase);
-
-                if (updateBestPath)
-                    updateBestPath(iter, ase, traversalId);
-            }
-        }
-    }
-
-    public void updateBestPath(EdgeIteratorState edgeState, AStarEntry entryCurrent, int currLoc) {
-        AStarEntry entryOther = bestWeightMapOther.get(currLoc);
-        if (entryOther == null)
-            return;
-
-        boolean reverse = bestWeightMapFrom == bestWeightMapOther;
-        // update μ
-        double newWeight = entryCurrent.weightOfVisitedPath + entryOther.weightOfVisitedPath;
-        if (traversalMode.isEdgeBased()) {
-            if (entryOther.edge != entryCurrent.edge)
-                throw new IllegalStateException("cannot happen for edge based execution of " + getName());
-
-            // see DijkstraBidirectionRef
-            if (entryOther.adjNode != entryCurrent.adjNode) {
-                entryCurrent = (AStar.AStarEntry) entryCurrent.parent;
-                newWeight -= weighting.calcWeight(edgeState, reverse, EdgeIterator.NO_EDGE);
-            } else if (!traversalMode.hasUTurnSupport())
-                // we detected a u-turn at meeting point, skip if not supported
-                return;
-        }
-
-        if (newWeight < bestPath.getWeight()) {
-            bestPath.setSwitchToFrom(reverse);
-            bestPath.sptEntry = entryCurrent;
-            bestPath.edgeTo = entryOther;
-            bestPath.setWeight(newWeight);
-        }
+    protected double calcWeight(EdgeIteratorState iter, SPTEntry currEdge, boolean reverse) {
+        // TODO performance: check if the node is already existent in the opposite direction
+        // then we could avoid the approximation as we already know the exact complete path!
+        return super.calcWeight(iter, currEdge, reverse);
     }
 
-    IntObjectMap<AStarEntry> getBestFromMap() {
-        return bestWeightMapFrom;
-    }
-
-    IntObjectMap<AStarEntry> getBestToMap() {
-        return bestWeightMapTo;
+    public WeightApproximator getApproximation() {
+        return weightApprox.getApproximation();
     }
 
-    void setBestOtherMap(IntObjectMap<AStarEntry> other) {
-        bestWeightMapOther = other;
+    /**
+     * @param approx if true it enables approximate distance calculation from lat,lon values
+     */
+    public AStarBidirection setApproximation(WeightApproximator approx) {
+        weightApprox = new ConsistentWeightApproximator(approx);
+        return this;
     }
 
     void setFromDataStructures(AStarBidirection astar) {
-        pqOpenSetFrom = astar.pqOpenSetFrom;
-        bestWeightMapFrom = astar.bestWeightMapFrom;
-        finishedFrom = astar.finishedFrom;
-        currFrom = astar.currFrom;
-        visitedCountFrom = astar.visitedCountFrom;
-        ignoreExplorationFrom = astar.ignoreExplorationFrom;
+        super.setFromDataStructures(astar);
         weightApprox.setFrom(astar.currFrom.adjNode);
-        // outEdgeExplorer
     }
 
     void setToDataStructures(AStarBidirection astar) {
-        pqOpenSetTo = astar.pqOpenSetTo;
-        bestWeightMapTo = astar.bestWeightMapTo;
-        finishedTo = astar.finishedTo;
-        currTo = astar.currTo;
-        visitedCountTo = astar.visitedCountTo;
-        ignoreExplorationTo = astar.ignoreExplorationTo;
+        super.setToDataStructures(astar);
         weightApprox.setTo(astar.currTo.adjNode);
-        // inEdgeExplorer
     }
 
     @Override
diff --git a/core/src/main/java/com/graphhopper/routing/AStarBidirectionCH.java b/core/src/main/java/com/graphhopper/routing/AStarBidirectionCH.java
index 7dd961447..28f257d6a 100644
--- a/core/src/main/java/com/graphhopper/routing/AStarBidirectionCH.java
+++ b/core/src/main/java/com/graphhopper/routing/AStarBidirectionCH.java
@@ -25,6 +25,10 @@ import com.graphhopper.storage.Graph;
 public class AStarBidirectionCH extends AStarBidirection {
     public AStarBidirectionCH(Graph graph, Weighting weighting, TraversalMode traversalMode) {
         super(graph, weighting, traversalMode);
+        if (traversalMode.isEdgeBased()) {
+            throw new IllegalArgumentException("Edge based traversal is not supported" +
+                    "for CH");
+        }
     }
 
     @Override
diff --git a/core/src/main/java/com/graphhopper/routing/AbstractBidirAlgo.java b/core/src/main/java/com/graphhopper/routing/AbstractBidirAlgo.java
index 771b9b85f..e660c7244 100644
--- a/core/src/main/java/com/graphhopper/routing/AbstractBidirAlgo.java
+++ b/core/src/main/java/com/graphhopper/routing/AbstractBidirAlgo.java
@@ -1,14 +1,14 @@
 /*
  *  Licensed to GraphHopper GmbH under one or more contributor
- *  license agreements. See the NOTICE file distributed with this work for 
+ *  license agreements. See the NOTICE file distributed with this work for
  *  additional information regarding copyright ownership.
- * 
- *  GraphHopper GmbH licenses this file to you under the Apache License, 
- *  Version 2.0 (the "License"); you may not use this file except in 
+ *
+ *  GraphHopper GmbH licenses this file to you under the Apache License,
+ *  Version 2.0 (the "License"); you may not use this file except in
  *  compliance with the License. You may obtain a copy of the License at
- * 
+ *
  *       http://www.apache.org/licenses/LICENSE-2.0
- * 
+ *
  *  Unless required by applicable law or agreed to in writing, software
  *  distributed under the License is distributed on an "AS IS" BASIS,
  *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
@@ -17,9 +17,18 @@
  */
 package com.graphhopper.routing;
 
+import com.carrotsearch.hppc.IntObjectMap;
+import com.graphhopper.coll.GHIntObjectHashMap;
 import com.graphhopper.routing.util.TraversalMode;
 import com.graphhopper.routing.weighting.Weighting;
 import com.graphhopper.storage.Graph;
+import com.graphhopper.storage.SPTEntry;
+import com.graphhopper.util.EdgeExplorer;
+import com.graphhopper.util.EdgeIterator;
+import com.graphhopper.util.EdgeIteratorState;
+import com.graphhopper.util.GHUtility;
+
+import java.util.PriorityQueue;
 
 /**
  * Common subclass for bidirectional algorithms.
@@ -28,6 +37,15 @@ import com.graphhopper.storage.Graph;
  * @author Peter Karich
  */
 public abstract class AbstractBidirAlgo extends AbstractRoutingAlgorithm {
+    protected IntObjectMap<SPTEntry> bestWeightMapFrom;
+    protected IntObjectMap<SPTEntry> bestWeightMapTo;
+    protected IntObjectMap<SPTEntry> bestWeightMapOther;
+    protected SPTEntry currFrom;
+    protected SPTEntry currTo;
+    protected PathBidirRef bestPath;
+    PriorityQueue<SPTEntry> pqOpenSetFrom;
+    PriorityQueue<SPTEntry> pqOpenSetTo;
+    private boolean updateBestPath = true;
     protected boolean finishedFrom;
     protected boolean finishedTo;
     int visitedCountFrom;
@@ -35,32 +53,73 @@ public abstract class AbstractBidirAlgo extends AbstractRoutingAlgorithm {
 
     public AbstractBidirAlgo(Graph graph, Weighting weighting, TraversalMode tMode) {
         super(graph, weighting, tMode);
+        int size = Math.min(Math.max(200, graph.getNodes() / 10), 150_000);
+        initCollections(size);
     }
 
-    abstract void initFrom(int from, double dist);
-
-    abstract void initTo(int to, double dist);
-
-    protected abstract Path createAndInitPath();
+    protected void initCollections(int size) {
+        pqOpenSetFrom = new PriorityQueue<>(size);
+        bestWeightMapFrom = new GHIntObjectHashMap<>(size);
 
-    protected abstract double getCurrentFromWeight();
-
-    protected abstract double getCurrentToWeight();
+        pqOpenSetTo = new PriorityQueue<>(size);
+        bestWeightMapTo = new GHIntObjectHashMap<>(size);
+    }
 
-    abstract boolean fillEdgesFrom();
+    protected abstract SPTEntry createStartEntry(int node, double weight, boolean reverse);
 
-    abstract boolean fillEdgesTo();
+    protected abstract SPTEntry createEntry(EdgeIteratorState edge, double weight, SPTEntry parent, boolean reverse);
 
     @Override
     public Path calcPath(int from, int to) {
         checkAlreadyRun();
         createAndInitPath();
-        initFrom(from, 0);
-        initTo(to, 0);
+        init(from, 0, to, 0);
         runAlgo();
         return extractPath();
     }
 
+    protected Path createAndInitPath() {
+        bestPath = new PathBidirRef(graph, weighting);
+        return bestPath;
+    }
+
+    void init(int from, double fromWeight, int to, double toWeight) {
+        initFrom(from, fromWeight);
+        initTo(to, toWeight);
+        postInit(from, to);
+    }
+
+    protected void initFrom(int from, double weight) {
+        currFrom = createStartEntry(from, weight, false);
+        pqOpenSetFrom.add(currFrom);
+        if (!traversalMode.isEdgeBased()) {
+            bestWeightMapFrom.put(from, currFrom);
+        }
+    }
+
+    protected void initTo(int to, double weight) {
+        currTo = createStartEntry(to, weight, true);
+        pqOpenSetTo.add(currTo);
+        if (!traversalMode.isEdgeBased()) {
+            bestWeightMapTo.put(to, currTo);
+        }
+    }
+
+    protected void postInit(int from, int to) {
+        if (!traversalMode.isEdgeBased()) {
+            if (updateBestPath) {
+                bestWeightMapOther = bestWeightMapFrom;
+                updateBestPath(GHUtility.getEdge(graph, currFrom.adjNode, to), currFrom, to, true);
+            }
+        } else if (from == to) {
+            // special case of identical start and end
+            bestPath.sptEntry = currFrom;
+            bestPath.edgeTo = currTo;
+            finishedFrom = true;
+            finishedTo = true;
+        }
+    }
+
     protected void runAlgo() {
         while (!finished() && !isMaxVisitedNodesExceeded()) {
             if (!finishedFrom)
@@ -71,8 +130,203 @@ public abstract class AbstractBidirAlgo extends AbstractRoutingAlgorithm {
         }
     }
 
+    // http://www.cs.princeton.edu/courses/archive/spr06/cos423/Handouts/EPP%20shortest%20path%20algorithms.pdf
+    // a node from overlap may not be on the best path!
+    // => when scanning an arc (v, w) in the forward search and w is scanned in the reverseOrder
+    //    search, update extractPath = μ if df (v) + (v, w) + dr (w) < μ
+    @Override
+    protected boolean finished() {
+        if (finishedFrom || finishedTo)
+            return true;
+
+        return currFrom.weight + currTo.weight >= bestPath.getWeight();
+    }
+
+    boolean fillEdgesFrom() {
+        if (pqOpenSetFrom.isEmpty()) {
+            return false;
+        }
+        currFrom = pqOpenSetFrom.poll();
+        visitedCountFrom++;
+        if (fromEntryCanBeSkipped()) {
+            return true;
+        }
+        if (fwdSearchCanBeStopped()) {
+            return false;
+        }
+        bestWeightMapOther = bestWeightMapTo;
+        fillEdges(currFrom, pqOpenSetFrom, bestWeightMapFrom, outEdgeExplorer, false);
+        return true;
+    }
+
+    boolean fillEdgesTo() {
+        if (pqOpenSetTo.isEmpty()) {
+            return false;
+        }
+        currTo = pqOpenSetTo.poll();
+        visitedCountTo++;
+        if (toEntryCanBeSkipped()) {
+            return true;
+        }
+        if (bwdSearchCanBeStopped()) {
+            return false;
+        }
+        bestWeightMapOther = bestWeightMapFrom;
+        fillEdges(currTo, pqOpenSetTo, bestWeightMapTo, inEdgeExplorer, true);
+        return true;
+    }
+
+    private void fillEdges(SPTEntry currEdge, PriorityQueue<SPTEntry> prioQueue,
+                           IntObjectMap<SPTEntry> bestWeightMap, EdgeExplorer explorer, boolean reverse) {
+        EdgeIterator iter = explorer.setBaseNode(currEdge.adjNode);
+        while (iter.next()) {
+            if (!accept(iter, currEdge, reverse))
+                continue;
+
+            final int origEdgeId = getOrigEdgeId(iter, reverse);
+            final int traversalId = getTraversalId(iter, origEdgeId, reverse);
+            final double weight = calcWeight(iter, currEdge, reverse);
+            if (Double.isInfinite(weight))
+                continue;
+            SPTEntry entry = bestWeightMap.get(traversalId);
+            if (entry == null) {
+                entry = createEntry(iter, weight, currEdge, reverse);
+                bestWeightMap.put(traversalId, entry);
+                prioQueue.add(entry);
+            } else if (entry.getWeightOfVisitedPath() > weight) {
+                prioQueue.remove(entry);
+                updateEntry(entry, iter, weight, currEdge, reverse);
+                prioQueue.add(entry);
+            } else
+                continue;
+
+            if (updateBestPath)
+                updateBestPath(iter, entry, traversalId, reverse);
+        }
+    }
+
+    protected void updateBestPath(EdgeIteratorState edgeState, SPTEntry entry, int traversalId, boolean reverse) {
+        SPTEntry entryOther = bestWeightMapOther.get(traversalId);
+        if (entryOther == null)
+            return;
+
+        // update μ
+        double weight = entry.getWeightOfVisitedPath() + entryOther.getWeightOfVisitedPath();
+        if (traversalMode.isEdgeBased()) {
+            if (entryOther.edge != entry.edge)
+                throw new IllegalStateException("cannot happen for edge based execution of " + getName());
+
+            if (entryOther.adjNode != entry.adjNode) {
+                // prevents the path to contain the edge at the meeting point twice and subtracts the weight (excluding turn weight => no previous edge)
+                entry = entry.getParent();
+                weight -= weighting.calcWeight(edgeState, reverse, EdgeIterator.NO_EDGE);
+            } else if (!traversalMode.hasUTurnSupport())
+                // we detected a u-turn at meeting point, skip if not supported
+                return;
+        }
+
+        if (weight < bestPath.getWeight()) {
+            bestPath.setSwitchToFrom(reverse);
+            bestPath.setSPTEntry(entry);
+            bestPath.setSPTEntryTo(entryOther);
+            bestPath.setWeight(weight);
+        }
+    }
+
+    protected void updateEntry(SPTEntry entry, EdgeIteratorState edge, double weight, SPTEntry parent, boolean reverse) {
+        entry.edge = edge.getEdge();
+        entry.weight = weight;
+        entry.parent = parent;
+    }
+
+    protected boolean accept(EdgeIteratorState edge, SPTEntry currEdge, boolean reverse) {
+        return accept(edge, currEdge.edge);
+    }
+
+    protected int getOrigEdgeId(EdgeIteratorState edge, boolean reverse) {
+        return edge.getEdge();
+    }
+
+    protected int getTraversalId(EdgeIteratorState edge, int origEdgeId, boolean reverse) {
+        return traversalMode.createTraversalId(edge, reverse);
+    }
+
+    protected double calcWeight(EdgeIteratorState iter, SPTEntry currEdge, boolean reverse) {
+        return weighting.calcWeight(iter, reverse, currEdge.edge) + currEdge.getWeightOfVisitedPath();
+    }
+
+    @Override
+    protected Path extractPath() {
+        if (finished())
+            return bestPath.extract();
+
+        return bestPath;
+    }
+
+    protected boolean fromEntryCanBeSkipped() {
+        return false;
+    }
+
+    protected boolean fwdSearchCanBeStopped() {
+        return false;
+    }
+
+    protected boolean toEntryCanBeSkipped() {
+        return false;
+    }
+
+    protected boolean bwdSearchCanBeStopped() {
+        return false;
+    }
+
+    protected double getCurrentFromWeight() {
+        return currFrom.weight;
+    }
+
+    protected double getCurrentToWeight() {
+        return currTo.weight;
+    }
+
+    IntObjectMap<SPTEntry> getBestFromMap() {
+        return bestWeightMapFrom;
+    }
+
+    IntObjectMap<SPTEntry> getBestToMap() {
+        return bestWeightMapTo;
+    }
+
+    void setBestOtherMap(IntObjectMap<SPTEntry> other) {
+        bestWeightMapOther = other;
+    }
+
+    protected void setUpdateBestPath(boolean b) {
+        updateBestPath = b;
+    }
+
+    void setBestPath(PathBidirRef bestPath) {
+        this.bestPath = bestPath;
+    }
+
     @Override
     public int getVisitedNodes() {
         return visitedCountFrom + visitedCountTo;
     }
+
+    void setFromDataStructures(AbstractBidirAlgo other) {
+        pqOpenSetFrom = other.pqOpenSetFrom;
+        bestWeightMapFrom = other.bestWeightMapFrom;
+        finishedFrom = other.finishedFrom;
+        currFrom = other.currFrom;
+        visitedCountFrom = other.visitedCountFrom;
+        // outEdgeExplorer
+    }
+
+    void setToDataStructures(AbstractBidirAlgo other) {
+        pqOpenSetTo = other.pqOpenSetTo;
+        bestWeightMapTo = other.bestWeightMapTo;
+        finishedTo = other.finishedTo;
+        currTo = other.currTo;
+        visitedCountTo = other.visitedCountTo;
+        // inEdgeExplorer
+    }
 }
diff --git a/core/src/main/java/com/graphhopper/routing/AbstractRoutingAlgorithm.java b/core/src/main/java/com/graphhopper/routing/AbstractRoutingAlgorithm.java
index 490b8812c..bac753898 100644
--- a/core/src/main/java/com/graphhopper/routing/AbstractRoutingAlgorithm.java
+++ b/core/src/main/java/com/graphhopper/routing/AbstractRoutingAlgorithm.java
@@ -26,7 +26,6 @@ import com.graphhopper.storage.Graph;
 import com.graphhopper.storage.NodeAccess;
 import com.graphhopper.storage.SPTEntry;
 import com.graphhopper.util.EdgeExplorer;
-import com.graphhopper.util.EdgeIterator;
 import com.graphhopper.util.EdgeIteratorState;
 
 import java.util.Collections;
@@ -44,7 +43,7 @@ public abstract class AbstractRoutingAlgorithm implements RoutingAlgorithm {
     protected EdgeExplorer inEdgeExplorer;
     protected EdgeExplorer outEdgeExplorer;
     protected int maxVisitedNodes = Integer.MAX_VALUE;
-    private EdgeFilter additionalEdgeFilter;
+    protected EdgeFilter additionalEdgeFilter;
     private boolean alreadyRun;
 
     /**
@@ -72,16 +71,13 @@ public abstract class AbstractRoutingAlgorithm implements RoutingAlgorithm {
         return this;
     }
 
-    protected boolean accept(EdgeIterator iter, int prevOrNextEdgeId) {
+    protected boolean accept(EdgeIteratorState iter, int prevOrNextEdgeId) {
         if (!traversalMode.hasUTurnSupport() && iter.getEdge() == prevOrNextEdgeId)
             return false;
 
         return additionalEdgeFilter == null || additionalEdgeFilter.accept(iter);
     }
 
-    protected void updateBestPath(EdgeIteratorState edgeState, SPTEntry bestSPTEntry, int traversalId) {
-    }
-
     protected void checkAlreadyRun() {
         if (alreadyRun)
             throw new IllegalStateException("Create a new instance per call");
@@ -89,10 +85,6 @@ public abstract class AbstractRoutingAlgorithm implements RoutingAlgorithm {
         alreadyRun = true;
     }
 
-    protected SPTEntry createSPTEntry(int node, double weight) {
-        return new SPTEntry(EdgeIterator.NO_EDGE, node, weight);
-    }
-
     /**
      * To be overwritten from extending class. Should we make this available in RoutingAlgorithm
      * interface?
diff --git a/core/src/main/java/com/graphhopper/routing/AlgorithmOptions.java b/core/src/main/java/com/graphhopper/routing/AlgorithmOptions.java
index 8412043c2..f30b86083 100644
--- a/core/src/main/java/com/graphhopper/routing/AlgorithmOptions.java
+++ b/core/src/main/java/com/graphhopper/routing/AlgorithmOptions.java
@@ -142,7 +142,7 @@ public class AlgorithmOptions {
         }
 
         /**
-         * For possible values see Parameters.Algorithms
+         * For possible values see {@link Parameters.Algorithms}
          */
         public Builder algorithm(String algorithm) {
             this.opts.algorithm = algorithm;
diff --git a/core/src/main/java/com/graphhopper/routing/AlternativeRoute.java b/core/src/main/java/com/graphhopper/routing/AlternativeRoute.java
index 404eb7e1e..85f0f617c 100644
--- a/core/src/main/java/com/graphhopper/routing/AlternativeRoute.java
+++ b/core/src/main/java/com/graphhopper/routing/AlternativeRoute.java
@@ -265,14 +265,6 @@ public class AlternativeRoute implements RoutingAlgorithm {
             this.explorationFactor = explorationFactor;
         }
 
-        public IntObjectMap<AStarEntry> getBestWeightMapFrom() {
-            return bestWeightMapFrom;
-        }
-
-        public IntObjectMap<AStarEntry> getBestWeightMapTo() {
-            return bestWeightMapTo;
-        }
-
         @Override
         public boolean finished() {
             // we need to finish BOTH searches identical to CH
@@ -293,10 +285,9 @@ public class AlternativeRoute implements RoutingAlgorithm {
             // For bidir A* and AStarEdge.getWeightOfVisitedPath see comment in AStarBidirection.finished
         }
 
-        public Path searchBest(int to, int from) {
+        public Path searchBest(int from, int to) {
             createAndInitPath();
-            initFrom(to, 0);
-            initTo(from, 0);
+            init(from, 0, to, 0);
             // init collections and bestPath.getWeight properly
             runAlgo();
             return extractPath();
diff --git a/core/src/main/java/com/graphhopper/routing/Dijkstra.java b/core/src/main/java/com/graphhopper/routing/Dijkstra.java
index b57650f9e..0371ca67e 100644
--- a/core/src/main/java/com/graphhopper/routing/Dijkstra.java
+++ b/core/src/main/java/com/graphhopper/routing/Dijkstra.java
@@ -25,6 +25,7 @@ import com.graphhopper.storage.Graph;
 import com.graphhopper.storage.SPTEntry;
 import com.graphhopper.util.EdgeExplorer;
 import com.graphhopper.util.EdgeIterator;
+import com.graphhopper.util.EdgeIteratorState;
 import com.graphhopper.util.Parameters;
 
 import java.util.PriorityQueue;
@@ -50,15 +51,15 @@ public class Dijkstra extends AbstractRoutingAlgorithm {
     }
 
     protected void initCollections(int size) {
-        fromHeap = new PriorityQueue<SPTEntry>(size);
-        fromMap = new GHIntObjectHashMap<SPTEntry>(size);
+        fromHeap = new PriorityQueue<>(size);
+        fromMap = new GHIntObjectHashMap<>(size);
     }
 
     @Override
     public Path calcPath(int from, int to) {
         checkAlreadyRun();
         this.to = to;
-        currEdge = createSPTEntry(from, 0);
+        currEdge = new SPTEntry(from, 0);
         if (!traversalMode.isEdgeBased()) {
             fromMap.put(from, currEdge);
         }
@@ -130,6 +131,9 @@ public class Dijkstra extends AbstractRoutingAlgorithm {
         return visitedNodes;
     }
 
+    protected void updateBestPath(EdgeIteratorState edgeState, SPTEntry bestSPTEntry, int traversalId) {
+    }
+
     @Override
     public String getName() {
         return Parameters.Algorithms.DIJKSTRA;
diff --git a/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionCH.java b/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionCH.java
index ffe70dfc4..fee0b22e1 100644
--- a/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionCH.java
+++ b/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionCH.java
@@ -38,33 +38,13 @@ public class DijkstraBidirectionCH extends DijkstraBidirectionCHNoSOD {
     }
 
     @Override
-    public boolean fillEdgesFrom() {
-        if (pqOpenSetFrom.isEmpty()) {
-            return false;
-        }
-        currFrom = pqOpenSetFrom.poll();
-        visitedCountFrom++;
-        if (entryIsStallable(currFrom, bestWeightMapFrom, inEdgeExplorer, false)) {
-            return true;
-        }
-        bestWeightMapOther = bestWeightMapTo;
-        fillEdges(currFrom, pqOpenSetFrom, bestWeightMapFrom, outEdgeExplorer, false);
-        return true;
+    protected boolean fromEntryCanBeSkipped() {
+        return entryIsStallable(currFrom, bestWeightMapFrom, inEdgeExplorer, false);
     }
 
     @Override
-    public boolean fillEdgesTo() {
-        if (pqOpenSetTo.isEmpty()) {
-            return false;
-        }
-        currTo = pqOpenSetTo.poll();
-        visitedCountTo++;
-        if (entryIsStallable(currTo, bestWeightMapTo, outEdgeExplorer, true)) {
-            return true;
-        }
-        bestWeightMapOther = bestWeightMapFrom;
-        fillEdges(currTo, pqOpenSetTo, bestWeightMapTo, inEdgeExplorer, true);
-        return true;
+    protected boolean toEntryCanBeSkipped() {
+        return entryIsStallable(currTo, bestWeightMapTo, outEdgeExplorer, true);
     }
 
     @Override
diff --git a/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionCHNoSOD.java b/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionCHNoSOD.java
index b810763ff..d8db428f6 100644
--- a/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionCHNoSOD.java
+++ b/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionCHNoSOD.java
@@ -25,6 +25,9 @@ import com.graphhopper.storage.Graph;
 public class DijkstraBidirectionCHNoSOD extends DijkstraBidirectionRef {
     public DijkstraBidirectionCHNoSOD(Graph graph, Weighting weighting, TraversalMode traversalMode) {
         super(graph, weighting, traversalMode);
+        if (traversalMode.isEdgeBased()) {
+            throw new IllegalArgumentException("Edge based traversal is not supported for CH");
+        }
     }
 
     @Override
diff --git a/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionRef.java b/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionRef.java
index 3e122fad1..4c3a236ef 100644
--- a/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionRef.java
+++ b/core/src/main/java/com/graphhopper/routing/DijkstraBidirectionRef.java
@@ -17,16 +17,12 @@
  */
 package com.graphhopper.routing;
 
-import com.carrotsearch.hppc.IntObjectMap;
-import com.graphhopper.coll.GHIntObjectHashMap;
 import com.graphhopper.routing.util.TraversalMode;
 import com.graphhopper.routing.weighting.Weighting;
 import com.graphhopper.storage.Graph;
 import com.graphhopper.storage.SPTEntry;
 import com.graphhopper.util.*;
 
-import java.util.PriorityQueue;
-
 /**
  * Calculates best path in bidirectional way.
  * <p>
@@ -36,225 +32,24 @@ import java.util.PriorityQueue;
  * @author Peter Karich
  */
 public class DijkstraBidirectionRef extends AbstractBidirAlgo {
-    protected IntObjectMap<SPTEntry> bestWeightMapFrom;
-    protected IntObjectMap<SPTEntry> bestWeightMapTo;
-    protected IntObjectMap<SPTEntry> bestWeightMapOther;
-    protected SPTEntry currFrom;
-    protected SPTEntry currTo;
-    protected PathBidirRef bestPath;
-    PriorityQueue<SPTEntry> pqOpenSetFrom;
-    PriorityQueue<SPTEntry> pqOpenSetTo;
-    private boolean updateBestPath = true;
-
     public DijkstraBidirectionRef(Graph graph, Weighting weighting, TraversalMode tMode) {
         super(graph, weighting, tMode);
-        int size = Math.min(Math.max(200, graph.getNodes() / 10), 150_000);
-        initCollections(size);
-    }
-
-    protected void initCollections(int size) {
-        pqOpenSetFrom = new PriorityQueue<SPTEntry>(size);
-        bestWeightMapFrom = new GHIntObjectHashMap<SPTEntry>(size);
-
-        pqOpenSetTo = new PriorityQueue<SPTEntry>(size);
-        bestWeightMapTo = new GHIntObjectHashMap<SPTEntry>(size);
-    }
-
-    @Override
-    public void initFrom(int from, double weight) {
-        currFrom = createSPTEntry(from, weight);
-        pqOpenSetFrom.add(currFrom);
-        if (!traversalMode.isEdgeBased()) {
-            bestWeightMapFrom.put(from, currFrom);
-            if (currTo != null) {
-                bestWeightMapOther = bestWeightMapTo;
-                updateBestPath(GHUtility.getEdge(graph, from, currTo.adjNode), currTo, from);
-            }
-        } else if (currTo != null && currTo.adjNode == from) {
-            // special case of identical start and end
-            bestPath.sptEntry = currFrom;
-            bestPath.edgeTo = currTo;
-            finishedFrom = true;
-            finishedTo = true;
-        }
-    }
-
-    @Override
-    public void initTo(int to, double weight) {
-        currTo = createSPTEntry(to, weight);
-        pqOpenSetTo.add(currTo);
-        if (!traversalMode.isEdgeBased()) {
-            bestWeightMapTo.put(to, currTo);
-            if (currFrom != null) {
-                bestWeightMapOther = bestWeightMapFrom;
-                updateBestPath(GHUtility.getEdge(graph, currFrom.adjNode, to), currFrom, to);
-            }
-        } else if (currFrom != null && currFrom.adjNode == to) {
-            // special case of identical start and end
-            bestPath.sptEntry = currFrom;
-            bestPath.edgeTo = currTo;
-            finishedFrom = true;
-            finishedTo = true;
-        }
-    }
-
-    @Override
-    protected Path createAndInitPath() {
-        bestPath = new PathBidirRef(graph, weighting);
-        return bestPath;
-    }
-
-    @Override
-    protected Path extractPath() {
-        if (finished())
-            return bestPath.extract();
-
-        return bestPath;
-    }
-
-    @Override
-    protected double getCurrentFromWeight() {
-        return currFrom.weight;
-    }
-
-    @Override
-    protected double getCurrentToWeight() {
-        return currTo.weight;
-    }
-
-    @Override
-    public boolean fillEdgesFrom() {
-        if (pqOpenSetFrom.isEmpty())
-            return false;
-
-        currFrom = pqOpenSetFrom.poll();
-        bestWeightMapOther = bestWeightMapTo;
-        fillEdges(currFrom, pqOpenSetFrom, bestWeightMapFrom, outEdgeExplorer, false);
-        visitedCountFrom++;
-        return true;
     }
 
     @Override
-    public boolean fillEdgesTo() {
-        if (pqOpenSetTo.isEmpty())
-            return false;
-        currTo = pqOpenSetTo.poll();
-        bestWeightMapOther = bestWeightMapFrom;
-        fillEdges(currTo, pqOpenSetTo, bestWeightMapTo, inEdgeExplorer, true);
-        visitedCountTo++;
-        return true;
+    protected SPTEntry createStartEntry(int node, double weight, boolean reverse) {
+        return new SPTEntry(node, weight);
     }
 
-    // http://www.cs.princeton.edu/courses/archive/spr06/cos423/Handouts/EPP%20shortest%20path%20algorithms.pdf
-    // a node from overlap may not be on the best path!
-    // => when scanning an arc (v, w) in the forward search and w is scanned in the reverseOrder 
-    //    search, update extractPath = μ if df (v) + (v, w) + dr (w) < μ            
     @Override
-    public boolean finished() {
-        if (finishedFrom || finishedTo)
-            return true;
-
-        return currFrom.weight + currTo.weight >= bestPath.getWeight();
-    }
-
-    void fillEdges(SPTEntry currEdge, PriorityQueue<SPTEntry> prioQueue,
-                   IntObjectMap<SPTEntry> bestWeightMap, EdgeExplorer explorer, boolean reverse) {
-        EdgeIterator iter = explorer.setBaseNode(currEdge.adjNode);
-        while (iter.next()) {
-            if (!accept(iter, currEdge.edge))
-                continue;
-
-            int traversalId = traversalMode.createTraversalId(iter, reverse);
-            double tmpWeight = weighting.calcWeight(iter, reverse, currEdge.edge) + currEdge.weight;
-            if (Double.isInfinite(tmpWeight))
-                continue;
-            SPTEntry ee = bestWeightMap.get(traversalId);
-            if (ee == null) {
-                ee = new SPTEntry(iter.getEdge(), iter.getAdjNode(), tmpWeight);
-                ee.parent = currEdge;
-                bestWeightMap.put(traversalId, ee);
-                prioQueue.add(ee);
-            } else if (ee.weight > tmpWeight) {
-                prioQueue.remove(ee);
-                ee.edge = iter.getEdge();
-                ee.weight = tmpWeight;
-                ee.parent = currEdge;
-                prioQueue.add(ee);
-            } else
-                continue;
-
-            if (updateBestPath)
-                updateBestPath(iter, ee, traversalId);
-        }
-    }
-
-    @Override
-    protected void updateBestPath(EdgeIteratorState edgeState, SPTEntry entryCurrent, int traversalId) {
-        SPTEntry entryOther = bestWeightMapOther.get(traversalId);
-        if (entryOther == null)
-            return;
-
-        boolean reverse = bestWeightMapFrom == bestWeightMapOther;
-
-        // update μ
-        double newWeight = entryCurrent.weight + entryOther.weight;
-        if (traversalMode.isEdgeBased()) {
-            if (entryOther.edge != entryCurrent.edge)
-                throw new IllegalStateException("cannot happen for edge based execution of " + getName());
-
-            if (entryOther.adjNode != entryCurrent.adjNode) {
-                // prevents the path to contain the edge at the meeting point twice and subtract the weight (excluding turn weight => no previous edge)
-                entryCurrent = entryCurrent.parent;
-                newWeight -= weighting.calcWeight(edgeState, reverse, EdgeIterator.NO_EDGE);
-            } else if (!traversalMode.hasUTurnSupport())
-                // we detected a u-turn at meeting point, skip if not supported
-                return;
-        }
-
-        if (newWeight < bestPath.getWeight()) {
-            bestPath.setSwitchToFrom(reverse);
-            bestPath.setSPTEntry(entryCurrent);
-            bestPath.setWeight(newWeight);
-            bestPath.setSPTEntryTo(entryOther);
-        }
-    }
-
-    IntObjectMap<SPTEntry> getBestFromMap() {
-        return bestWeightMapFrom;
-    }
-
-    IntObjectMap<SPTEntry> getBestToMap() {
-        return bestWeightMapTo;
-    }
-
-    void setBestOtherMap(IntObjectMap<SPTEntry> other) {
-        bestWeightMapOther = other;
-    }
-
-    void setFromDataStructures(DijkstraBidirectionRef dijkstra) {
-        pqOpenSetFrom = dijkstra.pqOpenSetFrom;
-        bestWeightMapFrom = dijkstra.bestWeightMapFrom;
-        finishedFrom = dijkstra.finishedFrom;
-        currFrom = dijkstra.currFrom;
-        visitedCountFrom = dijkstra.visitedCountFrom;
-        // outEdgeExplorer
-    }
-
-    void setToDataStructures(DijkstraBidirectionRef dijkstra) {
-        pqOpenSetTo = dijkstra.pqOpenSetTo;
-        bestWeightMapTo = dijkstra.bestWeightMapTo;
-        finishedTo = dijkstra.finishedTo;
-        currTo = dijkstra.currTo;
-        visitedCountTo = dijkstra.visitedCountTo;
-        // inEdgeExplorer
-    }
-
-    protected void setUpdateBestPath(boolean b) {
-        updateBestPath = b;
+    protected SPTEntry createEntry(EdgeIteratorState edge, double weight, SPTEntry parent, boolean reverse) {
+        SPTEntry entry = new SPTEntry(edge.getEdge(), edge.getAdjNode(), weight);
+        entry.parent = parent;
+        return entry;
     }
 
-    void setBestPath(PathBidirRef bestPath) {
-        this.bestPath = bestPath;
+    protected SPTEntry getParent(SPTEntry entry) {
+        return entry.getParent();
     }
 
     @Override
diff --git a/core/src/main/java/com/graphhopper/routing/PathBidirRef.java b/core/src/main/java/com/graphhopper/routing/PathBidirRef.java
index 6a1a43583..447161eed 100644
--- a/core/src/main/java/com/graphhopper/routing/PathBidirRef.java
+++ b/core/src/main/java/com/graphhopper/routing/PathBidirRef.java
@@ -61,7 +61,7 @@ public class PathBidirRef extends Path {
             return this;
 
         if (sptEntry.adjNode != edgeTo.adjNode)
-            throw new IllegalStateException("Locations of the 'to'- and 'from'-Edge has to be the same." + toString() + ", fromEntry:" + sptEntry + ", toEntry:" + edgeTo);
+            throw new IllegalStateException("Locations of the 'to'- and 'from'-Edge have to be the same. " + toString() + ", fromEntry:" + sptEntry + ", toEntry:" + edgeTo);
 
         extractSW.start();
         if (switchFromAndToSPTEntry) {
diff --git a/core/src/main/java/com/graphhopper/routing/QueryGraph.java b/core/src/main/java/com/graphhopper/routing/QueryGraph.java
index a05b15078..39e875258 100644
--- a/core/src/main/java/com/graphhopper/routing/QueryGraph.java
+++ b/core/src/main/java/com/graphhopper/routing/QueryGraph.java
@@ -52,7 +52,7 @@ import java.util.*;
  * @author Peter Karich
  */
 public class QueryGraph implements Graph {
-    final static int VE_BASE = 0, VE_BASE_REV = 1, VE_ADJ = 2, VE_ADJ_REV = 3;
+    static final int VE_BASE = 0, VE_BASE_REV = 1, VE_ADJ = 2, VE_ADJ_REV = 3;
     private static final AngleCalc AC = Helper.ANGLE_CALC;
     private final Graph mainGraph;
     private final NodeAccess mainNodeAccess;
@@ -61,7 +61,7 @@ public class QueryGraph implements Graph {
     private final QueryGraph baseGraph;
     private final GraphExtension wrappedExtension;
     // TODO when spreading it on different threads we need multiple independent explorers
-    private final Map<Integer, EdgeExplorer> cacheMap = new HashMap<Integer, EdgeExplorer>(4);
+    private final Map<Integer, EdgeExplorer> cacheMap = new HashMap<>(4);
 
     // For every virtual node there are 4 edges: base-snap, snap-base, snap-adj, adj-snap.
     List<VirtualEdgeIteratorState> virtualEdges;
diff --git a/core/src/main/java/com/graphhopper/routing/ch/CHAlgoFactoryDecorator.java b/core/src/main/java/com/graphhopper/routing/ch/CHAlgoFactoryDecorator.java
index f4898ea1a..96b69376c 100644
--- a/core/src/main/java/com/graphhopper/routing/ch/CHAlgoFactoryDecorator.java
+++ b/core/src/main/java/com/graphhopper/routing/ch/CHAlgoFactoryDecorator.java
@@ -39,8 +39,8 @@ import static com.graphhopper.util.Helper.*;
 import static com.graphhopper.util.Parameters.CH.DISABLE;
 
 /**
- * This class implements the CH decorator and provides several helper methods related to CH
- * preparation and its vehicle profiles.
+ * This class implements the CH decorator for the routing algorithm factory and provides several
+ * helper methods related to CH preparation and its vehicle profiles.
  *
  * @author Peter Karich
  */
@@ -80,7 +80,7 @@ public class CHAlgoFactoryDecorator implements RoutingAlgorithmFactoryDecorator
         // default is enabled & fastest
         String chWeightingsStr = args.get(CH.PREPARE + "weightings", "");
 
-        if ("no".equals(chWeightingsStr)) {
+        if ("no".equals(chWeightingsStr) || "false".equals(chWeightingsStr)) {
             // default is fastest and we need to clear this explicitely
             weightingsAsStrings.clear();
         } else if (!chWeightingsStr.isEmpty()) {
diff --git a/core/src/main/java/com/graphhopper/routing/ch/NodeContractor.java b/core/src/main/java/com/graphhopper/routing/ch/NodeContractor.java
index 63500bd8c..817ae8f9f 100644
--- a/core/src/main/java/com/graphhopper/routing/ch/NodeContractor.java
+++ b/core/src/main/java/com/graphhopper/routing/ch/NodeContractor.java
@@ -69,7 +69,7 @@ class NodeContractor {
     void initFromGraph() {
         // todo: do we really need this method ? the problem is that ghStorage/prepareGraph can potentially be modified
         // between the constructor call and contractNode,calcShortcutCount etc. ...
-        maxLevel = prepareGraph.getNodes() + 1;
+        maxLevel = prepareGraph.getNodes();
         maxEdgesCount = ghStorage.getAllEdges().length();
         ignoreNodeFilter = new IgnoreNodeFilter(prepareGraph, maxLevel);
         FlagEncoder prepareFlagEncoder = prepareWeighting.getFlagEncoder();
diff --git a/core/src/main/java/com/graphhopper/routing/ch/PreparationWeighting.java b/core/src/main/java/com/graphhopper/routing/ch/PreparationWeighting.java
index e937edeb4..79c6ad1b1 100644
--- a/core/src/main/java/com/graphhopper/routing/ch/PreparationWeighting.java
+++ b/core/src/main/java/com/graphhopper/routing/ch/PreparationWeighting.java
@@ -24,7 +24,7 @@ import com.graphhopper.util.CHEdgeIteratorState;
 import com.graphhopper.util.EdgeIteratorState;
 
 /**
- * Used in CH preparation and therefor assumed that all edges are of type CHEdgeIteratorState
+ * Used in CH preparation and therefore assumed that all edges are of type CHEdgeIteratorState
  * <p>
  *
  * @author Peter Karich
diff --git a/core/src/main/java/com/graphhopper/routing/ch/PrepareContractionHierarchies.java b/core/src/main/java/com/graphhopper/routing/ch/PrepareContractionHierarchies.java
index 6106deb49..2f22d8c85 100644
--- a/core/src/main/java/com/graphhopper/routing/ch/PrepareContractionHierarchies.java
+++ b/core/src/main/java/com/graphhopper/routing/ch/PrepareContractionHierarchies.java
@@ -28,6 +28,7 @@ import org.slf4j.LoggerFactory;
 
 import java.util.*;
 
+import static com.graphhopper.util.Helper.nf;
 import static com.graphhopper.util.Parameters.Algorithms.ASTAR_BI;
 import static com.graphhopper.util.Parameters.Algorithms.DIJKSTRA_BI;
 
@@ -59,7 +60,7 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation imple
     private CHEdgeExplorer vehicleAllTmpExplorer;
     private CHEdgeExplorer calcPrioAllExplorer;
     private int maxLevel;
-    // the most important nodes comes last
+    // nodes with highest priority come last
     private GHTreeMapComposed sortedNodes;
     private int oldPriorities[];
     private double meanDegree;
@@ -72,6 +73,7 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation imple
     private double periodTime;
     private double lazyTime;
     private double neighborTime;
+    private int initSize;
 
     public PrepareContractionHierarchies(Directory dir, GraphHopperStorage ghStorage, CHGraph chGraph,
                                          Weighting weighting, TraversalMode traversalMode) {
@@ -156,14 +158,31 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation imple
     }
 
     @Override
-    public void doWork() {
+    public void doSpecificWork() {
         allSW.start();
-        super.doWork();
-
         initFromGraph();
+        runGraphContraction();
+
+        logger.info("took:" + (int) allSW.stop().getSeconds() + "s "
+                + ", new shortcuts: " + nf(nodeContractor.getAddedShortcutsCount())
+                + ", " + prepareWeighting
+                + ", dijkstras:" + nf(nodeContractor.getDijkstraCount())
+                + ", " + getTimesAsString()
+                + ", meanDegree:" + (long) meanDegree
+                + ", initSize:" + nf(initSize)
+                + ", periodic:" + periodicUpdatesPercentage
+                + ", lazy:" + lastNodesLazyUpdatePercentage
+                + ", neighbor:" + neighborUpdatePercentage
+                + ", " + Helper.getMemInfo());
+
+        int edgeCount = ghStorage.getAllEdges().length();
+        logger.info("graph now - num edges: {}, num nodes: {}, num shortcuts: {}",
+                nf(edgeCount), nf(ghStorage.getNodes()), nf(prepareGraph.getAllEdges().length() - edgeCount));
+    }
+
+    protected void runGraphContraction() {
         if (!prepareNodes())
             return;
-
         contractNodes();
     }
 
@@ -201,7 +220,7 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation imple
             }
         };
 
-        maxLevel = prepareGraph.getNodes() + 1;
+        maxLevel = prepareGraph.getNodes();
         vehicleAllExplorer = prepareGraph.createEdgeExplorer(allFilter);
         vehicleAllTmpExplorer = prepareGraph.createEdgeExplorer(allFilter);
         calcPrioAllExplorer = prepareGraph.createEdgeExplorer(accessWithLevelFilter);
@@ -239,10 +258,10 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation imple
         // no witness path can be found. this is not really what we want, but changing it requires re-optimizing the
         // graph contraction parameters, because it affects the node contraction order.
         meanDegree = prepareGraph.getAllEdges().length() / prepareGraph.getNodes();
-        int level = 1;
+        initSize = sortedNodes.getSize();
+        int level = 0;
         long counter = 0;
-        int initSize = sortedNodes.getSize();
-        long logSize = Math.round(Math.max(10, sortedNodes.getSize() / 100 * logMessagesPercentage));
+        long logSize = Math.round(Math.max(10, initSize / 100d * logMessagesPercentage));
         if (logMessagesPercentage == 0)
             logSize = Integer.MAX_VALUE;
 
@@ -261,7 +280,7 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation imple
 
         // according to paper "Polynomial-time Construction of Contraction Hierarchies for Multi-criteria Objectives" by Funke and Storandt
         // we don't need to wait for all nodes to be contracted
-        long nodesToAvoidContract = Math.round((100 - nodesContractedPercentage) / 100 * sortedNodes.getSize());
+        long nodesToAvoidContract = Math.round((100 - nodesContractedPercentage) / 100d * sortedNodes.getSize());
         StopWatch lazySW = new StopWatch();
 
         // Recompute priority of uncontracted neighbors.
@@ -297,14 +316,7 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation imple
                 lazyTime += lazySW.getSeconds();
                 neighborTime += neighborSW.getSeconds();
 
-                logger.info(Helper.nf(counter) + ", updates:" + updateCounter
-                        + ", nodes: " + Helper.nf(sortedNodes.getSize())
-                        + ", shortcuts:" + Helper.nf(nodeContractor.getAddedShortcutsCount())
-                        + ", dijkstras:" + Helper.nf(nodeContractor.getDijkstraCount())
-                        + ", " + getTimesAsString()
-                        + ", meanDegree:" + (long) meanDegree
-                        + ", algo:" + nodeContractor.getPrepareAlgoMemoryUsage()
-                        + ", " + Helper.getMemInfo());
+                logStats(counter, updateCounter);
 
                 nodeContractor.resetDijkstraTime();
                 periodSW = new StopWatch();
@@ -364,28 +376,18 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation imple
             }
         }
 
-        // Preparation works only once so we can release temporary data.
-        // The preparation object itself has to be intact to create the algorithm.
-        close();
-
         dijkstraTime += nodeContractor.getDijkstraSeconds();
         periodTime += periodSW.getSeconds();
         lazyTime += lazySW.getSeconds();
         neighborTime += neighborSW.getSeconds();
-        logger.info("took:" + (int) allSW.stop().getSeconds()
-                + ", new shortcuts: " + Helper.nf(nodeContractor.getAddedShortcutsCount())
-                + ", " + prepareWeighting
-                + ", dijkstras:" + nodeContractor.getDijkstraCount()
-                + ", " + getTimesAsString()
-                + ", meanDegree:" + (long) meanDegree
-                + ", initSize:" + initSize
-                + ", periodic:" + periodicUpdatesPercentage
-                + ", lazy:" + lastNodesLazyUpdatePercentage
-                + ", neighbor:" + neighborUpdatePercentage
-                + ", " + Helper.getMemInfo());
+        logStats(counter, updateCounter);
+
+        // Preparation works only once so we can release temporary data.
+        // The preparation object itself has to be intact to create the algorithm.
+        close();
     }
 
-    public void close() {
+    private void close() {
         nodeContractor.close();
         sortedNodes = null;
         oldPriorities = null;
@@ -420,10 +422,9 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation imple
     }
 
     private String getTimesAsString() {
-        return "t(dijk):" + Helper.round2(dijkstraTime)
-                + ", t(period):" + Helper.round2(periodTime)
-                + ", t(lazy):" + Helper.round2(lazyTime)
-                + ", t(neighbor):" + Helper.round2(neighborTime);
+        return String.format(Locale.ROOT,
+                "t(dijk): %6.2f, t(period): %6.2f, t(lazy): %6.2f, t(neighbor): %6.2f",
+                dijkstraTime, periodTime, lazyTime, neighborTime);
     }
 
     /**
@@ -481,4 +482,12 @@ public class PrepareContractionHierarchies extends AbstractAlgoPreparation imple
         return "prepare|dijkstrabi|ch";
     }
 
+    private void logStats(long counter, int updateCounter) {
+        logger.info(String.format(Locale.ROOT,
+                "%10s, updates: %2d, nodes: %10s, shortcuts: %10s, dijkstras: %10s, %s, meanDegree: %2d, %s, %s",
+                nf(counter), updateCounter, nf(sortedNodes.getSize()),
+                nf(nodeContractor.getAddedShortcutsCount()), nf(nodeContractor.getDijkstraCount()),
+                getTimesAsString(), (long) meanDegree, nodeContractor.getPrepareAlgoMemoryUsage(),
+                Helper.getMemInfo()));
+    }
 }
diff --git a/core/src/main/java/com/graphhopper/routing/lm/LandmarkStorage.java b/core/src/main/java/com/graphhopper/routing/lm/LandmarkStorage.java
index a1f26bcb2..d437a0773 100644
--- a/core/src/main/java/com/graphhopper/routing/lm/LandmarkStorage.java
+++ b/core/src/main/java/com/graphhopper/routing/lm/LandmarkStorage.java
@@ -365,10 +365,10 @@ public class LandmarkStorage implements Storable<LandmarkStorage> {
         } else {
             // 1a) pick landmarks via special weighting for a better geographical spreading
             Weighting initWeighting = lmSelectionWeighting;
-            LandmarkExplorer explorer = new LandmarkExplorer(graph, this, initWeighting, traversalMode);
-            explorer.initFrom(startNode, 0);
+            LandmarkExplorer explorer = new LandmarkExplorer(graph, this, initWeighting, traversalMode, true);
+            explorer.setStartNode(startNode);
             explorer.setFilter(blockedEdges, true, true);
-            explorer.runAlgo(true);
+            explorer.runAlgo();
 
             if (explorer.getFromCount() < minimumNodes) {
                 // too small subnetworks are initialized with special id==0
@@ -382,13 +382,13 @@ public class LandmarkStorage implements Storable<LandmarkStorage> {
                 if (Thread.currentThread().isInterrupted()) {
                     throw new RuntimeException("Thread was interrupted");
                 }
-                explorer = new LandmarkExplorer(graph, this, initWeighting, traversalMode);
+                explorer = new LandmarkExplorer(graph, this, initWeighting, traversalMode, true);
                 explorer.setFilter(blockedEdges, true, true);
                 // set all current landmarks as start so that the next getLastNode is hopefully a "far away" node
                 for (int j = 0; j < lmIdx + 1; j++) {
-                    explorer.initFrom(tmpLandmarkNodeIds[j], 0);
+                    explorer.setStartNode(tmpLandmarkNodeIds[j]);
                 }
-                explorer.runAlgo(true);
+                explorer.runAlgo();
                 tmpLandmarkNodeIds[lmIdx + 1] = explorer.getLastNode();
                 if (logDetails && lmIdx % logOffset == 0)
                     LOGGER.info("Finding landmarks [" + weighting + "] in network [" + explorer.getVisitedNodes() + "]. "
@@ -405,10 +405,10 @@ public class LandmarkStorage implements Storable<LandmarkStorage> {
                 throw new RuntimeException("Thread was interrupted");
             }
             int lmNodeId = tmpLandmarkNodeIds[lmIdx];
-            LandmarkExplorer explorer = new LandmarkExplorer(graph, this, weighting, traversalMode);
-            explorer.initFrom(lmNodeId, 0);
+            LandmarkExplorer explorer = new LandmarkExplorer(graph, this, weighting, traversalMode, true);
+            explorer.setStartNode(lmNodeId);
             explorer.setFilter(blockedEdges, false, true);
-            explorer.runAlgo(true);
+            explorer.runAlgo();
             explorer.initLandmarkWeights(lmIdx, lmNodeId, LM_ROW_LENGTH, FROM_OFFSET);
 
             // set subnetwork id to all explored nodes, but do this only for the first landmark
@@ -417,10 +417,10 @@ public class LandmarkStorage implements Storable<LandmarkStorage> {
                     return false;
             }
 
-            explorer = new LandmarkExplorer(graph, this, weighting, traversalMode);
-            explorer.initTo(lmNodeId, 0);
+            explorer = new LandmarkExplorer(graph, this, weighting, traversalMode, false);
+            explorer.setStartNode(lmNodeId);
             explorer.setFilter(blockedEdges, true, false);
-            explorer.runAlgo(false);
+            explorer.runAlgo();
             explorer.initLandmarkWeights(lmIdx, lmNodeId, LM_ROW_LENGTH, TO_OFFSET);
 
             if (lmIdx == 0) {
@@ -716,15 +716,25 @@ public class LandmarkStorage implements Storable<LandmarkStorage> {
 
     /**
      * This class is used to calculate landmark location (equally distributed).
+     * It derives from DijkstraBidirectionRef, but is only used as forward or backward search.
      */
     private static class LandmarkExplorer extends DijkstraBidirectionRef {
         private int lastNode;
+        // todo: rename 'from' to 'reverse' (and flip it) ? 'from' is used in many places for node ids and 'reverse' is mostly used for the direction
         private boolean from;
         private final LandmarkStorage lms;
 
-        public LandmarkExplorer(Graph g, LandmarkStorage lms, Weighting weighting, TraversalMode tMode) {
+        public LandmarkExplorer(Graph g, LandmarkStorage lms, Weighting weighting, TraversalMode tMode, boolean from) {
             super(g, weighting, tMode);
             this.lms = lms;
+            this.from = from;
+            // set one of the bi directions as already finished            
+            if (from)
+                finishedTo = true;
+            else
+                finishedFrom = true;
+            // no path should be calculated
+            setUpdateBestPath(false);
         }
 
         public void setFilter(IntHashSet set, boolean bwd, boolean fwd) {
@@ -745,16 +755,14 @@ public class LandmarkStorage implements Storable<LandmarkStorage> {
             return lastNode;
         }
 
-        public void runAlgo(boolean from) {
-            // no path should be calculated
-            setUpdateBestPath(false);
-            // set one of the bi directions as already finished            
+        public void setStartNode(int startNode) {
             if (from)
-                finishedTo = true;
+                initFrom(startNode, 0);
             else
-                finishedFrom = true;
+                initTo(startNode, 0);
+        }
 
-            this.from = from;
+        public void runAlgo() {
             super.runAlgo();
         }
 
diff --git a/core/src/main/java/com/graphhopper/routing/lm/PrepareLandmarks.java b/core/src/main/java/com/graphhopper/routing/lm/PrepareLandmarks.java
index 50b083a82..0de7f6524 100644
--- a/core/src/main/java/com/graphhopper/routing/lm/PrepareLandmarks.java
+++ b/core/src/main/java/com/graphhopper/routing/lm/PrepareLandmarks.java
@@ -122,9 +122,7 @@ public class PrepareLandmarks extends AbstractAlgoPreparation {
     }
 
     @Override
-    public void doWork() {
-        super.doWork();
-
+    public void doSpecificWork() {
         StopWatch sw = new StopWatch().start();
         LOGGER.info("Start calculating " + lms.getLandmarkCount() + " landmarks, default active lms:"
                 + defaultActiveLandmarks + ", weighting:" + lms.getLmSelectionWeighting() + ", " + Helper.getMemInfo());
@@ -141,7 +139,7 @@ public class PrepareLandmarks extends AbstractAlgoPreparation {
         int activeLM = Math.max(1, opts.getHints().getInt(Landmark.ACTIVE_COUNT, defaultActiveLandmarks));
         if (algo instanceof AStar) {
             if (!lms.isInitialized())
-                throw new IllegalStateException("Initalize landmark storage before creating algorithms");
+                throw new IllegalStateException("Initialize landmark storage before creating algorithms");
 
             double epsilon = opts.getHints().getDouble(Parameters.Algorithms.AStar.EPSILON, 1);
             AStar astar = (AStar) algo;
@@ -150,7 +148,7 @@ public class PrepareLandmarks extends AbstractAlgoPreparation {
             return algo;
         } else if (algo instanceof AStarBidirection) {
             if (!lms.isInitialized())
-                throw new IllegalStateException("Initalize landmark storage before creating algorithms");
+                throw new IllegalStateException("Initialize landmark storage before creating algorithms");
 
             double epsilon = opts.getHints().getDouble(Parameters.Algorithms.AStarBi.EPSILON, 1);
             AStarBidirection astarbi = (AStarBidirection) algo;
@@ -159,7 +157,7 @@ public class PrepareLandmarks extends AbstractAlgoPreparation {
             return algo;
         } else if (algo instanceof AlternativeRoute) {
             if (!lms.isInitialized())
-                throw new IllegalStateException("Initalize landmark storage before creating algorithms");
+                throw new IllegalStateException("Initialize landmark storage before creating algorithms");
 
             double epsilon = opts.getHints().getDouble(Parameters.Algorithms.AStarBi.EPSILON, 1);
             AlternativeRoute altRoute = (AlternativeRoute) algo;
diff --git a/core/src/main/java/com/graphhopper/routing/template/ViaRoutingTemplate.java b/core/src/main/java/com/graphhopper/routing/template/ViaRoutingTemplate.java
index 749ca67c3..fc516f659 100644
--- a/core/src/main/java/com/graphhopper/routing/template/ViaRoutingTemplate.java
+++ b/core/src/main/java/com/graphhopper/routing/template/ViaRoutingTemplate.java
@@ -112,6 +112,8 @@ public class ViaRoutingTemplate extends AbstractRoutingTemplate implements Routi
             String debug = ", algoInit:" + sw.stop().getSeconds() + "s";
 
             sw = new StopWatch().start();
+
+            // calculate paths
             List<Path> tmpPathList = algo.calcPaths(fromQResult.getClosestNode(), toQResult.getClosestNode());
             debug += ", " + algo.getName() + "-routing:" + sw.stop().getSeconds() + "s";
             if (tmpPathList.isEmpty())
@@ -136,6 +138,7 @@ public class ViaRoutingTemplate extends AbstractRoutingTemplate implements Routi
                 throw new IllegalArgumentException("No path found due to maximum nodes exceeded " + algoOpts.getMaxVisitedNodes());
 
             visitedNodesSum += algo.getVisitedNodes();
+            altResponse.addDebugInfo("visited nodes sum: " + visitedNodesSum);
             fromQResult = toQResult;
         }
 
diff --git a/core/src/main/java/com/graphhopper/routing/util/AbstractAlgoPreparation.java b/core/src/main/java/com/graphhopper/routing/util/AbstractAlgoPreparation.java
index 7d2715e11..eabb9e109 100644
--- a/core/src/main/java/com/graphhopper/routing/util/AbstractAlgoPreparation.java
+++ b/core/src/main/java/com/graphhopper/routing/util/AbstractAlgoPreparation.java
@@ -26,10 +26,12 @@ public abstract class AbstractAlgoPreparation {
     public void doWork() {
         if (prepared)
             throw new IllegalStateException("Call doWork only once!");
-
         prepared = true;
+        doSpecificWork();
     }
 
+    protected abstract void doSpecificWork();
+
     public boolean isPrepared() {
         return prepared;
     }
diff --git a/core/src/main/java/com/graphhopper/routing/util/AbstractFlagEncoder.java b/core/src/main/java/com/graphhopper/routing/util/AbstractFlagEncoder.java
index 42ee44989..37252a55a 100644
--- a/core/src/main/java/com/graphhopper/routing/util/AbstractFlagEncoder.java
+++ b/core/src/main/java/com/graphhopper/routing/util/AbstractFlagEncoder.java
@@ -599,7 +599,7 @@ public abstract class AbstractFlagEncoder implements FlagEncoder, TurnCostEncode
             if (costs != 0 || Double.isInfinite(costs))
                 throw new IllegalArgumentException("Restricted turn can only have infinite costs (or use 0)");
         } else if (costs >= maxTurnCosts)
-            throw new IllegalArgumentException("Cost is too high. Or specifiy restricted == true");
+            throw new IllegalArgumentException("Cost is too high. Or specify restricted == true");
 
         if (costs < 0)
             throw new IllegalArgumentException("Turn costs cannot be negative");
@@ -682,7 +682,7 @@ public abstract class AbstractFlagEncoder implements FlagEncoder, TurnCostEncode
      */
     protected double applyMaxSpeed(ReaderWay way, double speed) {
         double maxSpeed = getMaxSpeed(way);
-        // We obay speed limits
+        // We obey speed limits
         if (maxSpeed >= 0) {
             // We assume that the average speed is 90% of the allowed maximum
             return maxSpeed * 0.9;
diff --git a/core/src/main/java/com/graphhopper/routing/util/LevelEdgeFilter.java b/core/src/main/java/com/graphhopper/routing/util/LevelEdgeFilter.java
index d3bbdaf56..4ce54b06e 100644
--- a/core/src/main/java/com/graphhopper/routing/util/LevelEdgeFilter.java
+++ b/core/src/main/java/com/graphhopper/routing/util/LevelEdgeFilter.java
@@ -22,7 +22,7 @@ import com.graphhopper.util.CHEdgeIteratorState;
 import com.graphhopper.util.EdgeIteratorState;
 
 /**
- * Only certain nodes are accepted and therefor the others are ignored.
+ * Only accepts edges leading to equal and higher level nodes. Used in CH preparation.
  * <p>
  *
  * @author Peter Karich
diff --git a/core/src/main/java/com/graphhopper/storage/GraphBuilder.java b/core/src/main/java/com/graphhopper/storage/GraphBuilder.java
index 00693d2dc..68286c0d8 100644
--- a/core/src/main/java/com/graphhopper/storage/GraphBuilder.java
+++ b/core/src/main/java/com/graphhopper/storage/GraphBuilder.java
@@ -23,7 +23,7 @@ import com.graphhopper.routing.weighting.Weighting;
 import java.util.Arrays;
 
 /**
- * For now this is just a helper class to quickly create a GraphStorage.
+ * For now this is just a helper class to quickly create a {@link GraphHopperStorage}
  * <p>
  *
  * @author Peter Karich
@@ -86,16 +86,14 @@ public class GraphBuilder {
     }
 
     /**
-     * Default graph is a GraphStorage with an in memory directory and disabled storing on flush.
-     * Afterwards you'll need to call GraphStorage. Create to have a usable object. Better use
-     * create.
+     * Default graph is a {@link GraphHopperStorage} with an in memory directory and disabled storing on flush.
+     * Afterwards you'll need to call {@link GraphHopperStorage#create} to have a usable object. Better use
+     * {@link GraphHopperStorage#create} directly.
      */
     public GraphHopperStorage build() {
-        Directory dir;
-        if (mmap)
-            dir = new MMapDirectory(location);
-        else
-            dir = new RAMDirectory(location, store);
+        Directory dir = mmap ?
+                new MMapDirectory(location) :
+                new RAMDirectory(location, store);
 
         GraphHopperStorage graph;
         if (encodingManager.needsTurnCostsSupport() || singleCHWeighting == null)
@@ -107,7 +105,7 @@ public class GraphBuilder {
     }
 
     /**
-     * Default graph is a GraphStorage with an in memory directory and disabled storing on flush.
+     * Default graph is a {@link GraphHopperStorage} with an in memory directory and disabled storing on flush.
      */
     public GraphHopperStorage create() {
         return build().create(byteCapacity);
diff --git a/core/src/main/java/com/graphhopper/storage/GraphExtension.java b/core/src/main/java/com/graphhopper/storage/GraphExtension.java
index 08aacbf05..0883f407d 100644
--- a/core/src/main/java/com/graphhopper/storage/GraphExtension.java
+++ b/core/src/main/java/com/graphhopper/storage/GraphExtension.java
@@ -23,12 +23,12 @@ package com.graphhopper.storage;
  */
 public interface GraphExtension extends Storable<GraphExtension> {
     /**
-     * @return true, if and only if, if an additional field at the graphs node storage is required
+     * @return true, if and only if an additional field at the graphs node storage is required
      */
     boolean isRequireNodeField();
 
     /**
-     * @return true, if and only if, if an additional field at the graphs edge storage is required
+     * @return true, if and only if an additional field at the graphs edge storage is required
      */
     boolean isRequireEdgeField();
 
diff --git a/core/src/main/java/com/graphhopper/storage/GraphHopperStorage.java b/core/src/main/java/com/graphhopper/storage/GraphHopperStorage.java
index 190005af5..9ac7866c9 100644
--- a/core/src/main/java/com/graphhopper/storage/GraphHopperStorage.java
+++ b/core/src/main/java/com/graphhopper/storage/GraphHopperStorage.java
@@ -47,7 +47,7 @@ public final class GraphHopperStorage implements GraphStorage, Graph {
     private final StorableProperties properties;
     private final BaseGraph baseGraph;
     // same flush order etc
-    private final Collection<CHGraphImpl> chGraphs = new ArrayList<CHGraphImpl>(5);
+    private final Collection<CHGraphImpl> chGraphs = new ArrayList<>(5);
 
     public GraphHopperStorage(Directory dir, EncodingManager encodingManager, boolean withElevation, GraphExtension extendedStorage) {
         this(Collections.<Weighting>emptyList(), dir, encodingManager, withElevation, extendedStorage);
@@ -100,7 +100,7 @@ public final class GraphHopperStorage implements GraphStorage, Graph {
         if (weighting == null)
             throw new IllegalStateException("Cannot find CHGraph with null weighting");
 
-        List<Weighting> existing = new ArrayList<Weighting>();
+        List<Weighting> existing = new ArrayList<>();
         for (CHGraphImpl cg : chGraphs) {
             if (cg.getWeighting() == weighting)
                 return (T) cg;
@@ -127,7 +127,7 @@ public final class GraphHopperStorage implements GraphStorage, Graph {
     }
 
     public List<Weighting> getCHWeightings() {
-        List<Weighting> list = new ArrayList<Weighting>(chGraphs.size());
+        List<Weighting> list = new ArrayList<>(chGraphs.size());
         for (CHGraphImpl cg : chGraphs) {
             list.add(cg.getWeighting());
         }
diff --git a/core/src/main/java/com/graphhopper/storage/SPTEntry.java b/core/src/main/java/com/graphhopper/storage/SPTEntry.java
index daaac4bcf..754e55b9d 100644
--- a/core/src/main/java/com/graphhopper/storage/SPTEntry.java
+++ b/core/src/main/java/com/graphhopper/storage/SPTEntry.java
@@ -17,6 +17,8 @@
  */
 package com.graphhopper.storage;
 
+import com.graphhopper.util.EdgeIterator;
+
 /**
  * This class is used to create the shortest-path-tree from linked entities.
  * <p>
@@ -35,6 +37,10 @@ public class SPTEntry implements Cloneable, Comparable<SPTEntry> {
         this.weight = weight;
     }
 
+    public SPTEntry(int node, double weight) {
+        this(EdgeIterator.NO_EDGE, node, weight);
+    }
+
     /**
      * This method returns the weight to the origin e.g. to the start for the forward SPT and to the
      * destination for the backward SPT. Where the variable 'weight' is used to let heap select
@@ -44,6 +50,10 @@ public class SPTEntry implements Cloneable, Comparable<SPTEntry> {
         return weight;
     }
 
+    public SPTEntry getParent() {
+        return parent;
+    }
+
     @Override
     public SPTEntry clone() {
         return new SPTEntry(edge, adjNode, weight);
diff --git a/core/src/main/java/com/graphhopper/util/GHUtility.java b/core/src/main/java/com/graphhopper/util/GHUtility.java
index a8c26cdaa..ff15a9bcb 100644
--- a/core/src/main/java/com/graphhopper/util/GHUtility.java
+++ b/core/src/main/java/com/graphhopper/util/GHUtility.java
@@ -26,6 +26,7 @@ import com.graphhopper.routing.util.AllEdgesIterator;
 import com.graphhopper.routing.util.EdgeFilter;
 import com.graphhopper.routing.util.FlagEncoder;
 import com.graphhopper.storage.*;
+import com.graphhopper.util.shapes.BBox;
 
 import java.util.*;
 import java.util.concurrent.atomic.AtomicInteger;
@@ -114,20 +115,49 @@ public class GHUtility {
     }
 
     public static void printEdgeInfo(final Graph g, FlagEncoder encoder) {
-        System.out.println("-- Graph n:" + g.getNodes() + " e:" + g.getAllEdges().length() + " ---");
+        System.out.println("-- Graph nodes:" + g.getNodes() + " edges:" + g.getAllEdges().length() + " ---");
         AllEdgesIterator iter = g.getAllEdges();
         while (iter.next()) {
-            String sc = "";
-            if (iter instanceof AllCHEdgesIterator) {
-                AllCHEdgesIterator aeSkip = (AllCHEdgesIterator) iter;
-                sc = aeSkip.isShortcut() ? "sc" : "  ";
-            }
+            String prefix = (iter instanceof AllCHEdgesIterator && ((AllCHEdgesIterator) iter).isShortcut()) ? "sc" : "  ";
             String fwdStr = iter.isForward(encoder) ? "fwd" : "   ";
-            String bckStr = iter.isBackward(encoder) ? "bckwd" : "";
-            System.out.println(sc + " " + iter + " " + fwdStr + " " + bckStr);
+            String bwdStr = iter.isBackward(encoder) ? "bwd" : "   ";
+            System.out.println(prefix + " " + iter + " " + fwdStr + " " + bwdStr + " " + iter.getDistance());
         }
     }
 
+    public static void printGraphForUnitTest(Graph g, FlagEncoder encoder) {
+        printGraphForUnitTest(g, encoder, new BBox(
+                Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
+    }
+
+    public static void printGraphForUnitTest(Graph g, FlagEncoder encoder, BBox bBox) {
+        NodeAccess na = g.getNodeAccess();
+        for (int node = 0; node < g.getNodes(); ++node) {
+            if (bBox.contains(na.getLat(node), na.getLon(node))) {
+                System.out.printf(Locale.ROOT, "na.setNode(%d, %f, %f);\n", node, na.getLat(node), na.getLon(node));
+            }
+        }
+        AllEdgesIterator iter = g.getAllEdges();
+        while (iter.next()) {
+            if (bBox.contains(na.getLat(iter.getBaseNode()), na.getLon(iter.getBaseNode())) &&
+                    bBox.contains(na.getLat(iter.getAdjNode()), na.getLon(iter.getAdjNode()))) {
+                printUnitTestEdge(encoder, iter);
+            }
+        }
+    }
+
+    private static void printUnitTestEdge(FlagEncoder flagEncoder, EdgeIteratorState edge) {
+        boolean fwd = edge.isForward(flagEncoder);
+        boolean bwd = edge.isBackward(flagEncoder);
+        if (!fwd && !bwd) {
+            return;
+        }
+        int from = fwd ? edge.getBaseNode() : edge.getAdjNode();
+        int to = fwd ? edge.getAdjNode() : edge.getBaseNode();
+        System.out.printf(Locale.ROOT,
+                "graph.edge(%d, %d, %f, %s);\n", from, to, edge.getDistance(), fwd && bwd ? "true" : "false");
+    }
+
     public static void printInfo(final Graph g, int startNode, final int counts, final EdgeFilter filter) {
         new BreadthFirstSearch() {
             int counter = 0;
@@ -135,10 +165,7 @@ public class GHUtility {
             @Override
             protected boolean goFurther(int nodeId) {
                 System.out.println(getNodeInfo(g, nodeId, filter));
-                if (counter++ > counts) {
-                    return false;
-                }
-                return true;
+                return counter++ <= counts;
             }
         }.start(g.createEdgeExplorer(), startNode);
     }
@@ -307,8 +334,6 @@ public class GHUtility {
         };
     }
 
-    ;
-
     /**
      * @return the <b>first</b> edge containing the specified nodes base and adj. Returns null if
      * not found.
diff --git a/core/src/main/java/com/graphhopper/util/StopWatch.java b/core/src/main/java/com/graphhopper/util/StopWatch.java
index f32034478..de9d0c03a 100644
--- a/core/src/main/java/com/graphhopper/util/StopWatch.java
+++ b/core/src/main/java/com/graphhopper/util/StopWatch.java
@@ -58,7 +58,7 @@ public class StopWatch {
      * @return the time delta in milliseconds
      */
     public long getTime() {
-        return nanoTime / 1000000;
+        return nanoTime / 1_000_000;
     }
 
     public long getNanos() {
diff --git a/graphhopper.sh b/graphhopper.sh
index 004867f7e..2db0f393d 100755
--- a/graphhopper.sh
+++ b/graphhopper.sh
@@ -57,6 +57,9 @@ while [ ! -z $1 ]; do
     --port) GH_WEB_OPTS="$GH_WEB_OPTS -Ddw.server.applicationConnectors[0].port=$2"; shift 2;;
     -v|--version) echo $VERSION
     	exit 2;;
+    # forward VM options, here we assume no spaces ie. just one parameter!?
+    -D*)
+       GH_WEB_OPTS="$GH_WEB_OPTS $1"; shift 1;;
     # forward parameter via replacing first two characters of the key with -Dgraphhopper.
     --*)
        GH_WEB_OPTS="$GH_WEB_OPTS -Dgraphhopper.${1:2}=$2"; shift 2;;
@@ -235,6 +238,7 @@ packageJar
 echo "## now $ACTION. JAVA_OPTS=$JAVA_OPTS"
 
 if [[ "$ACTION" = "webdebug" ]]; then
+  echo $GH_WEB_OPTS
   exec "$JAVA" $JAVA_OPTS -Dgraphhopper.datareader.file="$OSM_FILE" -Dgraphhopper.graph.location="$GRAPH" \
                  $GH_WEB_OPTS -cp "$JAR" com.graphhopper.http.GraphHopperDebugApplication server $CONFIG
 
diff --git a/tools/src/main/java/com/graphhopper/tools/Measurement.java b/tools/src/main/java/com/graphhopper/tools/Measurement.java
index 2f3333505..bd4c51af2 100644
--- a/tools/src/main/java/com/graphhopper/tools/Measurement.java
+++ b/tools/src/main/java/com/graphhopper/tools/Measurement.java
@@ -56,7 +56,7 @@ import static com.graphhopper.util.Parameters.Algorithms.DIJKSTRA_BI;
  */
 public class Measurement {
     private static final Logger logger = LoggerFactory.getLogger(Measurement.class);
-    private final Map<String, String> properties = new TreeMap<String, String>();
+    private final Map<String, String> properties = new TreeMap<>();
     private long seed;
     private int maxNode;
 
diff --git a/tools/src/main/java/com/graphhopper/ui/DebugAStarBi.java b/tools/src/main/java/com/graphhopper/ui/DebugAStarBi.java
index 949808883..7218ae11c 100644
--- a/tools/src/main/java/com/graphhopper/ui/DebugAStarBi.java
+++ b/tools/src/main/java/com/graphhopper/ui/DebugAStarBi.java
@@ -45,16 +45,11 @@ public class DebugAStarBi extends AStarBidirection implements DebugAlgo {
     }
 
     @Override
-    public void updateBestPath(EdgeIteratorState edgeState, AStar.AStarEntry entryCurrent, int currLoc) {
+    public void updateBestPath(EdgeIteratorState edgeState, SPTEntry entry, int traversalId, boolean reverse) {
         if (g2 != null) {
-            mg.plotNode(g2, currLoc, Color.YELLOW);
+            mg.plotNode(g2, traversalId, Color.YELLOW);
         }
-        super.updateBestPath(edgeState, entryCurrent, currLoc);
-    }
-
-    @Override
-    public void updateBestPath(EdgeIteratorState es, SPTEntry bestEE, int currLoc) {
-        throw new IllegalStateException("cannot happen");
+        super.updateBestPath(edgeState, entry, traversalId, reverse);
     }
 
     @Override
diff --git a/tools/src/main/java/com/graphhopper/ui/DebugDijkstraBidirection.java b/tools/src/main/java/com/graphhopper/ui/DebugDijkstraBidirection.java
index 8c2b04ed5..aa232621a 100644
--- a/tools/src/main/java/com/graphhopper/ui/DebugDijkstraBidirection.java
+++ b/tools/src/main/java/com/graphhopper/ui/DebugDijkstraBidirection.java
@@ -47,11 +47,11 @@ public class DebugDijkstraBidirection extends DijkstraBidirectionRef implements
     }
 
     @Override
-    public void updateBestPath(EdgeIteratorState es, SPTEntry bestEE, int currLoc) {
+    public void updateBestPath(EdgeIteratorState es, SPTEntry entry, int traversalId, boolean reverse) {
         if (g2 != null) {
-            mg.plotEdge(g2, na.getLat(bestEE.parent.adjNode), na.getLon(bestEE.parent.adjNode), na.getLat(currLoc), na.getLon(currLoc), .8f);
+            mg.plotEdge(g2, na.getLat(entry.parent.adjNode), na.getLon(entry.parent.adjNode), na.getLat(entry.adjNode), na.getLon(entry.adjNode), .8f);
         }
         // System.out.println("new node:" + currLoc);
-        super.updateBestPath(es, bestEE, currLoc);
+        super.updateBestPath(es, entry, traversalId, reverse);
     }
 }
diff --git a/tools/src/main/java/com/graphhopper/ui/MiniGraphUI.java b/tools/src/main/java/com/graphhopper/ui/MiniGraphUI.java
index 1e549e0b0..e38cddced 100644
--- a/tools/src/main/java/com/graphhopper/ui/MiniGraphUI.java
+++ b/tools/src/main/java/com/graphhopper/ui/MiniGraphUI.java
@@ -112,11 +112,11 @@ public class MiniGraphUI {
                     }
 
                     @Override
-                    public void updateBestPath(EdgeIteratorState es, SPTEntry bestEE, int currLoc) {
+                    public void updateBestPath(EdgeIteratorState es, SPTEntry entry, int traversalId, boolean reverse) {
                         if (g2 != null)
-                            mg.plotNode(g2, currLoc, Color.YELLOW, 6);
+                            mg.plotNode(g2, traversalId, Color.YELLOW, 6);
 
-                        super.updateBestPath(es, bestEE, currLoc);
+                        super.updateBestPath(es, entry, traversalId, reverse);
                     }
                 }
 
@@ -268,7 +268,7 @@ public class MiniGraphUI {
                     g2.setColor(color);
                     boolean fwd = encoder.isForward(edge.getFlags());
                     boolean bwd = encoder.isBackward(edge.getFlags());
-                    float width = speed > 90? 1f : 0.8f;
+                    float width = speed > 90 ? 1f : 0.8f;
                     if (fwd && !bwd) {
                         mg.plotDirectedEdge(g2, lat, lon, lat2, lon2, width);
                     } else {


Test patch:
diff --git a/core/src/test/java/com/graphhopper/routing/AStarBidirectionTest.java b/core/src/test/java/com/graphhopper/routing/AStarBidirectionTest.java
index 9e74302cc..60b70c23d 100644
--- a/core/src/test/java/com/graphhopper/routing/AStarBidirectionTest.java
+++ b/core/src/test/java/com/graphhopper/routing/AStarBidirectionTest.java
@@ -1,14 +1,14 @@
 /*
  *  Licensed to GraphHopper GmbH under one or more contributor
- *  license agreements. See the NOTICE file distributed with this work for 
+ *  license agreements. See the NOTICE file distributed with this work for
  *  additional information regarding copyright ownership.
- * 
- *  GraphHopper GmbH licenses this file to you under the Apache License, 
- *  Version 2.0 (the "License"); you may not use this file except in 
+ *
+ *  GraphHopper GmbH licenses this file to you under the Apache License,
+ *  Version 2.0 (the "License"); you may not use this file except in
  *  compliance with the License. You may obtain a copy of the License at
- * 
+ *
  *       http://www.apache.org/licenses/LICENSE-2.0
- * 
+ *
  *  Unless required by applicable law or agreed to in writing, software
  *  distributed under the License is distributed on an "AS IS" BASIS,
  *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
@@ -21,6 +21,7 @@ import com.graphhopper.routing.util.TraversalMode;
 import com.graphhopper.routing.weighting.ShortestWeighting;
 import com.graphhopper.storage.Graph;
 import com.graphhopper.storage.GraphHopperStorage;
+import com.graphhopper.storage.SPTEntry;
 import org.junit.Test;
 import org.junit.runner.RunWith;
 import org.junit.runners.Parameterized;
@@ -73,28 +74,22 @@ public class AStarBidirectionTest extends AbstractRoutingAlgorithmTester {
         updateDistancesFor(g, 0, 0.00, 0.00);
         updateDistancesFor(g, 1, 0.01, 0.01);
 
-        final AtomicReference<AStar.AStarEntry> fromRef = new AtomicReference<AStar.AStarEntry>();
-        final AtomicReference<AStar.AStarEntry> toRef = new AtomicReference<AStar.AStarEntry>();
+        final AtomicReference<SPTEntry> fromRef = new AtomicReference<>();
+        final AtomicReference<SPTEntry> toRef = new AtomicReference<>();
         AStarBidirection astar = new AStarBidirection(g, new ShortestWeighting(carEncoder), traversalMode) {
             @Override
-            public void initFrom(int from, double weight) {
-                super.initFrom(from, weight);
+            public void init(int from, double fromWeight, int to, double toWeight) {
+                super.init(from, fromWeight, to, toWeight);
                 fromRef.set(currFrom);
-            }
-
-            @Override
-            public void initTo(int to, double weight) {
-                super.initTo(to, weight);
                 toRef.set(currTo);
             }
         };
-        astar.initFrom(0, 1);
-        astar.initTo(1, 0.5);
+        astar.init(0, 1, 1, 0.5);
 
-        assertEquals(1, fromRef.get().weightOfVisitedPath, .1);
+        assertEquals(1, ((AStar.AStarEntry) fromRef.get()).weightOfVisitedPath, .1);
         assertEquals(787.3, fromRef.get().weight, .1);
 
-        assertEquals(0.5, toRef.get().weightOfVisitedPath, .1);
+        assertEquals(0.5, ((AStar.AStarEntry) toRef.get()).weightOfVisitedPath, .1);
         assertEquals(786.8, toRef.get().weight, .1);
     }
 }
diff --git a/core/src/test/java/com/graphhopper/routing/AbstractRoutingAlgorithmTester.java b/core/src/test/java/com/graphhopper/routing/AbstractRoutingAlgorithmTester.java
index ccb057206..e65940ec3 100644
--- a/core/src/test/java/com/graphhopper/routing/AbstractRoutingAlgorithmTester.java
+++ b/core/src/test/java/com/graphhopper/routing/AbstractRoutingAlgorithmTester.java
@@ -145,9 +145,13 @@ public abstract class AbstractRoutingAlgorithmTester {
 
     @Before
     public void setUp() {
-        carEncoder = (CarFlagEncoder) encodingManager.getEncoder("car");
-        footEncoder = (FootFlagEncoder) encodingManager.getEncoder("foot");
-        defaultOpts = AlgorithmOptions.start().
+        carEncoder = encodingManager.getEncoder("car");
+        footEncoder = encodingManager.getEncoder("foot");
+        defaultOpts = createAlgoOptions();
+    }
+
+    protected AlgorithmOptions createAlgoOptions() {
+        return AlgorithmOptions.start().
                 weighting(new ShortestWeighting(carEncoder)).build();
     }
 
@@ -182,6 +186,47 @@ public abstract class AbstractRoutingAlgorithmTester {
         assertEquals(p.toString(), 62.1, p.getDistance(), .1);
     }
 
+    @Test
+    public void testCalcShortestPath_sourceEqualsTarget() {
+        GraphHopperStorage graph = createGHStorage(false);
+        graph.edge(0, 1, 1, true);
+        graph.edge(1, 2, 2, true);
+
+        RoutingAlgorithm algo = createAlgo(graph);
+        Path p = algo.calcPath(0, 0);
+        assertEquals(p.toString(), Helper.createTList(0), p.calcNodes());
+        assertEquals(p.toString(), 0, p.getDistance(), 1.e-6);
+    }
+
+    @Test
+    public void testSimpleAlternative() {
+        // 0--2--1
+        //    |  |
+        //    3--4
+        GraphHopperStorage graph = createGHStorage(false);
+        graph.edge(0, 2, 9, true);
+        graph.edge(2, 1, 2, true);
+        graph.edge(2, 3, 11, true);
+        graph.edge(3, 4, 6, true);
+        graph.edge(4, 1, 9, true);
+        Path p = createAlgo(graph).calcPath(0, 4);
+        assertEquals(p.toString(), 20, p.getDistance(), 1e-4);
+        assertEquals(Helper.createTList(0, 2, 1, 4), p.calcNodes());
+    }
+
+    @Test
+    public void testBidirectionalLinear() {
+        //3--2--1--4--5
+        GraphHopperStorage graph = createGHStorage(false);
+        graph.edge(2, 1, 2, true);
+        graph.edge(2, 3, 11, true);
+        graph.edge(5, 4, 6, true);
+        graph.edge(4, 1, 9, true);
+        Path p = createAlgo(graph).calcPath(3, 5);
+        assertEquals(p.toString(), 28, p.getDistance(), 1e-4);
+        assertEquals(Helper.createTList(3, 2, 1, 4, 5), p.calcNodes());
+    }
+
     // see calc-fastest-graph.svg
     @Test
     public void testCalcFastestPath() {
@@ -349,8 +394,8 @@ public abstract class AbstractRoutingAlgorithmTester {
     public void testWikipediaShortestPath() {
         GraphHopperStorage ghStorage = createWikipediaTestGraph();
         Path p = createAlgo(ghStorage).calcPath(0, 4);
+        assertEquals(p.toString(), Helper.createTList(0, 2, 5, 4), p.calcNodes());
         assertEquals(p.toString(), 20, p.getDistance(), 1e-4);
-        assertEquals(p.toString(), 4, p.calcNodes().size());
     }
 
     @Test
@@ -392,6 +437,28 @@ public abstract class AbstractRoutingAlgorithmTester {
         assertEquals(p.toString(), 10007.7, p.getDistance(), .1);
     }
 
+    @Test
+    public void testCreateAlgoTwice() {
+        GraphHopperStorage graph = createGHStorage(false);
+        graph.edge(0, 1, 1, true);
+        graph.edge(1, 2, 1, true);
+        graph.edge(2, 3, 1, true);
+        graph.edge(3, 4, 1, true);
+        graph.edge(4, 5, 1, true);
+        graph.edge(5, 6, 1, true);
+        graph.edge(6, 7, 1, true);
+        graph.edge(7, 0, 1, true);
+        graph.edge(3, 8, 1, true);
+        graph.edge(8, 6, 1, true);
+
+        // run the same query twice, this can be interesting because in the second call algorithms that pre-process
+        // the graph might depend on the state of the graph after the first call 
+        Path p1 = createAlgo(graph).calcPath(0, 4);
+        Path p2 = createAlgo(graph).calcPath(0, 4);
+
+        assertEquals(p1.calcNodes(), p2.calcNodes());
+    }
+
     @Test
     public void testMaxVisitedNodes() {
         GraphHopperStorage graph = createGHStorage(false);
@@ -407,11 +474,11 @@ public abstract class AbstractRoutingAlgorithmTester {
         assertFalse(p.isFound());
     }
 
-    // 1-2-3-4-5
+    // 0-1-2-3-4
     // |     / |
-    // |    9  |
+    // |    8  |
     // \   /   /
-    //  8-7-6-/
+    //  7-6-5-/
     @Test
     public void testBidirectional2() {
         GraphHopperStorage graph = createGHStorage(false);
diff --git a/core/src/test/java/com/graphhopper/routing/RoutingAlgorithmIT.java b/core/src/test/java/com/graphhopper/routing/RoutingAlgorithmIT.java
index 31375af0b..b77ee88d5 100644
--- a/core/src/test/java/com/graphhopper/routing/RoutingAlgorithmIT.java
+++ b/core/src/test/java/com/graphhopper/routing/RoutingAlgorithmIT.java
@@ -166,7 +166,7 @@ public class RoutingAlgorithmIT {
             float perRun = sw.stop().getSeconds() / ((float) (N - noJvmWarming));
             System.out.println("# " + getClass().getSimpleName() + " " + entry
                     + ":" + sw.stop().getSeconds() + ", per run:" + perRun);
-            assertTrue("speed to low!? " + perRun + " per run", perRun < 0.08);
+            assertTrue("speed too low!? " + perRun + " per run", perRun < 0.08);
         }
     }
 }
diff --git a/core/src/test/java/com/graphhopper/routing/ch/NodeContractorTest.java b/core/src/test/java/com/graphhopper/routing/ch/NodeContractorTest.java
index c73b25eeb..e623ffe7b 100644
--- a/core/src/test/java/com/graphhopper/routing/ch/NodeContractorTest.java
+++ b/core/src/test/java/com/graphhopper/routing/ch/NodeContractorTest.java
@@ -85,7 +85,7 @@ public class NodeContractorTest {
 
         setMaxLevelOnAllNodes();
 
-        algo.setEdgeFilter(new NodeContractor.IgnoreNodeFilter(lg, graph.getNodes() + 1).setAvoidNode(3));
+        algo.setEdgeFilter(new NodeContractor.IgnoreNodeFilter(lg, graph.getNodes()).setAvoidNode(3));
         algo.setWeightLimit(100);
         int nodeEntry = algo.findEndNode(4, 2);
         assertTrue(algo.getWeight(nodeEntry) > normalDist);
@@ -105,7 +105,7 @@ public class NodeContractorTest {
 
         setMaxLevelOnAllNodes();
 
-        algo.setEdgeFilter(new NodeContractor.IgnoreNodeFilter(lg, graph.getNodes() + 1).setAvoidNode(3));
+        algo.setEdgeFilter(new NodeContractor.IgnoreNodeFilter(lg, graph.getNodes()).setAvoidNode(3));
         algo.setWeightLimit(10);
         int nodeEntry = algo.findEndNode(4, 2);
         assertEquals(4, algo.getWeight(nodeEntry), 1e-5);
@@ -121,7 +121,7 @@ public class NodeContractorTest {
 
         setMaxLevelOnAllNodes();
 
-        algo.setEdgeFilter(new NodeContractor.IgnoreNodeFilter(lg, graph.getNodes() + 1).setAvoidNode(0));
+        algo.setEdgeFilter(new NodeContractor.IgnoreNodeFilter(lg, graph.getNodes()).setAvoidNode(0));
         algo.setWeightLimit(2);
         int endNode = algo.findEndNode(4, 1);
         // did not reach endNode
@@ -333,7 +333,7 @@ public class NodeContractorTest {
     private void setMaxLevelOnAllNodes() {
         int nodes = lg.getNodes();
         for (int node = 0; node < nodes; node++) {
-            lg.setLevel(node, nodes + 1);
+            lg.setLevel(node, nodes);
         }
     }
 
diff --git a/core/src/test/java/com/graphhopper/storage/GraphHopperStorageCHTest.java b/core/src/test/java/com/graphhopper/storage/GraphHopperStorageCHTest.java
index 07bcfa25e..638ff061d 100644
--- a/core/src/test/java/com/graphhopper/storage/GraphHopperStorageCHTest.java
+++ b/core/src/test/java/com/graphhopper/storage/GraphHopperStorageCHTest.java
@@ -296,7 +296,7 @@ public class GraphHopperStorageCHTest extends GraphHopperStorageTest {
         assertEquals(1, GHUtility.count(graph.createEdgeExplorer().setBaseNode(1)));
         assertEquals(1, GHUtility.count(chGraph.createEdgeExplorer().setBaseNode(1)));
 
-        // base graph does not see shortcut        
+        // base graph does not see shortcut
         assertEquals(0, GHUtility.count(graph.createEdgeExplorer().setBaseNode(2)));
         assertEquals(1, GHUtility.count(chGraph.createEdgeExplorer().setBaseNode(2)));
 
diff --git a/reader-osm/src/test/java/com/graphhopper/routing/RoutingAlgorithmWithOSMIT.java b/reader-osm/src/test/java/com/graphhopper/routing/RoutingAlgorithmWithOSMIT.java
index 1f53faf8c..62c72b43e 100644
--- a/reader-osm/src/test/java/com/graphhopper/routing/RoutingAlgorithmWithOSMIT.java
+++ b/reader-osm/src/test/java/com/graphhopper/routing/RoutingAlgorithmWithOSMIT.java
@@ -139,7 +139,7 @@ public class RoutingAlgorithmWithOSMIT {
     @Test
     public void testOneWayCircleBug() {
         // export from http://www.openstreetmap.org/export#map=19/51.37605/-0.53155
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         // going the bit longer way out of the circle
         list.add(new OneRun(51.376197, -0.531576, 51.376509, -0.530863, 153, 18));
         // now exacle the opposite direction: going into the circle (shorter)
@@ -153,7 +153,7 @@ public class RoutingAlgorithmWithOSMIT {
     @Test
     public void testMoscow() {
         // extracted via ./graphhopper.sh extract "37.582641,55.805261,37.626929,55.824455"
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         // choose perpendicular
         // http://localhost:8989/?point=55.818994%2C37.595354&point=55.819175%2C37.596931
         list.add(new OneRun(55.818994, 37.595354, 55.819175, 37.596931, 1052, 14));
@@ -170,7 +170,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testMoscowTurnCosts() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         list.add(new OneRun(55.813357, 37.5958585, 55.811042, 37.594689, 1043.99, 12));
         list.add(new OneRun(55.813159, 37.593884, 55.811278, 37.594217, 1048, 13));
         // TODO include CH
@@ -183,7 +183,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testSidewalkNo() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         // roundabout contains sidewalk=no which should be avoided
         list.add(new OneRun(57.154888, -2.101822, 57.153445, -2.099869, 329, 31));
         // longer path should go through tertiary, see discussion in #476
@@ -227,7 +227,7 @@ public class RoutingAlgorithmWithOSMIT {
     }
 
     List<OneRun> createMonacoFoot() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         list.add(new OneRun(43.730729, 7.421288, 43.727697, 7.419199, 1566, 92));
         list.add(new OneRun(43.727687, 7.418737, 43.74958, 7.436566, 3438, 136));
         list.add(new OneRun(43.728677, 7.41016, 43.739213, 7.427806, 2085, 112));
@@ -269,7 +269,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testNorthBayreuthHikeFastestAnd3D() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         // prefer hiking route 'Teufelsloch Unterwaiz' and 'Rotmain-Wanderweg'        
         list.add(new OneRun(49.974972, 11.515657, 49.991022, 11.512299, 2365, 66));
         // prefer hiking route 'Markgrafenweg Bayreuth Kulmbach' but avoid tertiary highway from Pechgraben
@@ -281,7 +281,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testMonacoBike3D_twoSpeedsPerEdge() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         // 1. alternative: go over steps 'Rampe Major' => 1.7km vs. around 2.7km
         list.add(new OneRun(43.730864, 7.420771, 43.727687, 7.418737, 2710, 118));
         // 2.
@@ -319,7 +319,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testMonacoBike() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         list.add(new OneRun(43.730864, 7.420771, 43.727687, 7.418737, 1642, 87));
         list.add(new OneRun(43.727687, 7.418737, 43.74958, 7.436566, 3580, 168));
         list.add(new OneRun(43.728677, 7.41016, 43.739213, 7.427806, 2323, 121));
@@ -331,7 +331,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testMonacoMountainBike() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         list.add(new OneRun(43.730864, 7.420771, 43.727687, 7.418737, 2322, 110));
         list.add(new OneRun(43.727687, 7.418737, 43.74958, 7.436566, 3655, 176));
         list.add(new OneRun(43.728677, 7.41016, 43.739213, 7.427806, 2331, 121));
@@ -348,7 +348,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testMonacoRacingBike() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         list.add(new OneRun(43.730864, 7.420771, 43.727687, 7.418737, 2594, 111));
         list.add(new OneRun(43.727687, 7.418737, 43.74958, 7.436566, 3588, 170));
         list.add(new OneRun(43.728677, 7.41016, 43.739213, 7.427806, 2572, 135));
@@ -364,7 +364,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testKremsBikeRelation() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         list.add(new OneRun(48.409523, 15.602394, 48.375466, 15.72916, 12491, 159));
         // 3109m is better as cyclepath is used
         list.add(new OneRun(48.410061, 15.63951, 48.411386, 15.604899, 3112, 87));
@@ -381,7 +381,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testKremsMountainBikeRelation() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         list.add(new OneRun(48.409523, 15.602394, 48.375466, 15.72916, 12574, 169));
         list.add(new OneRun(48.410061, 15.63951, 48.411386, 15.604899, 3101, 94));
         list.add(new OneRun(48.412294, 15.62007, 48.398306, 15.609667, 3965, 95));
@@ -396,7 +396,7 @@ public class RoutingAlgorithmWithOSMIT {
     }
 
     List<OneRun> createAndorra() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         list.add(new OneRun(42.56819, 1.603231, 42.571034, 1.520662, 17708, 524));
         list.add(new OneRun(42.529176, 1.571302, 42.571034, 1.520662, 11408, 305));
         return list;
@@ -436,7 +436,7 @@ public class RoutingAlgorithmWithOSMIT {
         // bzcat campo-grande.osm.bz2 
         //   | ./bin/osmosis --read-xml enableDateParsing=no file=- --bounding-box top=-20.4 left=-54.6 bottom=-20.6 right=-54.5 --write-xml file=- 
         //   | bzip2 > campo-grande.extracted.osm.bz2
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         list.add(new OneRun(-20.4, -54.6, -20.6, -54.54, 25516, 271));
         list.add(new OneRun(-20.43, -54.54, -20.537, -54.674, 18009, 237));
         runAlgo(testCollector, DIR + "/campo-grande.osm.gz", "target/campo-grande-gh", list,
@@ -451,7 +451,7 @@ public class RoutingAlgorithmWithOSMIT {
         oneRun.add(43.727697, 7.419199, 2581, 110);
         oneRun.add(43.726387, 7.4, 3001, 90);
 
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         list.add(oneRun);
 
         runAlgo(testCollector, DIR + "/monaco.osm.gz", "target/monaco-gh",
@@ -461,7 +461,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testHarsdorf() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         // TODO somehow the bigger road is take even if we make it less preferred (e.g. introduce AVOID AT ALL costs for lanes=2&&maxspeed>50)
         list.add(new OneRun(50.004333, 11.600254, 50.044449, 11.543434, 6952, 190));
 
@@ -474,7 +474,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testNeudrossenfeld() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         // choose cycleway (Dreschenauer Straße)
         list.add(new OneRun(49.987132, 11.510496, 50.018839, 11.505024, 3985, 106));
 
@@ -488,7 +488,7 @@ public class RoutingAlgorithmWithOSMIT {
 
     @Test
     public void testDisconnectedAreaAndMultiplePoints() {
-        List<OneRun> list = new ArrayList<OneRun>();
+        List<OneRun> list = new ArrayList<>();
         OneRun oneRun = new OneRun();
         oneRun.add(53.753177, 9.435968, 10, 10);
         oneRun.add(53.751299, 9.386959, 10, 10);