Introduce new routing

OsmAnd-java/src/net/osmand/router/BinaryRoutePlanner.java

@@ -4,10 +4,8 @@ import gnu.trove.map.hash.TLongObjectHashMap;
 
 import java.io.IOException;
 import java.text.MessageFormat;
-import java.util.ArrayList;
 import java.util.Comparator;
 import java.util.Iterator;
-import java.util.List;
 import java.util.PriorityQueue;
 
 import net.osmand.PlatformUtil;
@@ -19,6 +17,9 @@ import org.apache.commons.logging.Log;
 
 public class BinaryRoutePlanner {
 	
+	private static final int TEST_ID = 235108903;
+	private static final boolean TEST_SPECIFIC = false;
+	
 	private static final int REVERSE_WAY_RESTRICTION_ONLY = 1024;
 	/*private*/ static final int STANDARD_ROAD_IN_QUEUE_OVERHEAD = 220;
 	/*private*/ static final int STANDARD_ROAD_VISITED_OVERHEAD = 150;
@@ -68,19 +69,6 @@ public class BinaryRoutePlanner {
 		ctx.visitedSegments = 0;
 		ctx.memoryOverhead  = 1000;
 		ctx.timeToCalculate = System.nanoTime();
-		if(ctx.config.initialDirection != null) {
-			// mark here as positive for further check
-			ctx.firstRoadId = calculateRoutePointId(start.getRoad(), start.getSegmentStart(), true);
-			double plusDir = start.getRoad().directionRoute(start.getSegmentStart(), true);
-			double diff	 = plusDir - ctx.config.initialDirection;
-			if(Math.abs(MapUtils.alignAngleDifference(diff)) <= Math.PI / 3) {
-				ctx.firstRoadDirection = 1;
-			} else if(Math.abs(MapUtils.alignAngleDifference(diff - Math.PI)) <= Math.PI / 3) {
-				ctx.firstRoadDirection = -1;
-			}
-			
-		}
-
 		// Initializing priority queue to visit way segments 
 		Comparator<RouteSegment> nonHeuristicSegmentsComparator = new NonHeuristicSegmentsComparator();
 		PriorityQueue<RouteSegment> graphDirectSegments = new PriorityQueue<RouteSegment>(50, new SegmentsComparator(ctx));
@@ -90,32 +78,17 @@ public class BinaryRoutePlanner {
 		TLongObjectHashMap<RouteSegment> visitedDirectSegments = new TLongObjectHashMap<RouteSegment>();
 		TLongObjectHashMap<RouteSegment> visitedOppositeSegments = new TLongObjectHashMap<RouteSegment>();
 		
-		RouteSegment recalcEndSegment = smartRecalculationEnabled(ctx, visitedOppositeSegments);
-		boolean runRecalculation = false;
-		if(recalcEndSegment != null) {
-			runRecalculation = true;
-			end = recalcEndSegment;
-		}
 		
-		// for start : f(start) = g(start) + h(start) = 0 + h(start) = h(start)
-		float estimatedDistance = (float) estimatedDistance(ctx, ctx.targetX, ctx.targetY, ctx.startX, ctx.startY);
-		end.distanceToEnd = start.distanceToEnd	= estimatedDistance;
-		
-		graphDirectSegments.add(start);
-		graphReverseSegments.add(end);
+		initQueuesWithStartEnd(ctx, start, end, graphDirectSegments, graphReverseSegments);
 		
 		// Extract & analyze segment with min(f(x)) from queue while final segment is not found
-		boolean inverse = false;
-		boolean init = false;
+		boolean forwardSearch = true;
 		
-		PriorityQueue<RouteSegment>  graphSegments;
-		if(inverse) {
-			graphSegments = graphReverseSegments;
-		} else {
-			graphSegments = graphDirectSegments;
-		}
+		PriorityQueue<RouteSegment>  graphSegments = graphDirectSegments;
 		
 		FinalRouteSegment finalSegment = null;
+		boolean onlyBackward = ctx.getPlanRoadDirection() < 0;
+		boolean onlyForward = ctx.getPlanRoadDirection() > 0;
 		while (!graphSegments.isEmpty()) {
 			RouteSegment segment = graphSegments.poll();
 			// use accumulative approach
@@ -132,6 +105,9 @@ public class BinaryRoutePlanner {
 					printMemoryConsumption("Memory occupied after calculation : ");
 				}
 				finalSegment = (FinalRouteSegment) segment;
+				if(TRACE_ROUTING){
+					println("Final segment found");
+				}
 				break;
 			}
 			if (ctx.memoryOverhead > ctx.config.memoryLimitation * 0.95 && RoutingContext.SHOW_GC_SIZE) {
@@ -141,47 +117,45 @@ public class BinaryRoutePlanner {
 				throw new IllegalStateException("There is no enough memory " + ctx.config.memoryLimitation/(1<<20) + " Mb");
 			}
 			ctx.visitedSegments++;
-			if (!inverse) {
+			if (forwardSearch) {
+				boolean doNotAddIntersections = onlyBackward;
 				processRouteSegment(ctx, false, graphDirectSegments, visitedDirectSegments, 
-						segment, visitedOppositeSegments, true);
-				processRouteSegment(ctx, false, graphDirectSegments, visitedDirectSegments, 
-						segment, visitedOppositeSegments, false);
+						segment, visitedOppositeSegments, doNotAddIntersections);
 			} else {
+				boolean doNotAddIntersections = onlyForward;
 				processRouteSegment(ctx, true, graphReverseSegments, visitedOppositeSegments, segment,
-						visitedDirectSegments, true);
-				processRouteSegment(ctx, true, graphReverseSegments, visitedOppositeSegments,segment,
-						visitedDirectSegments, false);
+						visitedDirectSegments, doNotAddIntersections);
 			}
 			updateCalculationProgress(ctx, graphDirectSegments, graphReverseSegments);
-			if(graphReverseSegments.isEmpty()){
+			if(ctx.getPlanRoadDirection() <= 0 && graphReverseSegments.isEmpty()){
 				throw new IllegalArgumentException("Route is not found to selected target point.");
 			}
-			if(graphDirectSegments.isEmpty()){
+			if(ctx.getPlanRoadDirection() >= 0 && graphDirectSegments.isEmpty()){
 				throw new IllegalArgumentException("Route is not found from selected start point.");
 			}
-			if(runRecalculation) {
-				// nothing to do
-				inverse = false;
-			} else if (!init) {
-				inverse = !inverse;
-				init = true;
-			} else if (ctx.planRouteIn2Directions()) {
-				inverse = nonHeuristicSegmentsComparator.compare(graphDirectSegments.peek(), graphReverseSegments.peek()) > 0;
+			if (ctx.planRouteIn2Directions()) {
+				forwardSearch = !(nonHeuristicSegmentsComparator.compare(graphDirectSegments.peek(), graphReverseSegments.peek()) > 0);
 				if (graphDirectSegments.size() * 1.3 > graphReverseSegments.size()) {
-					inverse = true;
+					forwardSearch = false;
 				} else if (graphDirectSegments.size() < 1.3 * graphReverseSegments.size()) {
-					inverse = false;
+					forwardSearch = true;
 				}
 			} else {
 				// different strategy : use onedirectional graph
-				inverse = ctx.getPlanRoadDirection() < 0;
+				forwardSearch = onlyForward;
+				if(onlyBackward && !graphDirectSegments.isEmpty()) {
+					forwardSearch = true;
+				}
+				if(onlyForward && !graphReverseSegments.isEmpty()) {
+					forwardSearch = false;
+				}
 			}
-			if (inverse) {
-				graphSegments = graphReverseSegments;
-			} else {
+			
+			if (forwardSearch) {
 				graphSegments = graphDirectSegments;
+			} else {
+				graphSegments = graphReverseSegments;
 			}
-
 			// check if interrupted
 			if(ctx.calculationProgress != null && ctx.calculationProgress.isCancelled) {
 				throw new InterruptedException("Route calculation interrupted");
@@ -192,6 +166,47 @@ public class BinaryRoutePlanner {
 	}
 
 
+	private void initQueuesWithStartEnd(final RoutingContext ctx, RouteSegment start, RouteSegment end,
+			PriorityQueue<RouteSegment> graphDirectSegments, PriorityQueue<RouteSegment> graphReverseSegments) {
+		RouteSegment startPos = start.initRouteSegment(true);
+		RouteSegment startNeg = start.initRouteSegment(false);
+		RouteSegment endPos = end.initRouteSegment(true);
+		RouteSegment endNeg = end.initRouteSegment(false);
+		// for start : f(start) = g(start) + h(start) = 0 + h(start) = h(start)
+		if(ctx.config.initialDirection != null) {
+			// mark here as positive for further check
+			double plusDir = start.getRoad().directionRoute(start.getSegmentStart(), true);
+			double diff	 = plusDir - ctx.config.initialDirection;
+			if(Math.abs(MapUtils.alignAngleDifference(diff)) <= Math.PI / 3) {
+				if(startNeg != null) {
+					startNeg.distanceFromStart += 500;
+				}
+			} else if(Math.abs(MapUtils.alignAngleDifference(diff - Math.PI)) <= Math.PI / 3) {
+				if(startPos != null) {
+					startPos.distanceFromStart += 500;
+				}
+			}
+		}
+		float estimatedDistance = (float) estimatedDistance(ctx, ctx.targetX, ctx.targetY, ctx.startX, ctx.startY);
+		if(startPos != null) {
+			startPos.distanceToEnd = estimatedDistance;
+			graphDirectSegments.add(startPos);
+		}
+		if(startNeg != null) {
+			startNeg.distanceToEnd = estimatedDistance;
+			graphDirectSegments.add(startNeg);
+		}
+		if(endPos != null) {
+			endPos.distanceToEnd = estimatedDistance;
+			graphReverseSegments.add(endPos);
+		}
+		if(endNeg != null) {
+			endNeg.distanceToEnd = estimatedDistance;
+			graphReverseSegments.add(endNeg);
+		}
+	}
+
+
 	private void printMemoryConsumption( String string) {
 		long h1 = RoutingContext.runGCUsedMemory();
 		float mb = (1 << 20);
@@ -216,42 +231,6 @@ public class BinaryRoutePlanner {
 	}
 
 
-	private RouteSegment smartRecalculationEnabled(final RoutingContext ctx, TLongObjectHashMap<RouteSegment> visitedOppositeSegments) {
-		boolean runRecalculation = ctx.previouslyCalculatedRoute != null && ctx.previouslyCalculatedRoute.size() > 0
-				&& ctx.config.recalculateDistance != 0;
-		if (runRecalculation) {
-			RouteSegment previous = null;
-			List<RouteSegmentResult> rlist = new ArrayList<RouteSegmentResult>();
-			float distanceThreshold = ctx.config.recalculateDistance;
-			float threshold = 0;
-			for(RouteSegmentResult rr : ctx.previouslyCalculatedRoute) {
-				threshold += rr.getDistance();
-				if(threshold > distanceThreshold) {
-					rlist.add(rr);
-				}
-			}
-			runRecalculation = rlist.size() > 0;
-			if (rlist.size() > 0) {
-				for (RouteSegmentResult rr : rlist) {
-					RouteSegment segment = new RouteSegment(rr.getObject(), rr.getEndPointIndex());
-					if (previous != null) {
-						previous.setParentRoute(segment);
-						previous.setParentSegmentEnd(rr.getStartPointIndex());
-						boolean positive = rr.getStartPointIndex() < rr.getEndPointIndex();
-						long t = calculateRoutePointId(rr.getObject(), positive ? rr.getEndPointIndex() - 1 : rr.getEndPointIndex(),
-								positive);
-						visitedOppositeSegments.put(t, segment);
-					}
-					previous = segment;
-				}
-				return previous;
-			}
-		}
-		return null;
-	}
-	
-	
-
 	private void printRoad(String prefix, RouteSegment segment) {
 		String pr;
 		if(segment.parentRoute != null){
@@ -259,7 +238,7 @@ public class BinaryRoutePlanner {
 		} else {
 			pr = "";
 		}
-		println(prefix  +"" + segment.road + " ind=" + segment.getSegmentStart() + 
+		println(prefix  +"" + segment.road + " dir="+segment.getDirectionAssigned()+" ind=" + segment.getSegmentStart() + 
 				" ds=" + ((float)segment.distanceFromStart) + " es="+((float)segment.distanceToEnd) + pr);
 	}
 
@@ -269,16 +248,15 @@ public class BinaryRoutePlanner {
 		return (float) (distance / ctx.getRouter().getMaxDefaultSpeed());
 	}
 	
-	protected static float h(RoutingContext ctx, int begX, int begY, int endX, int endY, 
-			RouteSegment next) {
+	protected static float h(RoutingContext ctx, int begX, int begY, int endX, int endY) {
 		double distToFinalPoint = squareRootDist(begX, begY,  endX, endY);
+		double result = distToFinalPoint / ctx.getRouter().getMaxDefaultSpeed();
 		if(ctx.precalculatedRouteDirection != null){
 			float te = ctx.precalculatedRouteDirection.timeEstimate(begX, begY,  endX, endY);
 			if(te > 0) {
 				return te;
 			}
 		}
-		double result = distToFinalPoint / ctx.getRouter().getMaxDefaultSpeed();
 		return (float) result; 
 	}
 	
@@ -295,7 +273,9 @@ public class BinaryRoutePlanner {
 	
 	public void printDebugMemoryInformation(RoutingContext ctx, PriorityQueue<RouteSegment> graphDirectSegments, PriorityQueue<RouteSegment> graphReverseSegments, 
 			TLongObjectHashMap<RouteSegment> visitedDirectSegments,TLongObjectHashMap<RouteSegment> visitedOppositeSegments) {
-		printInfo("Time to calculate : " + (System.nanoTime() - ctx.timeToCalculate) / 1e6 + ", time to load : " + ctx.timeToLoad / 1e6 + ", time to load headers : " + ctx.timeToLoadHeaders / 1e6);
+		printInfo("Time to calculate : " + (System.nanoTime() - ctx.timeToCalculate) / 1e6 + 
+				", time to load : " + ctx.timeToLoad / 1e6 + ", time to load headers : " + ctx.timeToLoadHeaders / 1e6 + 
+				", time to calc dev : " + ctx.timeNanoToCalcDeviation/ 1e6);
 		int maxLoadedTiles = Math.max(ctx.maxLoadedTiles, ctx.getCurrentlyLoadedTiles());
 		printInfo("Current loaded tiles : " + ctx.getCurrentlyLoadedTiles() + ", maximum loaded tiles " + maxLoadedTiles);
 		printInfo("Loaded tiles " + ctx.loadedTiles + " (distinct "+ctx.distinctLoadedTiles+ "), unloaded tiles " + ctx.unloadedTiles + 
@@ -305,51 +285,57 @@ public class BinaryRoutePlanner {
 		if (graphDirectSegments != null && graphReverseSegments != null) {
 			printInfo("Priority queues sizes : " + graphDirectSegments.size() + "/" + graphReverseSegments.size());
 		}
+		printInfo("Already visited " + alreadyVisited);
+		alreadyVisited = 0;
 		if (visitedDirectSegments != null && visitedOppositeSegments != null) {
 			printInfo("Visited interval sizes: " + visitedDirectSegments.size() + "/" + visitedOppositeSegments.size());
 		}
 		
 	}
+	private static int alreadyVisited = 0;
 	
 	
+	@SuppressWarnings("unused")
 	private void processRouteSegment(final RoutingContext ctx, boolean reverseWaySearch,
 			PriorityQueue<RouteSegment> graphSegments, TLongObjectHashMap<RouteSegment> visitedSegments, 
-            RouteSegment segment, TLongObjectHashMap<RouteSegment> oppositeSegments, boolean direction) throws IOException {
+            RouteSegment segment, TLongObjectHashMap<RouteSegment> oppositeSegments, boolean doNotAddIntersections) throws IOException {
 		final RouteDataObject road = segment.road;
-		boolean initDirectionAllowed = checkIfInitialMovementAllowedOnSegment(ctx, reverseWaySearch, visitedSegments, segment, direction, road);
-		boolean directionAllowed = initDirectionAllowed; 
+		boolean initDirectionAllowed = checkIfInitialMovementAllowedOnSegment(ctx, reverseWaySearch, visitedSegments, segment, road);
+		if(TEST_SPECIFIC && road.getId() == TEST_ID ) {
+			printRoad(" ! " + +segment.distanceFromStart + " ", segment);
+		}
+		boolean directionAllowed = initDirectionAllowed;
+		if(!directionAllowed) {
+			alreadyVisited ++;
+			if(TRACE_ROUTING) {
+				println("  >> Already visited");
+			}
+			return;
+		}
 		// Go through all point of the way and find ways to continue
 		// ! Actually there is small bug when there is restriction to move forward on the way (it doesn't take into account)
 		float obstaclesTime = 0;
-		if(segment.getParentRoute() != null && directionAllowed) {
-			obstaclesTime = (float) ctx.getRouter().calculateTurnTime(segment, direction? segment.getRoad().getPointsLength() - 1 : 0,  
-					segment.getParentRoute(), segment.getParentSegmentEnd());
-		}
-		if(ctx.firstRoadId == calculateRoutePointId(road, segment.getSegmentStart(), true) ) {
-			if(direction && ctx.firstRoadDirection < 0) {
-				obstaclesTime += 500;
-			} else if(!direction && ctx.firstRoadDirection > 0) {
-				obstaclesTime += 500;
-			}
-		}
 		float segmentDist = 0;
 		// +/- diff from middle point
-		int segmentEnd = segment.getSegmentStart();
+		short segmentPoint = segment.getSegmentStart();
+		boolean dir = segment.isPositive();
 		while (directionAllowed) {
-			int prevInd = segmentEnd;
-			if(direction) {
-				segmentEnd ++;
+			// mark previous interval as visited and move to next intersection
+			short prevInd = segmentPoint;
+			if(dir) {
+				segmentPoint ++;
 			} else {
-				segmentEnd --;
+				segmentPoint --;
 			}
-			if (segmentEnd < 0 || segmentEnd >= road.getPointsLength()) {
+			if (segmentPoint < 0 || segmentPoint >= road.getPointsLength()) {
 				directionAllowed = false;
 				continue;
 			}
-			final int intervalId = direction ? segmentEnd - 1 : segmentEnd;
-			visitedSegments.put(calculateRoutePointId(road, intervalId, direction), segment);
-			final int x = road.getPoint31XTile(segmentEnd);
-			final int y = road.getPoint31YTile(segmentEnd);
+			// store <segment> in order to not have unique <segment, direction> in visitedSegments 
+			visitedSegments.put(calculateRoutePointId(segment.getRoad(), segment.isPositive() ? segmentPoint - 1 : segmentPoint, 
+					segment.isPositive()), segment);
+			final int x = road.getPoint31XTile(segmentPoint);
+			final int y = road.getPoint31YTile(segmentPoint);
 			final int prevx = road.getPoint31XTile(prevInd);
 			final int prevy = road.getPoint31YTile(prevInd);
 			if(x == prevx && y == prevy) {
@@ -360,102 +346,93 @@ public class BinaryRoutePlanner {
 			segmentDist  += squareRootDist(x, y,  prevx, prevy);
 			
 			// 2.1 calculate possible obstacle plus time
-			double obstacle = ctx.getRouter().defineRoutingObstacle(road, segmentEnd);
+			double obstacle = ctx.getRouter().defineRoutingObstacle(road, segmentPoint);
 			if (obstacle < 0) {
 				directionAllowed = false;
 				continue;
 			}
 			obstaclesTime += obstacle;
 			
-			boolean alreadyVisited = checkIfOppositieSegmentWasVisited(ctx, reverseWaySearch, graphSegments, segment, oppositeSegments, road,
-					segmentEnd, direction, intervalId, segmentDist, obstaclesTime);
+			boolean alreadyVisited = checkIfOppositieSegmentWasVisited(ctx, reverseWaySearch, graphSegments, segment, oppositeSegments, 
+					segmentPoint, segmentDist, obstaclesTime);
 			if (alreadyVisited) {
 				directionAllowed = false;
 				continue;
 			}
-			
+			// correct way of handling precalculatedRouteDirection 
+			if(ctx.precalculatedRouteDirection != null) {
+//				long nt = System.nanoTime();
+//				float devDistance = ctx.precalculatedRouteDirection.getDeviationDistance(x, y);
+//				// 1. linear method
+//				// segmentDist = segmentDist * (1 + ctx.precalculatedRouteDirection.getDeviationDistance(x, y) / ctx.config.DEVIATION_RADIUS);
+//				// 2. exponential method
+//				segmentDist = segmentDist * (float) Math.pow(1.5, devDistance / 500);
+//				ctx.timeNanoToCalcDeviation += (System.nanoTime() - nt);
+			}
 			// could be expensive calculation
 			// 3. get intersected ways
 			final RouteSegment roadNext = ctx.loadRouteSegment(x, y, ctx.config.memoryLimitation - ctx.memoryOverhead);
-			if(roadNext != null && 
-					!((roadNext == segment || roadNext.road.id == road.id) && roadNext.next == null)) {
-				// check if there are outgoing connections in that case we need to stop processing
-				boolean outgoingConnections = false;
-				RouteSegment r = roadNext;
-				while(r != null && !outgoingConnections) {
-					if(r.road.id != road.id || r.getSegmentStart() != 0 || r.road.getOneway() != 1){
-						outgoingConnections = true;
-					}
-					r = r.next;
-				}
-				if (outgoingConnections) {
-					directionAllowed = false;
-				}
-				
-				float distStartObstacles = segment.distanceFromStart + calculateTimeWithObstacles(ctx, road, segmentDist , obstaclesTime);
-				processIntersections(ctx, graphSegments, visitedSegments, 
-						distStartObstacles, segment, segmentEnd, 
-						roadNext, reverseWaySearch, outgoingConnections);
+			float distStartObstacles = segment.distanceFromStart + calculateTimeWithObstacles(ctx, road, segmentDist , obstaclesTime);
+			
+			// We don't check if there are outgoing connections
+			boolean processFurther = processIntersections(ctx, graphSegments, visitedSegments, distStartObstacles,
+					segment, segmentPoint, roadNext, reverseWaySearch, doNotAddIntersections);
+			if (!processFurther) {
+				directionAllowed = false;
+				continue;
 			}
 		}
 		if(initDirectionAllowed && ctx.visitor != null){
-			ctx.visitor.visitSegment(segment, segmentEnd, true);
+			ctx.visitor.visitSegment(segment, segmentPoint, true);
 		}
 	}
 
 	private boolean checkIfInitialMovementAllowedOnSegment(final RoutingContext ctx, boolean reverseWaySearch,
-			TLongObjectHashMap<RouteSegment> visitedSegments, RouteSegment segment, boolean direction, final RouteDataObject road
-			) {
+			TLongObjectHashMap<RouteSegment> visitedSegments, RouteSegment segment, final RouteDataObject road) {
 		boolean directionAllowed;
-		final int middle = segment.getSegmentStart();
 		int oneway = ctx.getRouter().isOneWay(road);
 		// use positive direction as agreed
 		if (!reverseWaySearch) {
-			if(direction){
+			if(segment.isPositive()){
 				directionAllowed = oneway >= 0;
 			} else {
 				directionAllowed = oneway <= 0;
 			}
 		} else {
-			if(direction){
+			if(segment.isPositive()){
 				directionAllowed = oneway <= 0;
 			} else {
 				directionAllowed = oneway >= 0;
 			}
 		}
-		if(direction) {
-			if(middle == road.getPointsLength() - 1 ||
-					visitedSegments.containsKey(calculateRoutePointId(road, middle, true)) || 
-					segment.getAllowedDirection() == -1) {
-				directionAllowed = false;
-			}	
-		} else {
-			if(middle == 0 || visitedSegments.containsKey(calculateRoutePointId(road, middle - 1, false)) || 
-					segment.getAllowedDirection() == 1) {
-				directionAllowed = false;
-			}	
+		
+		if(directionAllowed && visitedSegments.containsKey(calculateRoutePointId(segment, segment.isPositive()))) {
+			directionAllowed = false;
 		}
 		return directionAllowed;
 	}
 
 	private boolean checkIfOppositieSegmentWasVisited(final RoutingContext ctx, boolean reverseWaySearch,
 			PriorityQueue<RouteSegment> graphSegments, RouteSegment segment, TLongObjectHashMap<RouteSegment> oppositeSegments,
-			final RouteDataObject road, int segmentEnd, boolean positive, int intervalId, float segmentDist, float obstaclesTime) {
-		long opp = calculateRoutePointId(road, intervalId, !positive);
+			int segmentPoint, float segmentDist, float obstaclesTime) {
+		RouteDataObject road = segment.getRoad();
+		long opp = calculateRoutePointId(road, segment.isPositive() ? segmentPoint - 1 : segmentPoint, !segment.isPositive());
 		if (oppositeSegments.containsKey(opp)) {
 			RouteSegment opposite = oppositeSegments.get(opp);
-			if (opposite.getSegmentStart() == segmentEnd) {
-				FinalRouteSegment frs = new FinalRouteSegment(road, segment.getSegmentStart());
-				float distStartObstacles = segment.distanceFromStart + calculateTimeWithObstacles(ctx, road, segmentDist , obstaclesTime);
-				frs.setParentRoute(segment.getParentRoute());
-				frs.setParentSegmentEnd(segment.getParentSegmentEnd());
-				frs.reverseWaySearch = reverseWaySearch;
-				frs.distanceFromStart = opposite.distanceFromStart + distStartObstacles;
-				frs.distanceToEnd = 0;
-				frs.opposite = opposite;
-				graphSegments.add(frs);
-				return true;
+			FinalRouteSegment frs = new FinalRouteSegment(road, segmentPoint);
+			float distStartObstacles = segment.distanceFromStart
+					+ calculateTimeWithObstacles(ctx, road, segmentDist, obstaclesTime);
+			frs.setParentRoute(segment);
+			frs.setParentSegmentEnd(segmentPoint);
+			frs.reverseWaySearch = reverseWaySearch;
+			frs.distanceFromStart = opposite.distanceFromStart + distStartObstacles;
+			frs.distanceToEnd = 0;
+			frs.opposite = opposite;
+			graphSegments.add(frs);
+			if(TRACE_ROUTING){
+				printRoad("  >> Final segment : ", frs);
 			}
+			return true;
 		}
 		return false;
 	}
@@ -475,8 +452,23 @@ public class BinaryRoutePlanner {
 	}
 
 	private long calculateRoutePointId(final RouteDataObject road, int intervalId, boolean positive) {
+		if(intervalId < 0) {
+			// should be assert
+			throw new IllegalStateException("Assert failed");
+		}
 		return (road.getId() << ROUTE_POINTS) + (intervalId << 1) + (positive ? 1 : 0);
 	}
+	
+	private long calculateRoutePointId(RouteSegment segm, boolean direction) {
+		if(segm.getSegmentStart() == 0 && !direction) {
+			throw new IllegalStateException("Assert failed");	
+		}
+		if(segm.getSegmentStart() == segm.getRoad().getPointsLength() - 1 && direction) {
+			throw new IllegalStateException("Assert failed");	
+		}
+		return calculateRoutePointId(segm.getRoad(),
+				direction ? segm.getSegmentStart() : segm.getSegmentStart() - 1, direction);
+	}
 
 
 	private boolean proccessRestrictions(RoutingContext ctx, RouteDataObject road, RouteSegment inputNext, boolean reverseWay) {
@@ -557,20 +549,27 @@ public class BinaryRoutePlanner {
 	
 
 
-	private void processIntersections(RoutingContext ctx, PriorityQueue<RouteSegment> graphSegments,
-			TLongObjectHashMap<RouteSegment> visitedSegments,  float  distFromStart,  
-			RouteSegment segment, int segmentEnd,  
-			RouteSegment inputNext, boolean reverseWaySearch, 
-			boolean addSameRoadFutureDirection) {
-		byte searchDirection = reverseWaySearch ? (byte)-1 : (byte)1;
-		boolean thereAreRestrictions = proccessRestrictions(ctx, segment.road, inputNext, reverseWaySearch);
+	private boolean processIntersections(RoutingContext ctx, PriorityQueue<RouteSegment> graphSegments,
+			TLongObjectHashMap<RouteSegment> visitedSegments,  float  distFromStart, RouteSegment segment,
+			short segmentPoint, RouteSegment inputNext, boolean reverseWaySearch, boolean doNotAddIntersections) {
+		boolean thereAreRestrictions ;
+		boolean processFurther = true;
 		Iterator<RouteSegment> nextIterator = null;
-		if (thereAreRestrictions) {
-			nextIterator = ctx.segmentsToVisitPrescripted.iterator();
-			if(TRACE_ROUTING){
-				println("  >> There are restrictions");
+		if(inputNext != null && inputNext.getRoad().getId() == segment.getRoad().getId() && inputNext.next == null) {
+			thereAreRestrictions = false;
+		} else {
+			thereAreRestrictions = proccessRestrictions(ctx, segment.road, inputNext, reverseWaySearch);
+			if (thereAreRestrictions) {
+				nextIterator = ctx.segmentsToVisitPrescripted.iterator();
+				if (TRACE_ROUTING) {
+					println("  >> There are restrictions");
+				}
 			}
 		}
+		int targetEndX = reverseWaySearch ? ctx.startX : ctx.targetX;
+		int targetEndY = reverseWaySearch ? ctx.startY : ctx.targetY;
+		float distanceToEnd = h(ctx, segment.getRoad().getPoint31XTile(segmentPoint), segment.getRoad()
+				.getPoint31YTile(segmentPoint), targetEndX, targetEndY);
 		// Calculate possible ways to put into priority queue
 		RouteSegment next = inputNext;
 		boolean hasNext = nextIterator == null || nextIterator.hasNext();
@@ -578,69 +577,33 @@ public class BinaryRoutePlanner {
 			if (nextIterator != null) {
 				next = nextIterator.next();
 			}
-			boolean nextPlusNotAllowed = (next.getSegmentStart() == next.road.getPointsLength() - 1) ||
-					visitedSegments.containsKey(calculateRoutePointId(next.road, next.getSegmentStart(), true));
-			boolean nextMinusNotAllowed = (next.getSegmentStart() == 0) ||
-					visitedSegments.containsKey(calculateRoutePointId(next.road, next.getSegmentStart() - 1, false));
-			boolean sameRoadFutureDirection = next.road.id == segment.road.id && next.getSegmentStart() == segmentEnd;
-			// road.id could be equal on roundabout, but we should accept them
-			boolean alreadyVisited = nextPlusNotAllowed && nextMinusNotAllowed;
-			boolean skipRoad = sameRoadFutureDirection && !addSameRoadFutureDirection;
-			if (!alreadyVisited && !skipRoad) {
-				int targetEndX = reverseWaySearch? ctx.startX : ctx.targetX;
-				int targetEndY = reverseWaySearch? ctx.startY : ctx.targetY;
-				float distanceToEnd = h(ctx, segment.getRoad().getPoint31XTile(segmentEnd),
-						segment.getRoad().getPoint31YTile(segmentEnd), targetEndX, targetEndY, next);
-				// assigned to wrong direction
-				if(next.getDirectionAssigned() == -searchDirection){
-					next = new RouteSegment(next.getRoad(), next.getSegmentStart());
+			if (next.getSegmentStart() == segmentPoint && next.getRoad().getId() == segment.getRoad().id) {
+				// find segment itself  
+				// (and process it as other with small exception that we don't add to graph segments and process immediately)
+				RouteSegment itself = next.initRouteSegment(segment.isPositive());
+				if(itself == null) {
+					// do nothing
+				} else if (itself.getParentRoute() == null
+						|| ctx.roadPriorityComparator(itself.distanceFromStart, itself.distanceToEnd, distFromStart,
+								distanceToEnd) > 0) {
+					itself.distanceFromStart = distFromStart;
+					itself.distanceToEnd = distanceToEnd;
+					itself.setParentRoute(segment);
+					itself.setParentSegmentEnd(segmentPoint);
+				} else {
+					// we already processed that segment earlier or it is in graph segments
+					// and we had better results (so we shouldn't process)
+					processFurther = false;
 				}
-				 
-				if (next.getParentRoute() == null
-						|| ctx.roadPriorityComparator(next.distanceFromStart, next.distanceToEnd, distFromStart, distanceToEnd) > 0) {
-					if (next.getParentRoute() != null) {
-						if (!graphSegments.remove(next)) {
-							throw new IllegalStateException("Should be handled by direction flag");
-						} 
-					} 
-					next.assignDirection(searchDirection);
-					next.distanceFromStart = distFromStart;
-					next.distanceToEnd = distanceToEnd;
-					if(sameRoadFutureDirection) {
-						next.setAllowedDirection((byte) (segment.getSegmentStart() < next.getSegmentStart() ? 1 : - 1));
-					}
-					if(TRACE_ROUTING) {
-						printRoad("  >>", next);
-					}
-					// put additional information to recover whole route after
-					next.setParentRoute(segment);
-					next.setParentSegmentEnd(segmentEnd);
-					
-					graphSegments.add(next);
-				}
-				if (ctx.visitor != null) {
-//					ctx.visitor.visitSegment(next, false);
-				}
-			} else if(!sameRoadFutureDirection){
-				// the segment was already visited! We need to follow better route if it exists
-				// that is very strange situation and almost exception (it can happen when we underestimate distnceToEnd)
-				if (next.getDirectionAssigned() == searchDirection && 
-						distFromStart < next.distanceFromStart && next.road.id != segment.road.id) {
-					if(ctx.config.heuristicCoefficient <= 1) {
-						throw new IllegalStateException("distance from start " + distFromStart + " < " +next.distanceFromStart); 
-					}
-					// That code is incorrect (when segment is processed itself,
-					// then it tries to make wrong u-turn) -
-					// this situation should be very carefully checked in future (seems to be fixed)
-					next.distanceFromStart = distFromStart;
-					next.setParentRoute(segment);
-					next.setParentSegmentEnd(segmentEnd);
-					if (ctx.visitor != null) {
-//						ctx.visitor.visitSegment(next, false);
-					}
-				}
-			}
+			} else if(!doNotAddIntersections) {
+				RouteSegment nextPos = next.initRouteSegment(true);
+				RouteSegment nextNeg = next.initRouteSegment(false);
+				processOneRoadIntersection(ctx, graphSegments, visitedSegments, distFromStart, distanceToEnd, segment, segmentPoint,
+						nextPos);
+				processOneRoadIntersection(ctx, graphSegments, visitedSegments, distFromStart, distanceToEnd, segment, segmentPoint,
+						nextNeg);
 
+			}
 			// iterate to next road
 			if (nextIterator == null) {
 				next = next.next;
@@ -649,6 +612,61 @@ public class BinaryRoutePlanner {
 				hasNext = nextIterator.hasNext();
 			}
 		}
+		return processFurther;
+	}
+
+
+	@SuppressWarnings("unused")
+	private void processOneRoadIntersection(RoutingContext ctx, PriorityQueue<RouteSegment> graphSegments,
+			TLongObjectHashMap<RouteSegment> visitedSegments, float distFromStart, float distanceToEnd,  RouteSegment segment,
+			int segmentPoint, RouteSegment next) {
+		if (next != null) {
+			float obstaclesTime = (float) ctx.getRouter().calculateTurnTime(next, next.isPositive()? 
+					next.getRoad().getPointsLength() - 1 : 0,  
+					segment, segmentPoint);
+			distFromStart += obstaclesTime;
+			if(TEST_SPECIFIC && next.road.getId() == TEST_ID) {
+				printRoad(" !? distFromStart=" + +distFromStart + " from " + segment.getRoad().getId() + 
+						" dir=" + segment.getDirectionAssigned() + 
+						" distToEnd=" + distanceToEnd +
+						" segmentPoint="+ segmentPoint + " -- ", next);
+			}
+			if (!visitedSegments.containsKey(calculateRoutePointId(next, next.isPositive()))) {
+				if (next.getParentRoute() == null
+						|| ctx.roadPriorityComparator(next.distanceFromStart, next.distanceToEnd,
+								distFromStart, distanceToEnd) > 0) {
+					next.distanceFromStart = distFromStart;
+					next.distanceToEnd = distanceToEnd;
+					if (TRACE_ROUTING) {
+						printRoad("  >>", next);
+					}
+					// put additional information to recover whole route after
+					next.setParentRoute(segment);
+					next.setParentSegmentEnd(segmentPoint);
+					graphSegments.add(next);
+				}
+			} else {
+				// the segment was already visited! We need to follow better route if it exists
+				// that is very exceptional situation and almost exception, it can happen 
+				// 1. when we underestimate distnceToEnd - wrong h()
+				// 2. because we process not small segments but the whole road, it could be that 
+				// deviation from the road is faster than following the whole road itself!
+				if (distFromStart < next.distanceFromStart) {
+					if (ctx.config.heuristicCoefficient <= 1) {
+						System.err.println("! Alert distance from start " + distFromStart + " < "
+								+ next.distanceFromStart + " id="+next.road.id);
+					}
+					// A: we can't change parent route just here, because we need to update visitedSegments
+					// presumably we can do visitedSegments.put(calculateRoutePointId(next), next);
+//					next.distanceFromStart = distFromStart;
+//					next.setParentRoute(segment);
+//					next.setParentSegmentEnd(segmentPoint);
+					if (ctx.visitor != null) {
+						// ctx.visitor.visitSegment(next, false);
+					}
+				}
+			}
+		}
 	}
 	
 
@@ -670,6 +688,7 @@ public class BinaryRoutePlanner {
 		final RouteDataObject road;
 		// needed to store intersection of routes
 		RouteSegment next = null;
+		RouteSegment oppositeDirection = null;
 		
 		// search context (needed for searching route)
 		// Initially it should be null (!) because it checks was it segment visited before
@@ -677,8 +696,6 @@ public class BinaryRoutePlanner {
 		short parentSegmentEnd = 0;
 		// 1 - positive , -1 - negative, 0 not assigned
 		byte directionAssgn = 0;
-		// 1 - only positive allowed, -1 - only negative allowed
-		byte allowedDirection = 0;
 		
 		// distance measured in time (seconds)
 		float distanceFromStart = 0;
@@ -689,6 +706,39 @@ public class BinaryRoutePlanner {
 			this.segStart = (short) segmentStart;
 		}
 		
+		public RouteSegment initRouteSegment(boolean positiveDirection) {
+			if(segStart == 0 && !positiveDirection) {
+				return null;
+			}
+			if(segStart == road.getPointsLength() - 1 && positiveDirection) {
+				return null;
+			}
+			RouteSegment rs = this;
+			if(directionAssgn == 0) {
+				rs.directionAssgn = (byte) (positiveDirection ? 1 : -1);
+			} else {
+				if(positiveDirection != (directionAssgn == 1)) {
+					if(oppositeDirection == null) {
+						oppositeDirection = new RouteSegment(road, segStart);
+						oppositeDirection.directionAssgn = (byte) (positiveDirection ? 1 : -1);
+					}
+					if ((oppositeDirection.directionAssgn == 1) != positiveDirection) {
+						throw new IllegalStateException();
+					}
+					rs = oppositeDirection;
+				}
+			}
+			return rs;
+		}
+		
+		public int getSegmentIntervalInd() {
+			boolean positive = directionAssgn >= 0;
+			if(!positive) {
+				return segStart - 1;
+			}
+			return segStart;
+		}
+		
 		public byte getDirectionAssigned(){
 			return directionAssgn;
 		}
@@ -697,6 +747,10 @@ public class BinaryRoutePlanner {
 			return parentRoute;
 		}
 		
+		public boolean isPositive() {
+			return directionAssgn == 1;
+		}
+		
 		public void setParentRoute(RouteSegment parentRoute) {
 			this.parentRoute = parentRoute;
 		}
@@ -705,15 +759,6 @@ public class BinaryRoutePlanner {
 			directionAssgn = b;
 		}
 		
-		public byte getAllowedDirection() {
-			return allowedDirection;
-		}
-		
-		public void setAllowedDirection(byte allowedDirection) {
-			this.allowedDirection = allowedDirection;
-		}
-		
-		
 		public void setParentSegmentEnd(int parentSegmentEnd) {
 			this.parentSegmentEnd = (short) parentSegmentEnd;
 		}
@@ -726,7 +771,7 @@ public class BinaryRoutePlanner {
 			return next;
 		}
 		
-		public int getSegmentStart() {
+		public short getSegmentStart() {
 			return segStart;
 		}
 		

OsmAnd-java/src/net/osmand/router/PrecalculatedRouteDirection.java

@@ -4,11 +4,12 @@ import gnu.trove.list.array.TIntArrayList;
 
 import java.util.ArrayList;
 import java.util.List;
-import java.util.Map;
-import java.util.TreeMap;
 
 import net.osmand.binary.RouteDataObject;
-import net.osmand.data.DataTileManager;
+import net.osmand.data.QuadPoint;
+import net.osmand.data.QuadRect;
+import net.osmand.data.QuadTree;
+import net.osmand.util.MapUtils;
 
 public class PrecalculatedRouteDirection {
 	
@@ -16,16 +17,17 @@ public class PrecalculatedRouteDirection {
 	private TIntArrayList pointsY = new TIntArrayList();
 	private float speed;
 	private float[] tms;
-	private static final int SHIFT = (1 << (31 - 18));
-	private static final int[] SHIFTS = new int[]{1 << (31 - 17), 1 << (31 - 15), 1 << (31 - 13), 1 << (31 - 12), 
-		1 << (31 - 11), 1 << (31 - 9)};
+	private static final int SHIFT = (1 << (31 - 17));
+	private static final int[] SHIFTS = new int[]{1 << (31 - 15), 1 << (31 - 13), 1 << (31 - 12), 
+		1 << (31 - 11), 1 << (31 - 7)};
 	
 	private List<Integer> cachedS = new ArrayList<Integer>();
-	private float[] ct1 = new float[2];
-	private float[] ct2 = new float[2];
 	
-	private Map<Long, Integer> prereg = new TreeMap<Long, Integer>();
-	private DataTileManager<Integer> indexedPoints = new DataTileManager<Integer>(17);
+	private long startPoint = 0;
+	private long endPoint = 0;
+//	private DataTileManager<Integer> indexedPoints = new DataTileManager<Integer>(17);
+	QuadTree<Integer> quadTree = new QuadTree<Integer>(new QuadRect(0, 0, Integer.MAX_VALUE, Integer.MAX_VALUE),
+			8, 0.55f); 
 	
 	private PrecalculatedRouteDirection(List<RouteSegmentResult> ls, float avgSpeed) {
 		this.speed = avgSpeed;
@@ -38,7 +40,8 @@ public class PrecalculatedRouteDirection {
 		for (int i = s1; i <= s2; i++) {
 			pointsX.add(parent.pointsX.get(i));
 			pointsY.add(parent.pointsY.get(i));
-			indexedPoints.registerObjectXY(parent.pointsX.get(i), parent.pointsY.get(i), pointsX.size() - 1);
+//			indexedPoints.registerObjectXY(parent.pointsX.get(i), parent.pointsY.get(i), pointsX.size() - 1);
+			quadTree.insert(pointsX.size() - 1, parent.pointsX.get(i), parent.pointsY.get(i));
 			tms[i - s1] = parent.tms[i] - parent.tms[s2];
 		}
 	}
@@ -52,10 +55,10 @@ public class PrecalculatedRouteDirection {
 				break;
 			}
 		}
-		int endi = ls.size() - 1;
+		int endi = ls.size();
 		d = cutoffDistance;
-		for (; endi >= 0; endi--) {
-			d -= ls.get(endi).getDistance();
+		for (; endi > 0; endi--) {
+			d -= ls.get(endi - 1).getDistance();
 			if (d < 0) {
 				break;
 			}
@@ -74,17 +77,22 @@ public class PrecalculatedRouteDirection {
 			boolean plus = s.getStartPointIndex() < s.getEndPointIndex();
 			int i = s.getStartPointIndex();
 			RouteDataObject obj = s.getObject();
-			float spd = s.getSegmentSpeed();
+			float routeSpd = s.getDistance()/ s.getRoutingTime() ;
 			while (true) {
 				int iprev = i;
 				i = plus? i + 1 : i -1;
-				float tm = (float) (BinaryRoutePlanner.squareRootDist(obj.getPoint31XTile(iprev), obj.getPoint31YTile(iprev), 
-						obj.getPoint31XTile(i), obj.getPoint31YTile(i)) / spd);
+				// MapUtils.measuredDist31 vs BinaryRoutePlanner.squareRootDist
+				// use measuredDist31 because we use precise s.getDistance() to calculate routeSpd
+				float dist = (float) MapUtils.measuredDist31(obj.getPoint31XTile(iprev), obj.getPoint31YTile(iprev), 
+						obj.getPoint31XTile(i), obj.getPoint31YTile(i));
+				float tm = dist / routeSpd;
 				pointsX.add(obj.getPoint31XTile(i));
 				pointsY.add(obj.getPoint31YTile(i));
 				times.add(tm);
-				indexedPoints.registerObjectXY(obj.getPoint31XTile(i), obj.getPoint31YTile(i), pointsX.size() - 1);
+				quadTree.insert(pointsX.size() - 1, obj.getPoint31XTile(i), obj.getPoint31YTile(i));
+				// indexedPoints.registerObjectXY();
 				totaltm += tm;
+				
 				if (i == s.getEndPointIndex()) {
 					break;
 				}
@@ -100,68 +108,111 @@ public class PrecalculatedRouteDirection {
 	}
 
 	public float timeEstimate(int sx31, int sy31, int ex31, int ey31) {
-		getIndex(sx31, sy31, ct1);
-		getIndex(ex31, ey31, ct2);
-		return Math.abs(ct1[0] - ct2[0]) + ct1[1] + ct2[1];
+		long l1 = calc(sx31, sy31);
+		long l2 = calc(ex31, ey31);
+		int x31;
+		int y31;
+		boolean start;
+		if(l1 == startPoint || l1 == endPoint) {
+			start = l1 == startPoint;
+			x31 = ex31;
+			y31 = ey31;
+		} else if(l2 == startPoint || l2 == endPoint) {
+			start = l2 == startPoint;
+			x31 = sx31;
+			y31 = sy31;
+		} else {
+			throw new UnsupportedOperationException();
+		}
+		int ind = getIndex(x31, y31);
+		if(ind == -1) {
+			return -1;
+		}
+		if((ind == 0 && start) || 
+				(ind == pointsX.size() - 1 && !start)) {
+			return -1;
+		}
+		float distToPoint = getDeviationDistance(x31, y31, ind);
+		float deviationPenalty = distToPoint / speed;
+		if(start) {
+			return (tms[0] - tms[ind]) +  deviationPenalty;
+		} else {
+			return tms[ind] + deviationPenalty;
+		}
+	}
+	
+	public float getDeviationDistance(int x31, int y31) {
+		int ind = getIndex(x31, y31);
+		if(ind == -1) {
+			return 0;
+		}
+		return getDeviationDistance(x31, y31, ind);
 	}
 
-	private int getIndex(int x31, int y31, float[] ct) {
-		long l = ((long) x31) << 32l + ((long)y31);
-		Integer lt = prereg.get(l);
-		int ind = 0;
-		if(lt != null) {
-			ind = lt;
-		} else {
-			cachedS.clear();
-			indexedPoints.getObjects(x31 - SHIFT, y31 - SHIFT, x31 + SHIFT, y31 + SHIFT, cachedS);
-			if (cachedS.size() == 0) {
-				for (int k = 0; k < SHIFTS.length; k++) {
-					indexedPoints.getObjects(x31 - SHIFTS[k], y31 - SHIFTS[k], x31 + SHIFTS[k], y31 + SHIFTS[k],
-							cachedS);
-					if (cachedS.size() != 0) {
-						break;
-					}
-				}
-				if (cachedS.size() == 0) {
-					throw new IllegalStateException();
+	public float getDeviationDistance(int x31, int y31, int ind) {
+		float distToPoint = 0; //BinaryRoutePlanner.squareRootDist(x31, y31, pointsX.get(ind), pointsY.get(ind));
+		if(ind < pointsX.size() - 1 && ind != 0) {
+			double nx = BinaryRoutePlanner.squareRootDist(x31, y31, pointsX.get(ind + 1), pointsY.get(ind + 1));
+			double pr = BinaryRoutePlanner.squareRootDist(x31, y31, pointsX.get(ind - 1), pointsY.get(ind - 1));
+			int nind =  nx > pr ? ind -1 : ind +1;
+			QuadPoint proj = MapUtils.getProjectionPoint31(x31, y31, pointsX.get(ind), pointsY.get(ind), pointsX.get(nind), pointsX.get(nind));
+			distToPoint = (float) BinaryRoutePlanner.squareRootDist(x31, y31, (int)proj.x, (int)proj.y) ;
+		}
+		return distToPoint;
+	}
+
+	public int getIndex(int x31, int y31) {
+		int ind = -1;
+		cachedS.clear();
+//		indexedPoints.getObjects(x31 - SHIFT, y31 - SHIFT, x31 + SHIFT, y31 + SHIFT, cachedS);
+		quadTree.queryInBox(new QuadRect(x31 - SHIFT, y31 - SHIFT, x31 + SHIFT, y31 + SHIFT), cachedS);
+		if (cachedS.size() == 0) {
+			for (int k = 0; k < SHIFTS.length; k++) {
+				quadTree.queryInBox(new QuadRect(x31 - SHIFTS[k], y31 - SHIFTS[k], x31 + SHIFTS[k], y31 + SHIFTS[k]), cachedS);
+//				indexedPoints.getObjects(x31 - SHIFTS[k], y31 - SHIFTS[k], x31 + SHIFTS[k], y31 + SHIFTS[k],cachedS);
+				if (cachedS.size() != 0) {
+					break;
 				}
 			}
-			double minDist = 0;
-			for (int i = 0; i < cachedS.size(); i++) {
-				Integer n = cachedS.get(i);
-				double ds = BinaryRoutePlanner.squareRootDist(x31, y31, pointsX.get(n), pointsY.get(n));
-				if (ds < minDist || i == 0) {
-					ind = n;
-					minDist = ds;
-				}
+			if (cachedS.size() == 0) {
+				return -1;
+			}
+		}
+		double minDist = 0;
+		for (int i = 0; i < cachedS.size(); i++) {
+			Integer n = cachedS.get(i);
+			double ds = BinaryRoutePlanner.squareRootDist(x31, y31, pointsX.get(n), pointsY.get(n));
+			if (ds < minDist || i == 0) {
+				ind = n;
+				minDist = ds;
 			}
 		}
-		double ds = BinaryRoutePlanner.squareRootDist(x31, y31, pointsX.get(ind), pointsY.get(ind));
-		ct[0] = tms[ind];
-		ct[1] = (float) (ds / speed);
 		return ind;
 	}
+
+	private long calc(int x31, int y31) {
+		return ((long) x31) << 32l + ((long)y31);
+	}
 	
 	public PrecalculatedRouteDirection adopt(RoutingContext ctx) {
-		int ind1 = getIndex(ctx.startX, ctx.startY, ct1);
-		int ind2 = getIndex(ctx.targetX, ctx.targetY, ct2);
-		if (ind1 < ind2) {
-			PrecalculatedRouteDirection routeDirection = new PrecalculatedRouteDirection(this, ind1, ind2);
-			routeDirection.preRegisterPoint(ctx.startX, ctx.startY);
-			routeDirection.preRegisterPoint(ctx.targetX, ctx.targetY);
-			return routeDirection;
+		int ind1 = getIndex(ctx.startX, ctx.startY);
+		int ind2 = getIndex(ctx.targetX, ctx.targetY);
+		if(ind1 == -1) {
+			throw new IllegalArgumentException();
 		}
-		return null;
+		if(ind2 == -1) {
+			throw new IllegalArgumentException();
+		}
+		PrecalculatedRouteDirection routeDirection = new PrecalculatedRouteDirection(this, ind1, ind2);
+		routeDirection.startPoint = calc(ctx.startX, ctx.startY);
+//		routeDirection.startX31 = ctx.startX;
+//		routeDirection.startY31 = ctx.startY;
+		routeDirection.endPoint = calc(ctx.targetX, ctx.targetX);
+//		routeDirection.endX31 = ctx.targetX;
+//		routeDirection.endY31 = ctx.targetY;
+		return routeDirection;
 	}
 
-	private void preRegisterPoint(int x31, int y31) {
-		int ind = getIndex(x31, y31, ct1);
-		long l = ((long) x31) << 32l + ((long)y31);
-		if(ind == -1){
-			throw new IllegalStateException();
-		}
-		prereg.put(l, ind);
-	}
 
 	
 

OsmAnd-java/src/net/osmand/router/RoutePlannerFrontEnd.java

@@ -92,14 +92,13 @@ public class RoutePlannerFrontEnd {
 		boolean intermediatesEmpty = intermediates == null || intermediates.isEmpty();
 		// TODO native, empty route...
 		// + intermediates, + progress, +complex, 
-		// TODO remove fast recalculation
 		PrecalculatedRouteDirection routeDirection = null;
 		if(ctx.calculationMode == RouteCalculationMode.COMPLEX) {
 			RoutingContext nctx = buildRoutingContext(ctx.config, ctx.nativeLib, ctx.getMaps(), RouteCalculationMode.BASE);
 			nctx.calculationProgress = ctx.calculationProgress ;
 			List<RouteSegmentResult> ls = searchRoute(nctx, start, end, intermediates);
 			routeDirection = PrecalculatedRouteDirection.build(ls, 
-					5000, ctx.getRouter().getMaxDefaultSpeed() );
+					ctx.config.DEVIATION_RADIUS, ctx.getRouter().getMaxDefaultSpeed() );
 		}
 		if(intermediatesEmpty && ctx.nativeLib != null) {
 			ctx.startX = MapUtils.get31TileNumberX(start.getLongitude());

OsmAnd-java/src/net/osmand/router/RouteResultPreparation.java

@@ -163,8 +163,10 @@ public class RouteResultPreparation {
 			// Get results from opposite direction roads
 			RouteSegment segment = finalSegment.reverseWaySearch ? finalSegment : finalSegment.opposite.getParentRoute();
 			int parentSegmentStart = finalSegment.reverseWaySearch ? finalSegment.opposite.getSegmentStart() : finalSegment.opposite.getParentSegmentEnd();
+			float parentRoutingTime = -1;
 			while (segment != null) {
 				RouteSegmentResult res = new RouteSegmentResult(segment.road, parentSegmentStart, segment.getSegmentStart());
+				parentRoutingTime = calcRoutingTime(parentRoutingTime, finalSegment, segment, res);
 				parentSegmentStart = segment.getParentSegmentEnd();
 				segment = segment.getParentRoute();
 				addRouteSegmentToResult(ctx, result, res, false);
@@ -174,19 +176,39 @@ public class RouteResultPreparation {
 
 			segment = finalSegment.reverseWaySearch ? finalSegment.opposite.getParentRoute() : finalSegment;
 			int parentSegmentEnd = finalSegment.reverseWaySearch ? finalSegment.opposite.getParentSegmentEnd() : finalSegment.opposite.getSegmentStart();
-			
+			parentRoutingTime = -1;
 			while (segment != null) {
 				RouteSegmentResult res = new RouteSegmentResult(segment.road, segment.getSegmentStart(), parentSegmentEnd);
+				parentRoutingTime = calcRoutingTime(parentRoutingTime, finalSegment, segment, res);
 				parentSegmentEnd = segment.getParentSegmentEnd();
 				segment = segment.getParentRoute();
 				// happens in smart recalculation
 				addRouteSegmentToResult(ctx, result, res, true);
 			}
 			Collections.reverse(result);
-
+			// checkTotalRoutingTime(result);
 		}
 		return result;
 	}
+
+	protected void checkTotalRoutingTime(List<RouteSegmentResult> result) {
+		float totalRoutingTime = 0;
+		for(RouteSegmentResult r : result) {
+			totalRoutingTime += r.getRoutingTime();
+		}
+		println("Total routing time ! " + totalRoutingTime);
+	}
+
+	private float calcRoutingTime(float parentRoutingTime, RouteSegment finalSegment, RouteSegment segment,
+			RouteSegmentResult res) {
+		if(segment != finalSegment) {
+			if(parentRoutingTime != -1) {
+				res.setRoutingTime(parentRoutingTime - segment.distanceFromStart);
+			}
+			parentRoutingTime = segment.distanceFromStart;
+		}
+		return parentRoutingTime;
+	}
 	
 	private void addRouteSegmentToResult(RoutingContext ctx, List<RouteSegmentResult> result, RouteSegmentResult res, boolean reverse) {
 		if (res.getStartPointIndex() != res.getEndPointIndex()) {
@@ -209,9 +231,11 @@ public class RouteResultPreparation {
 		if (rd == ld) {
 			if (toAdd.getStartPointIndex() == previous.getEndPointIndex() && !reverse) {
 				previous.setEndPointIndex(toAdd.getEndPointIndex());
+				previous.setRoutingTime(previous.getRoutingTime() + toAdd.getRoutingTime());
 				return true;
 			} else if (toAdd.getEndPointIndex() == previous.getStartPointIndex() && reverse) {
 				previous.setStartPointIndex(toAdd.getStartPointIndex());
+				previous.setRoutingTime(previous.getRoutingTime() + toAdd.getRoutingTime());
 				return true;
 			}
 		}
@@ -249,6 +273,7 @@ public class RouteResultPreparation {
 				}
 				StringBuilder additional = new StringBuilder();
 				additional.append("time = \"").append(res.getSegmentTime()).append("\" ");
+				additional.append("rtime = \"").append(res.getRoutingTime()).append("\" ");
 				additional.append("name = \"").append(name).append("\" ");
 //				float ms = res.getSegmentSpeed();
 				float ms = res.getObject().getMaximumSpeed();

OsmAnd-java/src/net/osmand/router/RouteSegmentResult.java

@@ -17,6 +17,7 @@ public class RouteSegmentResult {
 	private List<RouteSegmentResult>[] attachedRoutes;
 	private RouteSegmentResult[][] preAttachedRoutes;
 	private float segmentTime;
+	private float routingTime;
 	private float speed;
 	private float distance;
 	private String description = "";
@@ -111,6 +112,14 @@ public class RouteSegmentResult {
 		this.segmentTime = segmentTime;
 	}
 	
+	public void setRoutingTime(float routingTime) {
+		this.routingTime = routingTime;
+	}
+	
+	public float getRoutingTime() {
+		return routingTime;
+	}
+	
 	public LatLon getStartPoint() {
 		return convertPoint(object, startPointIndex);
 	}

OsmAnd-java/src/net/osmand/router/RoutingConfiguration.java

@@ -15,6 +15,7 @@ import org.xmlpull.v1.XmlPullParserException;
 public class RoutingConfiguration {
 	
 	public static final int DEFAULT_MEMORY_LIMIT = 30;
+	public final float DEVIATION_RADIUS = 15000;
 	public Map<String, String> attributes = new LinkedHashMap<String, String>();
 
 	// 1. parameters of routing and different tweaks

OsmAnd-java/src/net/osmand/router/RoutingContext.java

@@ -43,6 +43,9 @@ public class RoutingContext {
 	public static final int OPTION_NO_LOAD = 0;
 	public static final int OPTION_SMART_LOAD = 1;
 	public static final int OPTION_IN_MEMORY_LOAD = 2;
+
+
+	
 	// Final context variables
 	public final RoutingConfiguration config;
 	public final RouteCalculationMode calculationMode;
@@ -51,13 +54,13 @@ public class RoutingContext {
 	public final Map<RouteRegion, BinaryMapIndexReader> reverseMap = new LinkedHashMap<RouteRegion, BinaryMapIndexReader>();
 	
 	// 1. Initial variables
-	public long firstRoadId = 0;
-	public int firstRoadDirection = 0;
-	
 	public int startX;
 	public int startY;
 	public int targetX;
 	public int targetY;
+	// deprecated
+	public long firstRoadId;
+	public int firstRoadDirection;
 	
 	public RouteCalculationProgress calculationProgress;
 	public boolean leftSideNavigation;
@@ -82,6 +85,7 @@ public class RoutingContext {
 	public int memoryOverhead = 0;
 	
 	
+	long timeNanoToCalcDeviation = 0;
 	long timeToLoad = 0;
 	long timeToLoadHeaders = 0;
 	long timeToFindInitialSegments = 0;
@@ -103,6 +107,10 @@ public class RoutingContext {
 	public FinalRouteSegment finalRouteSegment;
 
 
+
+
+
+
 	
 	public RoutingContext(RoutingContext cp) {
 		this.config = cp.config;