TraciSimulationController.java

package de.tud.kom.p2psim.impl.topology.movement.vehicular.sumo.simulation.controller.traci;

import java.io.File;
import java.io.FileInputStream;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.ObjectInputStream;
import java.io.ObjectOutputStream;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.Random;

import de.tud.kom.p2psim.api.simengine.SimulatorObserver;
import de.tud.kom.p2psim.impl.simengine.Simulator;
import de.tud.kom.p2psim.impl.topology.PositionVector;
import de.tud.kom.p2psim.impl.topology.movement.vehicular.sumo.simulation.controller.VehicleInformationContainer;
import de.tudarmstadt.maki.simonstrator.api.Randoms;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.Location;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.api.EdgeController;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.api.SimulationSetupExtractor;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.api.VehicleController;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.api.information.LocationUtils;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetwork;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkEdge;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkLane;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkRoute;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.SerializableRoadNetwork;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.exception.NoAdditionalRouteAvailableException;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.exception.NoExitAvailableException;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.routing.BreadthFirstSearchRoutingAlgorithm;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.routing.RoutingAlgorithm;
import de.tudresden.sumo.cmd.Edge;
import de.tudresden.sumo.cmd.Junction;
import de.tudresden.sumo.cmd.Lane;
import de.tudresden.sumo.cmd.Simulation;
import de.tudresden.sumo.cmd.Vehicle;
import de.tudresden.sumo.util.SumoCommand;
import de.tudresden.ws.container.SumoBoundingBox;
import de.tudresden.ws.container.SumoGeometry;
import de.tudresden.ws.container.SumoLink;
import de.tudresden.ws.container.SumoLinkList;
import de.tudresden.ws.container.SumoPosition2D;
import de.tudresden.ws.container.SumoStringList;
import it.polito.appeal.traci.SumoTraciConnection;

/**
 *
 * @author Tobias Meuser (tobias.meuser@kom.tu-darmstadt.de)
 * @version 1.0 at 06.11.2017
 *
 */
public class TraciSimulationController implements VehicleController, SimulationSetupExtractor, EdgeController, SimulatorObserver {
	private static final File TEMP_FILE = new File(new File(System.getProperty("java.io.tmpdir")), "road_network.tmp");

	private Random _random = Randoms.getRandom(getClass());

	private List<RoadNetworkEdge> modifiedEdges = new ArrayList<>();

	private static final Map<String, TraciSimulationController> CONTROLLER = new HashMap<>();

	private static final double CLUSTERING_DISTANCE = 50;

	private String _sumoExe;
	private String _configFile;

	private SumoTraciConnection _connection;

	private double _start = -1;
	private double _step = -1;

	private double _startX;
	private double _startY;
	private double _endX;
	private double _endY;

	private Map<Double, Map<String, VehicleInformationContainer>> _positonsByTimestamp = new HashMap<>();

	private int _futureInformation = 0;

	private boolean _initalized = false;

	private boolean _observedAreaSet;

	private Map<String, Double> _vehiclesOutOfRange = new HashMap<>();

	private RoadNetwork _roadNetwork;

	private RoutingAlgorithm _algorithm = new BreadthFirstSearchRoutingAlgorithm();

	public static synchronized TraciSimulationController createSimulationController(String pSumoExe, String pConfigFile) {
		if (!CONTROLLER.containsKey(pConfigFile)) {
			CONTROLLER.put(pConfigFile, new TraciSimulationController(pSumoExe, pConfigFile));
		}
		return CONTROLLER.get(pConfigFile);
	}

	private TraciSimulationController(String pSumoExe, String pConfigFile) {
		_sumoExe = pSumoExe;
		_configFile = pConfigFile;
	}

	public static VehicleController getSimulationController() {
		return CONTROLLER.values().iterator().next();
	}

