From 925509fa921205439db4d409b288f0c0c72f9a50 Mon Sep 17 00:00:00 2001
From: Victor Shcherb <victor.shcherb@gmail.com>
Date: Sun, 11 Nov 2012 12:35:16 +0100
Subject: [PATCH] Small fixes

---
 .../plus/routing/RouteCalculationResult.java  | 24 ++++++++++++++-----
 .../osmand/src/binaryRoutePlanner.cpp         | 22 ++++++++++-------
 2 files changed, 32 insertions(+), 14 deletions(-)

diff --git a/OsmAnd/src/net/osmand/plus/routing/RouteCalculationResult.java b/OsmAnd/src/net/osmand/plus/routing/RouteCalculationResult.java
index 37bb96dbab..d3c0635307 100644
--- a/OsmAnd/src/net/osmand/plus/routing/RouteCalculationResult.java
+++ b/OsmAnd/src/net/osmand/plus/routing/RouteCalculationResult.java
@@ -94,12 +94,20 @@ public class RouteCalculationResult {
 			int[] interLocations = new int[intermediates.size()];
 			int currentIntermediate = 0;
 			int currentLocation = 0;
-			while(currentIntermediate < intermediates.size() && currentLocation < this.locations.size()){
-				if(MapUtils.getDistance(intermediates.get(currentIntermediate), 
-						this.locations.get(currentLocation).getLatitude(), this.locations.get(currentLocation).getLongitude()) <
-						15) {
+			double distanceThreshold = 25;
+			double prevDistance = distanceThreshold * 4;
+			while((currentIntermediate < intermediates.size() || prevDistance > distanceThreshold)
+				&& currentLocation < this.locations.size()){
+				if(currentIntermediate < intermediates.size() && 
+						getDistanceToLocation(intermediates.get(currentIntermediate), currentLocation) < 50) {
+					prevDistance = getDistanceToLocation(intermediates.get(currentIntermediate), currentLocation);
 					interLocations[currentIntermediate] = currentLocation;
 					currentIntermediate++;
+				} else if(currentIntermediate > 0 && prevDistance > distanceThreshold && 
+						getDistanceToLocation(intermediates.get(currentIntermediate - 1), 
+						currentLocation) < prevDistance) {
+					prevDistance = getDistanceToLocation(intermediates.get(currentIntermediate - 1), currentLocation);
+					interLocations[currentIntermediate - 1] = currentLocation;
 				}
 				currentLocation ++;
 			}
@@ -110,8 +118,7 @@ public class RouteCalculationResult {
 				if (locationIndex >= interLocations[currentIntermediate]) {
 					// split directions
 					if (locationIndex > interLocations[currentIntermediate]
-							&& MapUtils.getDistance(intermediates.get(currentIntermediate),
-									this.locations.get(locationIndex).getLatitude(), this.locations.get(locationIndex).getLongitude()) > 50) {
+							&& getDistanceToLocation(intermediates.get(currentIntermediate), locationIndex) > 50) {
 						RouteDirectionInfo toSplit = localDirections.get(currentDirection);
 						RouteDirectionInfo info = new RouteDirectionInfo(localDirections.get(currentDirection).getAverageSpeed(), TurnType.valueOf(TurnType.C,
 								false));
@@ -129,6 +136,11 @@ public class RouteCalculationResult {
 		}
 	}
 
+	private double getDistanceToLocation(LatLon p, int currentLocation) {
+		return MapUtils.getDistance(p, 
+				this.locations.get(currentLocation).getLatitude(), this.locations.get(currentLocation).getLongitude());
+	}
+
 	private void attachAlarmInfo(List<AlarmInfo> alarms, RouteSegmentResult res, int intId, int locInd) {
 		int[] pointTypes = res.getObject().getPointTypes(intId);
 		RouteRegion reg = res.getObject().region;
diff --git a/Osmand-kernel/osmand/src/binaryRoutePlanner.cpp b/Osmand-kernel/osmand/src/binaryRoutePlanner.cpp
index 077a636b85..ed1c016484 100644
--- a/Osmand-kernel/osmand/src/binaryRoutePlanner.cpp
+++ b/Osmand-kernel/osmand/src/binaryRoutePlanner.cpp
@@ -513,6 +513,8 @@ SHARED_PTR<RouteSegment> findRouteSegment(int px, int py, RoutingContext* ctx) {
 	}
 	SHARED_PTR<RouteSegment> road;
 	double sdist = 0;
+	int foundx = 0;
+	int foundy = 0;
 	vector<SHARED_PTR<RouteDataObject> >::iterator it = dataObjects.begin();
 	for (; it!= dataObjects.end(); it++) {
 		SHARED_PTR<RouteDataObject> r = *it;
@@ -536,20 +538,24 @@ SHARED_PTR<RouteSegment> findRouteSegment(int px, int py, RoutingContext* ctx) {
 				}
 				double currentsDist = squareDist31TileMetric(prx, pry, px, py);
 				if (road.get() == NULL || currentsDist < sdist) {
-					// make copy before
-					// r->pointsX.insert(j, prx);
-					// r->pointsY.insert(j, pry);
 					road = SHARED_PTR<RouteSegment>(new RouteSegment(r, j));
+					foundx = prx;
+					foundy = pry;
 					sdist = currentsDist;
 				}
 			}
 		}
 	}
-	// TODO FIX
-//	if (road.get() != null) {
-//		// re-register the best road because one more point was inserted
-//		ctx->registerRouteDataObject(road.getRoad());
-//	}
+	if (road.get() != NULL) {
+		// make copy before
+		SHARED_PTR<RouteDataObject> r = road->road;
+		int index = road->getSegmentStart();
+		r->pointsX.insert(r->pointsX.begin() + index, foundx);
+		r->pointsY.insert(r->pointsY.begin() + index, foundy);
+		if(r->pointTypes.size() > index) {
+			r->pointTypes.insert(r->pointTypes.begin() + index, std::vector<uint32_t>());
+		}
+	}
 	return road;
 }