RealWorldStreetsMovement.java

/*
 * Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
 *
 * This file is part of PeerfactSim.KOM.
 * 
 * PeerfactSim.KOM is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 * 
 * PeerfactSim.KOM is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with PeerfactSim.KOM.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

package de.tud.kom.p2psim.impl.topology.movement.local;

import java.util.HashMap;
import java.util.LinkedHashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Locale;
import java.util.Map;
import java.util.Random;
import java.util.UUID;

import com.graphhopper.GHRequest;
import com.graphhopper.GHResponse;
import com.graphhopper.GraphHopper;
import com.graphhopper.PathWrapper;
import com.graphhopper.reader.osm.GraphHopperOSM;
import com.graphhopper.routing.util.EdgeFilter;
import com.graphhopper.routing.util.EncodingManager;
import com.graphhopper.routing.util.TraversalMode;
import com.graphhopper.routing.weighting.BlockAreaWeighting;
import com.graphhopper.storage.index.LocationIndex;
import com.graphhopper.storage.index.QueryResult;
import com.graphhopper.util.DistanceCalc2D;
import com.graphhopper.util.EdgeIteratorState;
import com.graphhopper.util.Instruction;
import com.graphhopper.util.InstructionList;
import com.graphhopper.util.Parameters;
import com.graphhopper.util.Parameters.Routing;
import com.graphhopper.util.PointList;
import com.graphhopper.util.shapes.GHPoint;

import de.tud.kom.p2psim.api.topology.Topology;
import de.tud.kom.p2psim.api.topology.movement.SimLocationActuator;
import de.tud.kom.p2psim.impl.topology.movement.modularosm.GPSCalculation;
import de.tud.kom.p2psim.impl.topology.movement.modularosm.ModularMovementModelViz;
import de.tud.kom.p2psim.impl.topology.movement.modularosm.RouteSensorComponent;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tud.kom.p2psim.impl.util.Either;
import de.tud.kom.p2psim.impl.util.Left;
import de.tudarmstadt.maki.simonstrator.api.Binder;
import de.tudarmstadt.maki.simonstrator.api.Monitor;
import de.tudarmstadt.maki.simonstrator.api.Monitor.Level;
import de.tudarmstadt.maki.simonstrator.api.component.ComponentNotAvailableException;
import de.tudarmstadt.maki.simonstrator.api.util.XMLConfigurableConstructor;

/**
 * This movement strategy uses the data from osm and navigates the nodes throught streets to the destination
 *
 * 13.03.2017 Clemens Krug: Fixed a bug. When the GraphHopper routing had errors the nodes would move to the
 * top right corner and not, as intended, straight to their destination.
 * 
 * @author Martin Hellwig
 * @version 1.0, 07.07.2015
 */
public class RealWorldStreetsMovement extends AbstractLocalMovementStrategy {
	
	private static PositionVector worldDimensions;
	private GraphHopper hopper;
	private LocationIndex index;
	private boolean init = false;
	
	private static HashMap<SimLocationActuator, RouteImpl> currentRoutes = new HashMap<>();

	private Map<SimLocationActuator, RouteSensorComponent> routeSensorComponents = new LinkedHashMap<>();

	private String osmFileLocation; //use pbf-format, because osm-format causes problems (xml-problems)
	private String graphFolderFiles;
	private String movementType; //car, bike or foot
	private String defaultMovement;
	private String navigationalType; //fastest,
	private String blockedAreas; // default null
	private static double latLower; //Values from -90 to 90; always smaller than latUpper
	private static double latUpper; //Values from -90 to 90
	private static double lonLeft; //Values from -180 to 180; Always smaller than lonRight
	private static double lonRight; //Values from -180 to 180
	private boolean uniqueFolders;

	/**
	 * Tolerance in meters (if the node reached a waypoint up to "tolerance"
	 * meters, it will select the next waypoint in the path.
	 */
	private double tolerance = 1;

	@XMLConfigurableConstructor({"blockedAreas"})
	public RealWorldStreetsMovement(String blockedAreas) {
		this();
		this.setBlockedAreas(blockedAreas);
	}
	
	public RealWorldStreetsMovement() {
		worldDimensions = Binder.getComponentOrNull(Topology.class)
				.getWorldDimensions();
		latLower = GPSCalculation.getLatLower();
		latUpper = GPSCalculation.getLatUpper();
		lonLeft = GPSCalculation.getLonLeft();
		lonRight = GPSCalculation.getLonRight();
	}
	