	@Override
	public synchronized void init() {
		if (!_initalized) {
			Random random = Randoms.getRandom("SUMO");

			// This will only work with the updated version of the TraaS API for sumo
			// It is available for download at https://dev.kom.e-technik.tu-darmstadt.de/gitlab/tobiasm/TraaS.git
			_connection = new SumoTraciConnection(_sumoExe, _configFile, random.nextInt());
			/*
			 * prevent vehicles form teleporting (http://sumo.dlr.de/wiki/Simulation/Why_Vehicles_are_teleporting)
			 */
			_connection.addOption("time-to-teleport", Integer.toString(-1));

			try {
				_connection.runServer();

				for (int i = 0; i < _futureInformation; i++) {
					nextStep();
				}
			} catch (RuntimeException e) {
				throw e;
			} catch (Exception e) {
				e.printStackTrace();
			}

			Simulator.getInstance().addObserver(this);

			_initalized = true;
		}
	}

	@Override
	public void simulationFinished() {
		/*
		 * This is called by the simulation scheduler once the simulation is
		 * finished - it should be used to terminate the connection to SUMO.
		 */
		if (_connection != null && !_connection.isClosed()) {
			_connection.close();
		}
	}

	@Override
	public Location getVehiclePosition(String pVehicleID) {
		return getVehiclePosition(_step, pVehicleID);
	}

	@Override
	public Location getVehiclePosition(double pStep, String pVehicleID) {
		Map<String, VehicleInformationContainer> map = _positonsByTimestamp.get(pStep);

		return map.get(pVehicleID).getPosition();
	}

	@Override
	public List<Location> getAllIntersections(boolean pCluster) {
		List<Location> result = new ArrayList<>();

		SumoCommand intersectionCommand = Junction.getIDList();

		Object intersectionObject = requestObject(intersectionCommand);

		SumoStringList intersections = (SumoStringList) intersectionObject;

		for (String intersection : intersections) {
			SumoCommand positionCommand = Junction.getPosition(intersection);
			Object positionObject = requestObject(positionCommand);
			SumoPosition2D sumoPosition = (SumoPosition2D) positionObject;

			if (_observedAreaSet) {
				if (_startX <= sumoPosition.x && sumoPosition.x <= _endX && _startY <= sumoPosition.y && sumoPosition.y <= _endY) {
					result.add(new PositionVector(sumoPosition.x - _startX, sumoPosition.y - _startY, 0));
				}
			} else {
				result.add(new PositionVector(sumoPosition.x, sumoPosition.y, 0));
			}
		}

		if (pCluster) {
			List<Location> tempResult = new ArrayList<>();

			outer:for (int i = 0; i < result.size(); i++) {
				Location position = result.get(i);

				for (int j = 0; j < tempResult.size(); j++) {
					Location addedPosition = tempResult.get(j);
					if (position != addedPosition && LocationUtils.calculateDistance(position, addedPosition) < CLUSTERING_DISTANCE / 1000.0) {
						continue outer;
					}
				}

				tempResult.add(position);
			}

			result = tempResult;
		}

		return result;
	}

	@Override
	public boolean nextStep() {
		if (Simulator.getEndTime() == Simulator.getCurrentTime()) {
			return false;
		}
		try {
			_connection.do_timestep();

			try {
				int temp = (Integer) _connection.do_job_get(Simulation.getCurrentTime());

				_step = temp / 1000.0 - _futureInformation;

				if (_start == -1) {
					_start = _step + _futureInformation;
				}

				double newStep = _step + _futureInformation;
				if (!_positonsByTimestamp.containsKey(newStep)) {
					Map<String, VehicleInformationContainer> vehiclePositions = new HashMap<>();
					_positonsByTimestamp.put(newStep, vehiclePositions);

					List<String> allVehicles = requestAllVehicles();
					for (String vehicle : allVehicles) {
						Location position = requestVehiclePosition(vehicle);
						if (position != null) {
							double heading = requestVehicleHeading(vehicle);
							double speed = requestVehicleSpeed(vehicle);

							VehicleInformationContainer informationContainer = new VehicleInformationContainer(position, heading, speed);

							vehiclePositions.put(vehicle, informationContainer);
						}
					}
				}

				if (_positonsByTimestamp.containsKey(_step - 1)) {
					_positonsByTimestamp.remove(_step - 1);
				}
			} catch (Exception e) {
				e.printStackTrace();
			}

			return true;
		} catch (RuntimeException e) {
			throw e;
		} catch (Exception e) {
			e.printStackTrace();
		}
		return false;
	}

