TraciSimulationController.java

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

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 de.tud.kom.p2psim.impl.topology.movement.vehicular.information.LocationUtils;
import de.tud.kom.p2psim.impl.topology.movement.vehicular.information.Position;
import de.tud.kom.p2psim.impl.topology.movement.vehicular.sumo.simulation.controller.SimulationSetupExtractor;
import de.tud.kom.p2psim.impl.topology.movement.vehicular.sumo.simulation.controller.VehicleController;
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.RoadNetworkRoute;
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.BreathFirstSearchRoutingAlgorithm;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.routing.RoutingAlgorithm;
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;

public class TraciSimulationController implements VehicleController, SimulationSetupExtractor {

	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, Position>> _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 BreathFirstSearchRoutingAlgorithm();

	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) {
			_connection = new SumoTraciConnection(_sumoExe, _configFile);

			try {
				_connection.runServer();

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

			Runnable shutdownHook = new Runnable() {

				@Override
				public void run() {
					_connection.close();
				}
			};

			Runtime.getRuntime().addShutdownHook(new Thread(shutdownHook));

			_initalized = true;
		}
	}

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

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

		return map.get(pVehicleID);
	}

	@Override
	public List<Position> getAllIntersections(boolean pCluster) {
		List<Position> 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 Position(sumoPosition.x - _startX, sumoPosition.y - _startY, 0, 0));
				}
			} else {
				result.add(new Position(sumoPosition.x, sumoPosition.y, 0, 0));
			}
		}

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

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

				for (int j = 0; j < tempResult.size(); j++) {
					Position 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() {
		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, Position> vehiclePositions = new HashMap<>();
					_positonsByTimestamp.put(newStep, vehiclePositions);

					List<String> allVehicles = requestAllVehicles();
					for (String vehicle : allVehicles) {
						Position vehiclePosition = requestVehiclePosition(vehicle);
						if (vehiclePosition != null) {
							vehiclePositions.put(vehicle, vehiclePosition);
						}
					}
				}

				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 Position requestVehiclePosition(String pVehicleID) {
		if (_vehiclesOutOfRange.containsKey(pVehicleID)) {
			if (_vehiclesOutOfRange.get(pVehicleID) < _step) {
				return null;
			} else {
				_vehiclesOutOfRange.remove(pVehicleID);
			}
		}

		SumoCommand positionCommand = Vehicle.getPosition(pVehicleID);
		SumoCommand angleCommand = Vehicle.getAngle(pVehicleID);
		SumoCommand speedCommand = Vehicle.getSpeed(pVehicleID);

		Object positionObject = requestObject(positionCommand);
		Object angleObject = requestObject(angleCommand);
		Object speedObject = requestObject(speedCommand);

		SumoPosition2D sumoPosition = (SumoPosition2D) positionObject;
		double angle = (Double) angleObject;
		double speed = (Double) speedObject;

		if (_observedAreaSet) {
			if (_startX <= sumoPosition.x && sumoPosition.x <= _endX && _startY <= sumoPosition.y && sumoPosition.y <= _endY) {
				return new Position(sumoPosition.x - _startX, sumoPosition.y - _startY, 0, angle, speed);
			} 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 Position(sumoPosition.x, sumoPosition.y, 0, angle);
		}
	}

	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, Position> map = _positonsByTimestamp.get(pStep);

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

	@Override
	public RoadNetworkRoute getCurrentRoute(String pVehicleID) {
		SumoCommand routeCommand = Vehicle.getRoute(pVehicleID);
		Object object = requestObject(routeCommand);
		SumoStringList streetList = (SumoStringList) object;

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

		obtainRoadNetwork();

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


		if (streets.size() == 0) {
			for (String street : streetList) {
				streets.add(_roadNetwork.getEdge(street));
			}
		}

		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();
	}

	public RoadNetwork getRoadNetwork() {
		obtainRoadNetwork();

		return _roadNetwork;
	}

	public RoadNetworkRoute findNewRoute(String pVehicle) throws NoAdditionalRouteAvailableException {
		List<RoadNetworkRoute> routes = new ArrayList<>();
		RoadNetworkRoute route = getCurrentRoute(pVehicle);
		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;
	}

	public RoadNetworkRoute findNewRoute(String pVehicle, List<RoadNetworkEdge> pEdgesToAvoid, boolean pKeepDestination) throws NoAdditionalRouteAvailableException, NoExitAvailableException {
		if (pKeepDestination) {
			List<RoadNetworkRoute> routes = new ArrayList<>();
			RoadNetworkRoute route = getCurrentRoute(pVehicle);
			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);
			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));

								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());
		}
	}

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

	public void obtainRoadNetwork() {
		if (_roadNetwork == null) {
			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));
					}

					RoadNetworkEdge edge = roadNetwork.get(edgeID);

					edge.setMaxSpeed(maxSpeed);
					edge.increaseLaneAmount();

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

						double linkAngle = getLaneAngle(laneID);

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

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


			}
			_roadNetwork = new RoadNetwork(roadNetwork);
		}
	}

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

}