/*
 * 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.ResponsePath;
import com.graphhopper.config.Profile;
import com.graphhopper.routing.util.EdgeFilter;
import com.graphhopper.storage.index.LocationIndex;
import com.graphhopper.storage.index.Snap;
import com.graphhopper.util.EdgeIteratorState;
import com.graphhopper.util.Parameters;
import com.graphhopper.util.PointList;
import com.graphhopper.util.shapes.GHPoint;

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.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.tud.kom.p2psim.impl.util.Tuple;
import de.tudarmstadt.maki.simonstrator.api.Monitor;
import de.tudarmstadt.maki.simonstrator.api.Randoms;
import de.tudarmstadt.maki.simonstrator.api.Monitor.Level;
import de.tudarmstadt.maki.simonstrator.api.component.ComponentNotAvailableException;
import de.tudarmstadt.maki.simonstrator.api.util.XMLConfigurableConstructor;

/**
 * Movement strategy based on OpenStreetMap (OSM) data, to allow movement of actuators on
 * pedestrian paths, roads, etc. Routing and navigation is performed by the GraphHopper
 * library, allowing different movement profiles, e.g., for restricting pedestrians to walkways
 * or cars to accessible roads. 
 * 
 *
 * @author Julian Zobel
 * @version 2.0, 12.10.2022
 * 
 * Moved from the very old GraphHopper Version 0.13.0 directly to version 6.0 
 *
 * @author Julian Zobel
 * @version 02.04.2020
 * 
 * - added the possibility to use alternative, longer routes to better represent non-optimal human behavior.
 * - added the possibility to block certain areas from access 
 * 
 * @author Clemens Krug
 * @version 13.03.2017
 * 
 * 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
 * 
 * - added a possibility to allow the usage of alternative routes
 * - set the probability for this alternatives
 * - allow a dynamic reload of graphhopper files when switching routing algorithms
 * - allow to define blocked areas, that are not used for routing
 *   
 */
public class RealWorldStreetsMovement extends AbstractLocalMovementStrategy {
	
	private Random random = Randoms.getRandom(RealWorldStreetsMovement.class);
	
	private boolean init = false;
	
	private GraphHopper hopper;
	private LocationIndex index;	
	
	private static HashMap<SimLocationActuator, RouteImpl> currentRoutes = new HashMap<>();
	private Map<SimLocationActuator, RouteSensorComponent> routeSensorComponents = new LinkedHashMap<>();
	
	/*
	 * Location of the OSM file. Must be in .pbf format.
	 */
	private String osmFileLocation; 
	private String graphFolderFiles;
	
	/*
	 * The movement type field defines one or more comma-separated types of vehicles, as defined by GraphHopper's 
	 * built-in vehicle types. See GH/docs/core/profiled for more information. 
	 * 
	 * The default movement type is "foot" for pedestrian movement. 
	 */
	private String movementType = "foot"; 
		
	/*
	 * "The weighting determines the 'cost function' for the route calculation" as comma-separated types, as defined 
	 * by GraphHopper's built-in weightings. See GH/docs/core/profiled for more information. 
	 * 
	 * The default weighting is "fastest" 
	 */
	private String weightingType = "fastest ";
	
	// GraphHopper can be configured with different localities. 
	private Locale localSetting = Locale.GERMANY;
	
	private String blockedAreas; // default null	
	private boolean uniqueFolders;
	
	private boolean allowAlternativeRoutes = false;
	private double probabilityForAlternativeRoute = 0.0;

	/**
	 * 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() {
		//
	}
	
	private void init() {
		
		hopper = new GraphHopper();
		hopper.setOSMFile(osmFileLocation);
		
		// GraphHopper file storage
		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);
		}
				
		/*
		 * Create GH profiles. 
		 */		
		String[] movements = movementType.split(",");
		String[] weights =  weightingType.split(",");
		
		// check that every movement type also has a weighting
		if(movements.length != weights.length) {
			Monitor.log(RealWorldStreetsMovement.class, Level.ERROR,
					"Provided types for movement and weighting do not match. Weighting must be provided per movement type.", 
					new Tuple<String, String>(movementType, weightingType));
			throw new UnsupportedOperationException("[GraphHopper] Initialization error. Provided types for movement and weighting do not match. "
					+ "Weighting must be provided per movement type.");
		}
		
		// create a GH profile for every movement type. 
		for(int i = 0; i < movements.length; i++) {
			hopper.setProfiles(new Profile(movements[i]).setVehicle(movements[i]).setWeighting(weights[i]).setTurnCosts(false));
		}		
		
		// Try to load a cached import. If the cached import does not match our setting, 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! This may take some time...");
			