	private Location requestVehiclePosition(String pVehicleID) {
		if (_vehiclesOutOfRange.containsKey(pVehicleID)) {
			if (_vehiclesOutOfRange.get(pVehicleID) < _step) {
				return null;
			} else {
				_vehiclesOutOfRange.remove(pVehicleID);
			}
		}

		SumoCommand positionCommand = Vehicle.getPosition(pVehicleID);

		Object positionObject = requestObject(positionCommand);

		SumoPosition2D sumoPosition = (SumoPosition2D) positionObject;

		if (_observedAreaSet) {
			if (_startX <= sumoPosition.x && sumoPosition.x <= _endX && _startY <= sumoPosition.y && sumoPosition.y <= _endY) {
				return new PositionVector(sumoPosition.x - _startX, sumoPosition.y - _startY, 0);
			} else {
				double diffX = _startX - sumoPosition.x;
				if (diffX < 0 || sumoPosition.x - _endX < diffX) {
					diffX = sumoPosition.x - _endX;
				}
				if (diffX < 0) {
					diffX = 0;
				}
				double diffY = _startY - sumoPosition.y;
				if (diffY < 0 || sumoPosition.y - _endY < diffY) {
					diffY = sumoPosition.y - _endY;
				}
				if (diffY < 0) {
					diffY = 0;
				}
				double diff = Math.sqrt(Math.pow(diffX, 2) + Math.pow(diffY, 2));

				double timeTillNextJoin = diff / 50;

				//                _vehiclesOutOfRange.put(pVehicleID, _step + timeTillNextJoin);

				return null;
			}
		} else {
			return new PositionVector(sumoPosition.x, sumoPosition.y, 0);
		}
	}

	private double requestVehicleHeading(String pVehicleID) {
		if (_vehiclesOutOfRange.containsKey(pVehicleID)) {
			if (_vehiclesOutOfRange.get(pVehicleID) < _step) {
				return -1;
			} else {
				_vehiclesOutOfRange.remove(pVehicleID);
			}
		}

		SumoCommand angleCommand = Vehicle.getAngle(pVehicleID);

		Object angleObject = requestObject(angleCommand);


		if (angleObject != null) {
			return (Double) angleObject;
		}
		return -1;
	}

	private double requestVehicleSpeed(String pVehicleID) {
		if (_vehiclesOutOfRange.containsKey(pVehicleID)) {
			if (_vehiclesOutOfRange.get(pVehicleID) < _step) {
				return -1;
			} else {
				_vehiclesOutOfRange.remove(pVehicleID);
			}
		}

		SumoCommand speedCommand = Vehicle.getSpeed(pVehicleID);

		Object speedObject = requestObject(speedCommand);


		if (speedObject != null) {
			return (Double) speedObject;
		}
		return -1;
	}

	private List<String> requestAllVehicles() {
		SumoCommand idList = Vehicle.getIDList();
		Object object = requestObject(idList);
		SumoStringList list = (SumoStringList)object;

		List<String> result = new ArrayList<>();

		for (String vehicle : list) {
			result.add(vehicle);
		}

		return result;
	}

	@Override
	public List<String> getAllVehicles() {
		return getAllVehicles(_step);
	}

	@Override
	public List<String> getAllVehicles(double pStep) {
		Map<String, VehicleInformationContainer> map = _positonsByTimestamp.get(pStep);

		return new ArrayList<>(map.keySet());
	}