	private void init() {
		hopper = new GraphHopperOSM().forServer();
		hopper.setDataReaderFile(osmFileLocation);
		
		// where to store graphhopper files?
		if (uniqueFolders) {
			Monitor.log(RealWorldStreetsMovement.class, Level.WARN,
					"Using per-simulation unique folders for GraphHopper temporary data in %s. Remember to delete them to prevent your disk from filling up.",
					graphFolderFiles);
			hopper.setGraphHopperLocation(graphFolderFiles + "/"
					+ UUID.randomUUID().toString());
		} else {
			hopper.setGraphHopperLocation(graphFolderFiles + "/"
					+ osmFileLocation.hashCode() + movementType);
		}
		hopper.setEncodingManager(EncodingManager.create(movementType));
				
		// if we have blocked areas defined, the CH in Graphhopper needs to be disabled!
		// this also requires a new import 
		if(blockedAreas != null && !blockedAreas.isEmpty()) {		
			hopper.setCHEnabled(false);
		}
		
		// try to load a cached import. If the cached import was with a different CH_ENABLED setting then now specified, this will fail.
		// in this case, clean cache and re-import.
		try {
			hopper.importOrLoad();
		} catch (Exception e) {
			System.err.println("[RealWorldStreetMovement] GraphHopper graph file must be imported again!");
			hopper.clean();
			hopper.importOrLoad();
		}	
		
		index = hopper.getLocationIndex();
		init = true;
	}	

	@Override
	public Either<PositionVector, Boolean> nextPosition(
			SimLocationActuator comp, PositionVector destination) {
        return nextPosition(comp, destination, defaultMovement);
    }

	public Either<PositionVector, Boolean> nextPosition(SimLocationActuator comp,
			PositionVector destination, String movementType) {

		if (movementType == null || movementType.equals("")
				|| !this.movementType.contains(movementType)) {
			throw new AssertionError("Invalid movement type: " + movementType);
		}

		if (!init) {
			init();
		}

		RouteSensorComponent routeSensor = routeSensorComponents.get(comp);
		if (routeSensor == null) {
			try {
				routeSensorComponents.put(comp, comp.getHost()
						.getComponent(RouteSensorComponent.class));
				routeSensor = routeSensorComponents.get(comp);
			} catch (ComponentNotAvailableException e) {
				throw new AssertionError(
						"The movement model needs to provide RouteSensorComponents.");
			}
		}


		PositionVector newPosition = null;
		if (destination
				.distanceTo(comp.getRealPosition()) > getMovementSpeed(comp)) {
			//if not set already for this node or new destination is different than last one
			RouteImpl trajectory = currentRoutes.get(comp);
			if(trajectory == null || destination.distanceTo(trajectory.getDestination()) > 1.0) {
				double[] startPosition = transformOwnWorldWindowToGPS(comp.getRealPosition().getX(), comp.getRealPosition().getY());
				double[] destinationPosition = transformOwnWorldWindowToGPS(destination.getX(), destination.getY());
				GHRequest req = new GHRequest(startPosition[0], startPosition[1], destinationPosition[0], destinationPosition[1]).
					    setWeighting(navigationalType).
					    setVehicle(movementType).
					    setLocale(Locale.GERMANY);
				
				// blocks one or multiple given areas from being used by the routing
				// https://github.com/graphhopper/graphhopper/blob/master/docs/web/api-doc.md#flexible				
				if(blockedAreas != null && !blockedAreas.isEmpty()) {		
					req.getHints().put(Routing.BLOCK_AREA, blockedAreas);
				}		
				
				req.setAlgorithm(Parameters.Algorithms.ALT_ROUTE);
				
				GHResponse rsp = hopper.route(req);				
				
				//If the requested point is not in the map data, simple return the destination as next point
				if(rsp.hasErrors()) {
					Monitor.log(this.getClass(), Monitor.Level.ERROR, "Routing request for Host %s with starting point (" +
							"%f,%f), destination (%f,%f) and type %s failed with error: %s.", comp.getHost().getId().valueAsString(),startPosition[0], startPosition[1],
							destinationPosition[0], destinationPosition[1], movementType, rsp.getErrors());
					
					PointList pointList = new PointList();
					pointList.add(new GHPoint(destinationPosition[0], destinationPosition[1]));
					trajectory = new RouteImpl(comp.getRealPosition(), destination, pointList);
					currentRoutes.put(comp, trajectory);

				} else {
					
					PointList pointList = rsp.getBest().getPoints();
					
					if(rsp.hasAlternatives()) {
//						System.out.println(" >>");
//						for (PathWrapper pw : rsp.getAll()) {
//							System.out.println(pw.getDistance());
//							ModularMovementModelViz.paths.add(pw.getPoints());
//						}
//						System.out.println(" ");
						if(Math.random() < 0.01) {
							pointList = rsp.getAll().get(1).getPoints();
						}
						
						
					}
					
					//PointList pointList = rsp.getBest().getPoints();

					if (isCalculateRouteSegments()) {
						/*
						 * Obtain segment IDs along the route.
						 */
						trajectory = new RouteImpl(comp.getRealPosition(),
								destination, pointList,
								routeSensor.getSegmentListeners(), routeSensor,
								calculateSegments(rsp.getBest()));
					} else {
						trajectory = new RouteImpl(comp.getRealPosition(),
								destination, pointList);
					}
					currentRoutes.put(comp, trajectory);
				}
				routeSensor.setNewRoute(trajectory);
			}
			newPosition = trajectory.updateCurrentLocation(comp, getMovementSpeed(comp), tolerance);

			if (trajectory.reachedDestination()) {
				routeSensor.reachedDestination();
			}
		}
		return new Left<PositionVector, Boolean>(newPosition);
	}
	
