private boolean updateCurrentRouteStatus(Location currentLocation, float posTolerance) {
List<Location> routeNodes = route.getImmutableAllLocations();
int currentRoute = route.currentRoute;
// 1. Try to proceed to next point using orthogonal distance (finding minimum orthogonal dist)
while (currentRoute + 1 < routeNodes.size()) {
double dist = currentLocation.distanceTo(routeNodes.get(currentRoute));
if (currentRoute > 0) {
dist =
getOrthogonalDistance(
currentLocation, routeNodes.get(currentRoute - 1), routeNodes.get(currentRoute));
}
boolean processed = false;
// if we are still too far try to proceed many points
// if not then look ahead only 3 in order to catch sharp turns
boolean longDistance = dist >= 250;
int newCurrentRoute =
lookAheadFindMinOrthogonalDistance(
currentLocation, routeNodes, currentRoute, longDistance ? 15 : 8);
double newDist =
getOrthogonalDistance(
currentLocation,
routeNodes.get(newCurrentRoute),
routeNodes.get(newCurrentRoute + 1));
if (longDistance) {
if (newDist < dist) {
if (log.isDebugEnabled()) {
log.debug(
"Processed by distance : (new) "
+ newDist
+ " (old) "
+ dist); //$NON-NLS-1$//$NON-NLS-2$
}
processed = true;
}
} else if (newDist < dist || newDist < 10) {
// newDist < 10 (avoid distance 0 till next turn)
if (dist > posTolerance) {
processed = true;
if (log.isDebugEnabled()) {
log.debug(
"Processed by distance : " + newDist + " " + dist); // $NON-NLS-1$//$NON-NLS-2$
}
} else {
// case if you are getting close to the next point after turn
// but you have not yet turned (could be checked bearing)
if (currentLocation.hasBearing() || lastFixedLocation != null) {
float bearingToRoute = currentLocation.bearingTo(routeNodes.get(currentRoute));
float bearingRouteNext =
routeNodes.get(newCurrentRoute).bearingTo(routeNodes.get(newCurrentRoute + 1));
float bearingMotion =
currentLocation.hasBearing()
? currentLocation.getBearing()
: lastFixedLocation.bearingTo(currentLocation);
double diff = Math.abs(MapUtils.degreesDiff(bearingMotion, bearingToRoute));
double diffToNext = Math.abs(MapUtils.degreesDiff(bearingMotion, bearingRouteNext));
if (diff > diffToNext) {
if (log.isDebugEnabled()) {
log.debug("Processed point bearing deltas : " + diff + " " + diffToNext);
}
processed = true;
}
}
}
}
if (processed) {
// that node already passed
route.updateCurrentRoute(newCurrentRoute + 1);
currentRoute = newCurrentRoute + 1;
} else {
break;
}
}
// 2. check if intermediate found
if (route.getIntermediatePointsToPass() > 0
&& route.getDistanceToNextIntermediate(lastFixedLocation) < POSITION_TOLERANCE * 2) {
showMessage(app.getString(R.string.arrived_at_intermediate_point));
route.passIntermediatePoint();
TargetPointsHelper targets = app.getTargetPointsHelper();
List<String> ns = targets.getIntermediatePointNames();
int toDel = targets.getIntermediatePoints().size() - route.getIntermediatePointsToPass();
int currentIndex = toDel - 1;
String name =
currentIndex < 0 || currentIndex >= ns.size() || ns.get(currentIndex) == null
? ""
: ns.get(currentIndex);
if (isFollowingMode) {
voiceRouter.arrivedIntermediatePoint(name);
}
while (toDel > 0) {
targets.removeWayPoint(false, 0);
toDel--;
}
while (intermediatePoints != null
&& route.getIntermediatePointsToPass() < intermediatePoints.size()) {
intermediatePoints.remove(0);
}
}
// 3. check if destination found
Location lastPoint = routeNodes.get(routeNodes.size() - 1);
if (currentRoute > routeNodes.size() - 3
&& currentLocation.distanceTo(lastPoint)
< (((float) settings.getApplicationMode().getArrivalDistance())
* settings.ARRIVAL_DISTANCE_FACTOR.get())) {
showMessage(app.getString(R.string.arrived_at_destination));
TargetPointsHelper targets = app.getTargetPointsHelper();
String description = targets.getPointNavigateDescription();
if (isFollowingMode) {
voiceRouter.arrivedDestinationPoint(description);
}
if (OsmandPlugin.onDestinationReached()) {
clearCurrentRoute(null, null);
setRoutePlanningMode(false);
app.runInUIThread(
new Runnable() {
@Override
public void run() {
settings.APPLICATION_MODE.set(settings.DEFAULT_APPLICATION_MODE.get());
}
});
// targets.clearPointToNavigate(false);
return true;
}
}
return false;
}