	public RoadNetworkEdge getCurrentEdge(String pVehicleID) {
		obtainRoadNetwork();

		SumoCommand roadIDCommand = Vehicle.getRoadID(pVehicleID);
		String currentRoadID = (String) requestObject(roadIDCommand);

		RoadNetworkEdge edge = _roadNetwork.getEdge(currentRoadID);
		while (edge.isInternal()) {
			if (edge.getAccessibleEdges().size() == 1) {
				edge = edge.getAccessibleEdges().get(0);
			} else {
				break;
			}
		}

		return edge;
	}

	@Override
	public RoadNetworkRoute getCurrentRoute(String pVehicleID) {
		obtainRoadNetwork();

		SumoCommand routeCommand = Vehicle.getRoute(pVehicleID);
		Object object = requestObject(routeCommand);
		SumoStringList streetList = (SumoStringList) object;

		RoadNetworkEdge currentEdge = getCurrentEdge(pVehicleID);

		if (currentEdge == null) {
			return null;
		}

		List<RoadNetworkEdge> streets = new ArrayList<>();
		boolean add = false;
		for (String street : streetList) {
			if (street.equals(currentEdge.getEdgeID())) {
				add = true;
			}
			if (add) {
				streets.add(_roadNetwork.getEdge(street));
			}
		}


		if (streets.size() == 0) {
			return new RoadNetworkRoute(new ArrayList<>());
		}

		return new RoadNetworkRoute(streets);
	}

	public Object requestObject(SumoCommand routeCommand) {
		Object object = null;
		try {
			object = _connection.do_job_get(routeCommand);
		} catch (RuntimeException e) {
			throw e;
		} catch (Exception e) {
			e.printStackTrace();
		}
		return object;
	}

	public void execute(SumoCommand routeCommand) {
		try {
			_connection.do_job_set(routeCommand);
		} catch (RuntimeException e) {
			throw e;
		} catch (Exception e) {
			e.printStackTrace();
		}
	}

	@Override
	public double getStep() {
		return _step;
	}

	@Override
	public double getStart() {
		return _start;
	}

	@Override
	public double getMaximumAvailablePrediction() {
		double max = Collections.max(_positonsByTimestamp.keySet());

		return max;
	}

	@Override
	public void setObservedArea(double pStartX, double pStartY, double pEndX, double pEndY) {
		_startX = pStartX;
		_startY = pStartY;
		_endX = pEndX;
		_endY = pEndY;

		_observedAreaSet = true;
	}

	@Override
	public double getScenarioWidth() {
		SumoCommand netBoundaryCommand = Simulation.getNetBoundary();

		try {
			SumoBoundingBox netBoundary = (SumoBoundingBox) _connection.do_job_get(netBoundaryCommand);
			return Math.max(netBoundary.x_max - netBoundary.x_min, 10);
		} catch (Exception e) {
			//Nothing to do
		}
		return -1;
	}

	@Override
	public double getScenarioHeight() {
		SumoCommand netBoundaryCommand = Simulation.getNetBoundary();

		try {
			SumoBoundingBox netBoundary = (SumoBoundingBox) _connection.do_job_get(netBoundaryCommand);
			return Math.max(netBoundary.y_max - netBoundary.y_min, 10);
		} catch (Exception e) {
			//Nothing to do
		}
		return -1;
	}

	public boolean providesRoadInformation() {
		return true;
	}

	public RoadNetworkEdge getVehicleDestination(String pVehicleID) {
		RoadNetworkRoute roadNetworkRoute = getCurrentRoute(pVehicleID);

		return roadNetworkRoute.getDestination();
	}

	@Override
	public RoadNetwork getRoadNetwork() {
		obtainRoadNetwork();

		return _roadNetwork;
	}

	@Override
	public RoadNetworkRoute findNewRoute(String pVehicle) throws NoAdditionalRouteAvailableException {
		List<RoadNetworkRoute> routes = new ArrayList<>();
		RoadNetworkRoute route = getCurrentRoute(pVehicle);

		if (route.getRoute().isEmpty()) {
			return null;
		}

		routes.add(route);
		RoadNetworkRoute findRoute = _algorithm.findRoute(_roadNetwork, route.getStart(), route.getDestination(), routes);
		if (findRoute == null) {
			throw new NoAdditionalRouteAvailableException(route.getStart(), route.getDestination(), routes);
		}
		return findRoute;
	}