	/**
	 * Calculate RouteSegments using the PathWrapper
	 * @param pathWrapper
	 * @return
	 */
	private List<RouteSegmentImpl> calculateSegments(PathWrapper pathWrapper) {
		InstructionList instructions = pathWrapper.getInstructions();
		int ptIdx = 0;
		List<RouteSegmentImpl> segments = new LinkedList<>();
		RouteSegmentImpl lastSegment = null;
		
		for (Instruction instr : instructions) {
			RouteSegmentImpl segment = null;
			PointList points = instr.getPoints();
			String name = instr.getName();
			if (name.isEmpty()) {				
				name = Integer.toString(instr.getPoints().get(0).hashCode());
				//name = Integer.toString(instr.getPoints().toGHPoint(0).hashCode());  @deprecated for old GraphHopper version
			}
			for (GHPoint point : points) {
				if (segment == null) {
					if (lastSegment != null) {
						lastSegment.addRouteLocation(transformGPSWindowToOwnWorld(
								point.getLat(), point.getLon()));
					}
					segment = new RouteSegmentImpl(name, ptIdx, transformGPSWindowToOwnWorld(point.getLat(),
						point.getLon()));
				} else {
					segment.addRouteLocation(transformGPSWindowToOwnWorld(
							point.getLat(), point.getLon()));
				}
				ptIdx++;
			}
			lastSegment = segment;
			segments.add(segment);
		}
		
		return segments;
	}
	
	/**
	 * Calculate the route segments along the given point list.
	 * 
	 * @return
	 */
	private List<RouteSegmentImpl> calculateSegments(PointList pointList) {
		int ptIdx = 0;
		int prevEdgeId = -1;
		List<RouteSegmentImpl> segments = new LinkedList<>();
		RouteSegmentImpl lastSegment = null;
		for (GHPoint point : pointList) {
			QueryResult qr = index.findClosest(point.getLat(), point.getLon(),
					EdgeFilter.ALL_EDGES);
			EdgeIteratorState nearEdge = qr.getClosestEdge();
			if (nearEdge != null) {
				int edgeId = nearEdge.getEdge();
				if (edgeId != prevEdgeId) {
					if (lastSegment != null) {
						lastSegment.addRouteLocation(
								transformGPSWindowToOwnWorld(point.getLat(),
										point.getLon()));
					}
					lastSegment = new RouteSegmentImpl(Integer.toString(edgeId),
							ptIdx, transformGPSWindowToOwnWorld(point.getLat(),
									point.getLon()));
					segments.add(lastSegment);
				} else {
					lastSegment.addRouteLocation(transformGPSWindowToOwnWorld(
							point.getLat(), point.getLon()));
				}
			}
			ptIdx++;
		}
		return segments;
	}