			Monitor.log(RealWorldStreetsMovement.class, Level.INFO,
					"GraphHopper graph file must be imported again! Cleaning all files before starting the import.",
					graphFolderFiles);			
			hopper.clean();
			hopper.importOrLoad();
		}	
		finally {
			Monitor.log(RealWorldStreetsMovement.class, Level.INFO, "GraphHopper graph file loaded.", graphFolderFiles);			
		}
		
		index = hopper.getLocationIndex();
		init = true;
	}	

	@Override
	public Either<PositionVector, Boolean> nextPosition(
			SimLocationActuator comp, PositionVector destination) {
		// use the "first" (maybe the only) defined movement type as default  
        return nextPosition(comp, destination, movementType.split(",")[0]);
    }
	
	/**
	 * Get the next position with a specific @param profile (must be pre-defined). 
	 */
	public Either<PositionVector, Boolean> nextPosition(SimLocationActuator comp,
			PositionVector destination, String profile) {
		
		// check that the given movement type is valid
		if (profile == null || profile.equals("")) {
			Monitor.log(RealWorldStreetsMovement.class, Level.ERROR,
					"Profile not defined!", profile);
			throw new UnsupportedOperationException("[GraphHopper] Cannot route with an undefined movement type!");	
		}
			
		if(!this.movementType.contains(profile)) {
			Monitor.log(RealWorldStreetsMovement.class, Level.ERROR,
					"The given profile is not part of the initialized GraphHopper graph!", profile);
			throw new UnsupportedOperationException("[GraphHopper] The given profile ("+profile+") is not part of the initialized GraphHopper graph!");				
		}
		
		// first time? --> initialize GH
		if (!init) {
			init();
		}

		RouteSensorComponent routeSensor = routeSensorComponents.get(comp);
		
		// if no route sensor is known for this component, try to fetch it from the component itself
		if (routeSensor == null) {
			try {
				routeSensorComponents.put(comp, comp.getHost().getComponent(RouteSensorComponent.class));
				routeSensor = routeSensorComponents.get(comp);
			} catch (ComponentNotAvailableException e) {
				Monitor.log(RealWorldStreetsMovement.class, Level.ERROR,
						"RouteSensorComponent cannot be fetched from component. The movement model needs to provide RouteSensorComponents.", comp);
				throw new AssertionError("[RWSMovement] RouteSensorComponent cannot be fetched from component. 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 there is no trajectory or the destination of the component has changed from the stored trajectory, 
			 * a new trajectory must be calculated.
			 */
			
			if(trajectory == null || destination.distanceTo(trajectory.getDestination()) > tolerance) {
				trajectory = calculateTrajectory(comp, destination, profile);
			}
			
			newPosition = trajectory.updateCurrentLocation(comp, getMovementSpeed(comp), tolerance);

			if (trajectory.reachedDestination()) {
				routeSensor.reachedDestination();
			}
		}
		return new Left<PositionVector, Boolean>(newPosition);
	}

	/**
	 * Calculate a trajectory for a given actuator component from its current location towards a given destination. 
	 * 
	 * @param comp
	 * @param destination
	 * @param profile
	 * @return
	 */
	private RouteImpl calculateTrajectory(SimLocationActuator comp, PositionVector destination, String profile) {
		double[] startPosition = GPSCalculation.transformOwnWorldWindowToGPS(comp.getRealPosition());
		double[] destinationPosition = GPSCalculation.transformOwnWorldWindowToGPS(destination);
		
		ResponsePath rp = calculateGHPath(startPosition, destinationPosition, profile);
		RouteImpl trajectory = null;
		
		/*
		 * In case of an error, e.g, when the requested point is not in the map data, return the destination as next point.
		 * 
		 * FIXME (JZ) this is a weird work-around and should be fixed somehow. 
		 */
		if(rp == null) {
			Monitor.log(this.getClass(), Monitor.Level.ERROR, "Routing request for Host %s failed.", comp.getHost().getId().valueAsString());
			
			PointList pointList = new PointList();
			pointList.add(new GHPoint(destinationPosition[0], destinationPosition[1]));
			trajectory = new RouteImpl(comp.getRealPosition(), destination, pointList);			
		}
		else {
			if (isCalculateRouteSegments()) {
				/*
				 * Obtain segment IDs along the route.
				 */
				trajectory = new RouteImpl(comp.getRealPosition(),
						destination, rp.getPoints(),
						routeSensorComponents.get(comp).getSegmentListeners(), routeSensorComponents.get(comp),
						calculateSegments(rp.getPoints()));
			} else {
				trajectory = new RouteImpl(comp.getRealPosition(),
						destination, rp.getPoints());
			}			
		}
		
		currentRoutes.put(comp, trajectory);
		routeSensorComponents.get(comp).setNewRoute(trajectory);
		return trajectory;
	}
	
	/**
	 * Use GraphHopper to calculate a possible path from one location to another.  
	 * 
	 * @param start Lat/Lon of start
	 * @param destination Lat/lon of destination
	 * @param profile Defines used routing profile
	 * @return A possible path from start to destination. Null in case of error.
	 */
	private ResponsePath calculateGHPath(double[] start, double[] destination, String profile) {
		// Create request from start to destination
		GHRequest req = new GHRequest(start[0], start[1], destination[0], destination[1])		   
			    .setLocale(localSetting)
			    .setProfile(profile);
		
		// blocks one or multiple given areas from being used by the routing				
		if(blockedAreas != null && !blockedAreas.isEmpty()) {
			req.putHint(Parameters.Routing.BLOCK_AREA, blockedAreas);
		}		
		
		// if alternative routes are allowed, we need to change the routing algorithm and define the maximum boundaries for allowed paths and weights.
		if(allowAlternativeRoutes) {
			// see: https://github.com/graphhopper/graphhopper/blob/master/example/src/main/java/com/graphhopper/example/RoutingExample.java
			req.setAlgorithm(Parameters.Algorithms.ALT_ROUTE);		
			
			// how much longer can alternatives be?
			req.getHints().putObject(Parameters.Algorithms.AltRoute.MAX_WEIGHT, 2.0);
			// how much alternatives should be returned at max?			
			req.getHints().putObject(Parameters.Algorithms.AltRoute.MAX_PATHS, 5);			
		}
				
		
		GHResponse res = hopper.route(req);		
					
		if(res.hasErrors()) {	
			Monitor.log(this.getClass(), Monitor.Level.ERROR, "Routing request from starting point (" +
					"%f,%f) to destination point (%f,%f) and profile %s failed with error: %s.", start[0], start[1],
					destination[0], destination[1], profile, res.getErrors());
			return null;
		} 
		
		/*
		 * Alternative routes to depict non-optimal movement, e.g., like humans tend to.
		 *  
		 * - Must be explicitly activated
		 * - Only possible if more than one route is available (obviously)			
		 * - An alternative route is taken with a certain chance against the stated probability
		 */			
		if(allowAlternativeRoutes && res.hasAlternatives() 
				&& probabilityForAlternativeRoute > 0 
				&& (random.nextDouble() <= probabilityForAlternativeRoute)) {				
						
			List<ResponsePath> paths = res.getAll();	
		
			// remove the best, we do not want this!				
			paths.remove(res.getBest()); 		
							
			if(paths.size() == 1) {
				// only one alternative remaining
				return paths.get(0);
			}
			else {
				return paths.get(random.nextInt(paths.size() - 1) + 1);
			}				
		}	
		else {
			return res.getBest();
		}
		
	}
		
	/**
	 * Calculate the route segments along the given point list.
	 * 
	 * FIXME (JZ) this is not documented and unclear if (and why it is) needed!
	 * 
	 * @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) {
			Snap 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(GPSCalculation.transformGPSWindowToOwnWorld(point.getLat(), point.getLon()));
					}
					lastSegment = new RouteSegmentImpl(Integer.toString(edgeId),
							ptIdx, GPSCalculation.transformGPSWindowToOwnWorld(point.getLat(), point.getLon()));
					segments.add(lastSegment);
				} else {
					lastSegment.addRouteLocation(GPSCalculation.transformGPSWindowToOwnWorld(point.getLat(), point.getLon()));
				}
			}
			ptIdx++;
		}
		return segments;
	}
	

	/**
	 * 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(GPSCalculation.transformGPSWindowToOwnWorld(p.getLat(), p.getLon()))));

		return positions;
	}

	/**
	 * Note: This is some single-use code fragment from Clemens Krug for a work-around utility function. 
	 * May not work as intended and also result in errors in the future. 
	 * 
	 * 
	 * -- original --
	 * 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.
	 */
	@Deprecated	
	public double calculateRouteLength(PositionVector start, PositionVector destination)
	{		
		ResponsePath path = this.calculateGHPath(GPSCalculation.transformOwnWorldWindowToGPS(start), 
				GPSCalculation.transformOwnWorldWindowToGPS(destination), 
				movementType.split(",")[0]);
		
		if(path == null) {
			return -1;
		}
		else {
			return path.getDistance();
		}
	}

	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];
	}

	/*
	 * FIXME (JZ) navigationalType is an incorrect name tag, as the GH naming is simply "weighting". 
	 * Must be adapted in all configs, so currently left as it is for legacy support.
	 */
	@Deprecated
	public void setNavigationalType(String weightingType) {
		this.weightingType = weightingType;
	}
	
	public void setWeightingType(String weightingType) {
		this.weightingType = weightingType;
	}
	
	public void setWaypointTolerance(double tolerance) {
		this.tolerance = tolerance;
	}

	/**
	 * For large batch simulations, we need to prevent same-time access to
	 * GraphHopper 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;
	}
	
	/**
	 * Blocked areas, i.e., through which the GraphHopper engine will not route, can be set via this string. 
	 * Each area is defined by a bounding box. Positions are given as lat,lon,lat,lon for the two points.
	 * Multiple areas are separated by semicolon. 
	 * 
	 * Example:  51.813680,8.783510,51.806795,8.804239;51.821036,8.771151,51.814987, 8.779090;
	 * 
	 * 
	 * @param blockedAreas
	 */
	public void setBlockedAreas(String blockedAreas) {
		if(!blockedAreas.isEmpty()) {
			this.blockedAreas = blockedAreas;
		}		
	}
	
	public String getBlockedAreas() {
		return this.blockedAreas;
	}
	
	public void setAllowAlternativeRoutes(boolean allowAlternativeRoutes) {
		this.allowAlternativeRoutes = allowAlternativeRoutes;
	}
	
	public void setProbabilityForAlternativeRoute(double p) {
		this.probabilityForAlternativeRoute = p;
	}
	 
}