	@Override
	public RoadNetworkRoute findNewRoute(String pVehicle, List<RoadNetworkEdge> pEdgesToAvoid, boolean pKeepDestination) throws NoAdditionalRouteAvailableException, NoExitAvailableException {
		if (pKeepDestination) {
			List<RoadNetworkRoute> routes = new ArrayList<>();
			RoadNetworkRoute route = getCurrentRoute(pVehicle);

			if (route.getRoute().isEmpty()) {
				return null;
			}

			routes.add(route);

			RoadNetworkRoute findRoute = _algorithm.findRoute(_roadNetwork, route.getStart(), route.getDestination(), routes, pEdgesToAvoid);
			if (findRoute == null) {
				throw new NoAdditionalRouteAvailableException(route.getStart(), route.getDestination(), routes);
			}
			return findRoute;
		} else {
			RoadNetworkRoute route = getCurrentRoute(pVehicle);

			if (route.getRoute().isEmpty()) {
				return null;
			}

			List<RoadNetworkEdge> routeList = route.getRoute();

			RoadNetworkEdge lastEdge = null;
			boolean search = false;
			outer:for (int i = routeList.size() - 1; i >= 0; i--) {
				if (search) {
					List<RoadNetworkEdge> accessibleEdges = routeList.get(i).getAccessibleEdges();
					if (accessibleEdges.size() > 1) {
						for (int j = 0; j < accessibleEdges.size(); j++) {
							if (!accessibleEdges.get(j).equals(lastEdge)) {
								List<RoadNetworkEdge> edges = new ArrayList<>();

								for (int k = 0; k <= i; k++) {
									edges.add(routeList.get(k));
								}
								edges.add(accessibleEdges.get(j));

								List<RoadNetworkEdge> newPaths;
								while ((newPaths = edges.get(edges.size() - 1).getAccessibleEdges()).size() > 0) {
									edges.add(newPaths.get(_random.nextInt(newPaths.size())));
								}

								for (RoadNetworkEdge roadNetworkEdge : edges) {
									if (pEdgesToAvoid.contains(roadNetworkEdge)) {
										continue outer;
									}
								}

								return new RoadNetworkRoute(edges);
							}
						}
					}
				}
				search = true;
				lastEdge = routeList.get(i);
			}
			throw new NoExitAvailableException(route.getStart(), route.getDestination());
		}
	}

	@Override
	public void rerouteVehicle(String pVehicle, RoadNetworkRoute pRoute) {
		SumoStringList routeEdges = new SumoStringList();
		for (RoadNetworkEdge edge : pRoute.getRoute()) {
			routeEdges.add(edge.getEdgeID());
		}
		execute(Vehicle.setRoute(pVehicle, routeEdges));
	}

	@Override
	public void stopVehicle(String pVehicle) {
		SumoCommand stopCommand = Vehicle.setSpeed(pVehicle, 0);
		execute(stopCommand);
	}