	/**
	 * Projects the world coordinates in the given gps window to the
	 * gps-coordinates
	 * 
	 * @param x
	 * @param y
	 * @return The projected position in gps-coordinates (lat, long)
	 */
	private double[] transformOwnWorldWindowToGPS(double x, double y) {
		double[] gps_coordinates = new double[2];
		gps_coordinates[0] = latLower + (latUpper - latLower) * (worldDimensions.getY() - y)/worldDimensions.getY();
		gps_coordinates[1] = lonLeft + (lonRight - lonLeft) * x/worldDimensions.getX();
		return gps_coordinates;
	}
	
	/**
	 * Projects the gps coordinates in the given gps window to the world-coordinates given in world-dimensions
	 * @param lat
	 * @param lon
	 * @return The projected position in world-dimensions
	 */
	public static PositionVector transformGPSWindowToOwnWorld(double lat, double lon) {
		double x = worldDimensions.getX() * (lon - lonLeft)/(lonRight - lonLeft);
		double y = worldDimensions.getY() - worldDimensions.getY() * (lat - latLower)/(latUpper - latLower);
		x = Math.max(0, x);
		x = Math.min(worldDimensions.getX(), x);
		y = Math.max(0, y);
		y = Math.min(worldDimensions.getY(), y);
		return new PositionVector(x, y);
	}

	/**
	 * Returns a list of points representing the current route of the component. Points are
	 * in x / y values of the own world.
	 * @param ms the component
	 * @return list of movement points.
	 */
	public List<PositionVector> getMovementPoints(SimLocationActuator ms)
	{
		List<PositionVector> positions = new LinkedList<>();
		PointList pointList = currentRoutes.get(ms).getPointList();

		pointList.forEach(p -> positions.add(new PositionVector(transformGPSWindowToOwnWorld(p.getLat(), p.getLon()))));

		return positions;
	}

	/**
	 * Calculates the length of a route in meters.
	 * @param start Starting position in own world coordinates (x / y)
	 * @param destination Destination on own world coordinates (x / y)
	 * @return the length of the route in meters.
	 */
	public double calculateRouteLength(PositionVector start, PositionVector destination)
	{
		PointList pointList;
			double[] startPosition = transformOwnWorldWindowToGPS(start.getX(), start.getY());
			double[] destinationPosition = transformOwnWorldWindowToGPS(destination.getX(), destination.getY());
			GHRequest req = new GHRequest(startPosition[0], startPosition[1], destinationPosition[0], destinationPosition[1]).
					setWeighting(navigationalType).
					setVehicle(movementType).
					setLocale(Locale.GERMANY);
			GHResponse rsp = hopper.route(req);

			//If the requested point is not in the map data, return -1
			if(rsp.hasErrors()) {
				return -1;
			}
			else {
				pointList = rsp.getBest().getPoints();
				return pointList.calcDistance(new DistanceCalc2D());
			}
	}

	/**
	 * Calculates the length of the current route of the SimLocationActuator.
	 * @param ms the component
	 * @return the length of the current route
	 */
	public double getCurrentRouteLength(SimLocationActuator ms)
	{
		return currentRoutes.get(ms).getPointList().calcDistance(new DistanceCalc2D());
	}
	
	public void setOsmFileLocation(String osmFileLocation) {
		this.osmFileLocation = osmFileLocation;
	}

	public void setGraphFolderFiles(String graphFolderFiles) {
		this.graphFolderFiles = graphFolderFiles;
	}

	public void setMovementType(String movementType) {
		this.movementType = movementType;
		defaultMovement = movementType.split(",")[0];
	}

	public void setNavigationalType(String navigationalType) {
		this.navigationalType = navigationalType;
	}
	
	public void setWaypointTolerance(double tolerance) {
		this.tolerance = tolerance;
	}

	/**
	 * For large batch simulations, we need to prevent same-time access to
	 * garphhopper temp data. Therefore, this flag creates unique folders for
	 * each run (which, obviously, wastes a lot of space and comp-resources and
	 * should not be used in standalone, single-threaded demo mode...)
	 *
	 * @param uniqueFolders
	 */
	public void setCreateUniqueFolders(boolean uniqueFolders) {
		this.uniqueFolders = uniqueFolders;
	}
	
	public void setBlockedAreas(String blockedAreas) {
		if(!blockedAreas.isEmpty()) {
			this.blockedAreas = blockedAreas;
		}
	}
}