	public void obtainRoadNetwork() {
		if (_roadNetwork == null) {
			if (TEMP_FILE.exists()) {
				try {
					ObjectInputStream inputStream = new ObjectInputStream(new FileInputStream(TEMP_FILE));

					Object readObject = inputStream.readObject();

					if (readObject instanceof SerializableRoadNetwork) {
						SerializableRoadNetwork serializedRoadNetwork = (SerializableRoadNetwork) readObject;
						_roadNetwork = new RoadNetwork(serializedRoadNetwork, this);
					}

					inputStream.close();
				} catch (IOException | ClassNotFoundException e) {
					//Nothing to do
				}
			}

			if (_roadNetwork != null) {
				System.out.println("Got network from cache");

				SumoCommand edgeIDCommand = Edge.getIDList();

				SumoStringList edgeIDStringList = (SumoStringList) requestObject(edgeIDCommand);

				if (_roadNetwork.getAvailableEdges().size() == edgeIDStringList.size()) {
					boolean matching = true;

					for (String edgeID : edgeIDStringList) {
						if (_roadNetwork.getEdge(edgeID) == null) {
							matching = false;
						}
					}

					if (matching) {
						return;
					}
				}
			}
			SumoCommand laneIDCommand = Lane.getIDList();

			Map<String, RoadNetworkEdge> roadNetwork = new HashMap<>();

			SumoStringList laneIDStringList = (SumoStringList) requestObject(laneIDCommand);

			//Requesting all lanes from sumo.
			for (String laneID : laneIDStringList) {
				SumoCommand edgeIDCommand = Lane.getEdgeID(laneID);

				SumoLinkList linkStringList = (SumoLinkList) requestObject(Lane.getLinks(laneID));

				double angle = getLaneAngle(laneID);

				double maxSpeed = getMaxSpeed(laneID);

				if (linkStringList.size() > 0) {
					String edgeID = (String) requestObject(edgeIDCommand);

					if (!roadNetwork.containsKey(edgeID)) {
						roadNetwork.put(edgeID, new RoadNetworkEdge(edgeID, angle, this));
					}

					RoadNetworkEdge edge = roadNetwork.get(edgeID);

					edge.setOriginalMaxSpeed(maxSpeed);
					edge.addLane(new RoadNetworkLane(laneID));

					for (SumoLink link : linkStringList) {
						String notInternalLane = link.notInternalLane;
						String connectedEdge = (String) requestObject(Lane.getEdgeID(notInternalLane));

						double linkAngle = getLaneAngle(laneID);

						if (!roadNetwork.containsKey(connectedEdge)) {
							RoadNetworkEdge roadNetworkEdge = new RoadNetworkEdge(connectedEdge, linkAngle, this);
							roadNetwork.put(connectedEdge, roadNetworkEdge);
						}

						edge.addConnectedEdge(roadNetwork.get(connectedEdge));
					}
				}


			}
			_roadNetwork = new RoadNetwork(roadNetwork);

			try {
				ObjectOutputStream outputStream = new ObjectOutputStream(new FileOutputStream(TEMP_FILE));

				outputStream.writeObject(new SerializableRoadNetwork(_roadNetwork));

				outputStream.flush();
				outputStream.close();
			} catch (IOException e) {
				//Nothing to do
				e.printStackTrace();
			}
		}
	}

	public double getMaxSpeed(String laneID) {
		SumoCommand maxSpeedCommand = Lane.getMaxSpeed(laneID);
		double maxSpeed = (Double) requestObject(maxSpeedCommand);
		return maxSpeed;
	}

	public double getLaneAngle(String laneID) {
		SumoCommand shapeCommand = Lane.getShape(laneID);
		SumoGeometry geometry = (SumoGeometry) requestObject(shapeCommand);

		LinkedList<SumoPosition2D> coords = geometry.coords;
		SumoPosition2D start = coords.getFirst();
		SumoPosition2D end = coords.getLast();

		double sin = (end.y - start.y) / Math.sqrt(Math.pow(end.x - start.x, 2) + Math.pow(end.y - start.y, 2));

		double angle = Math.asin(sin) * 180 / Math.PI;
		if (end.x - start.x < 0) {
			angle += 180;
		}
		return angle;
	}

	@Override
	public double getVehicleSpeed(String pVehicleID) {
		SumoCommand speedCommand = Vehicle.getSpeed(pVehicleID);
		Object object = requestObject(speedCommand);

		return (Double)object;
	}

	@Override
	public void setEdgeMaxSpeed(String pEdgeID, double pMaxSpeed) {
		SumoCommand setMaxSpeedCommand = Edge.setMaxSpeed(pEdgeID, pMaxSpeed);

		execute(setMaxSpeedCommand);

		RoadNetworkEdge edge = getRoadNetwork().getEdge(pEdgeID);

		edge.setMaxSpeed(pMaxSpeed);
	}

}