VehicleMovementModel.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;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.Queue;
import java.util.Set;

import de.tud.kom.p2psim.api.network.SimNetInterface;
import de.tud.kom.p2psim.api.topology.movement.MovementModel;
import de.tud.kom.p2psim.api.topology.movement.SimLocationActuator;
import de.tud.kom.p2psim.impl.network.routed.RoutedNetLayer;
import de.tud.kom.p2psim.impl.topology.PositionVector;
import de.tud.kom.p2psim.impl.topology.movement.vehicular.sumo.simulation.controller.traci.TraciSimulationController;
import de.tud.kom.p2psim.impl.topology.movement.vehicular.sumo.simulation.controller.xml.XMLSimulationController;
import de.tud.kom.p2psim.impl.vehicular.DefaultVehicleInformationComponent;
import de.tudarmstadt.maki.simonstrator.api.Event;
import de.tudarmstadt.maki.simonstrator.api.EventHandler;
import de.tudarmstadt.maki.simonstrator.api.Host;
import de.tudarmstadt.maki.simonstrator.api.Time;
import de.tudarmstadt.maki.simonstrator.api.common.graph.INodeID;
import de.tudarmstadt.maki.simonstrator.api.component.ComponentNotAvailableException;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.future_location.FutureLocationSensor;
import de.tudarmstadt.maki.simonstrator.api.component.sensor.location.Location;
import de.tudarmstadt.maki.simonstrator.api.component.sis.SiSComponent;
import de.tudarmstadt.maki.simonstrator.api.component.sis.SiSDataCallback;
import de.tudarmstadt.maki.simonstrator.api.component.sis.SiSInfoProperties;
import de.tudarmstadt.maki.simonstrator.api.component.sis.SiSInformationProvider.SiSProviderHandle;
import de.tudarmstadt.maki.simonstrator.api.component.sis.exception.InformationNotAvailableException;
import de.tudarmstadt.maki.simonstrator.api.component.sis.type.SiSTypes;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.VehicleInformationComponent;
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.Position;
import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetwork;
import de.tudarmstadt.maki.simonstrator.api.util.XMLConfigurableConstructor;

public class VehicleMovementModel implements MovementModel, EventHandler, FutureLocationSensor {

	private static VehicleMovementModel MOVEMENT;

	private long timeBetweenMoveOperations;

	private final List<SimLocationActuator> components;

	private final Queue<SimLocationActuator> freeComponents;

	private final Map<String, SimLocationActuator> idComponentMatcher;

	private final Map<INodeID, String> hostVehicleIDMatching = new HashMap<>();

	private final int offsetX;
	private final int offsetY;
	private final int width;
	private final int height;

	private double scenarioWidth = 0;
	private double scenarioHeight = 0;

	private final String sumoExe;
	private final String sumoConfigFile;
	private final String sumoTrace;
	private String sumoIntersections;

	private final long timestepConversion = Time.SECOND;

	private boolean initialized = false;

	private VehicleController _controller;
	private SimulationSetupExtractor _extractor;

	private List<Position> intersections;


	/**
	 * Constructor for the movement model using the sumo TraCI API
	 * @param timeBetweenMoveOperations The time between two movement operations.
	 * @param sumoExe The path to the executable of sumo
	 * @param sumoConfigFile The path to the configuration file of the scenario
	 * @param offsetX The offset that should be used. If no offset is required, offset 0 shall be used.
	 * @param offsetY The offset that should be used. If no offset is required, offset 0 shall be used.
	 * @param width The width of the scenario.
	 * @param height The height of the scenario.
	 */
	@XMLConfigurableConstructor({ "timeBetweenMoveOperations", "sumoExe", "sumoConfigFile", "offsetX", "offsetY", "width", "height" })
	public VehicleMovementModel(long timeBetweenMoveOperations, String sumoExe, String sumoConfigFile, String offsetX, String offsetY, String width, String height) {
		MOVEMENT = this;

		this.timeBetweenMoveOperations = timeBetweenMoveOperations;
		this.components = new LinkedList<>();
		this.freeComponents = new LinkedList<>();
		this.idComponentMatcher = new HashMap<>();
		this.sumoExe = sumoExe;
		this.sumoConfigFile = sumoConfigFile;
		this.sumoTrace = null;
		this.offsetX = Integer.parseInt(offsetX);
		this.offsetY = Integer.parseInt(offsetY);
		this.width = Integer.parseInt(width);
		this.height = Integer.parseInt(height);
	}

	/**
	 * Constructor for the movement model using the a generated sumo trace file
	 * @param timeBetweenMoveOperations The time between two movement operations.
	 * @param sumoTrace The path to the vehicle movement file (*.xml)
	 * @param sumoIntersections The path to the intersections file (*.csv)
	 * @param offsetX The offset that should be used. If no offset is required, offset 0 shall be used.
	 * @param offsetY The offset that should be used. If no offset is required, offset 0 shall be used.
	 * @param width The width of the scenario.
	 * @param height The height of the scenario.
	 */
	@XMLConfigurableConstructor({ "timeBetweenMoveOperations", "sumoTrace", "sumoIntersections", "offsetX", "offsetY", "width", "height" })
	public VehicleMovementModel(long timeBetweenMoveOperations, String sumoTrace, String sumoIntersections, int offsetX, int offsetY, int width, int height) {
		MOVEMENT = this;

		this.timeBetweenMoveOperations = timeBetweenMoveOperations;
		this.components = new LinkedList<>();
		this.freeComponents = new LinkedList<>();
		this.idComponentMatcher = new HashMap<>();
		this.sumoExe = null;
		this.sumoConfigFile = null;
		this.sumoTrace = sumoTrace;
		this.sumoIntersections = sumoIntersections;
		this.offsetX = offsetX;
		this.offsetY = offsetY;
		this.width = width;
		this.height = height;
	}

	/**
	 * Adding an additional component to be moved by this movement model
	 * @param comp The component to be added.
	 */
	@Override
	public final void addComponent(SimLocationActuator comp) {
		components.add(comp);
		freeComponents.add(comp);
	}

	/**
	 * Returns the time between movement operations
	 * @return time between movement operations
	 */
	public long getTimeBetweenMoveOperations() {
		return timeBetweenMoveOperations;
	}

	/**
	 * Set the time between movement operations
	 * @param time the time between movement operations
	 */
	@Override
	public void setTimeBetweenMoveOperations(long time) {
		this.timeBetweenMoveOperations = time;
	}

	/**
	 * Place a component at the correct location
	 * @param actuator The component to be placed.
	 */
	@Override
	public void placeComponent(SimLocationActuator actuator) {
		if (!initialized) {
			initializeModel();
			initialized = true;
		}
		// Initial placement
		actuator.updateCurrentLocation(new PositionVector(Double.NaN, Double.NaN));

		try {
			final SiSComponent sis = actuator.getHost().getComponent(SiSComponent.class);
			sis.provide().nodeState(SiSTypes.FUTURE_PHY_LOCATION,
					new SiSDataCallback<Location>() {

				Set<INodeID> localID = INodeID
						.getSingleIDSet(actuator.getHost().getId());

				@Override
				public Location getValue(INodeID nodeID,
						SiSProviderHandle providerHandle)
								throws InformationNotAvailableException {
					if (nodeID.equals(actuator.getHost().getId())) {
						return getFutureLocation(actuator.getHost());
					} else {
						throw new InformationNotAvailableException();
					}
				}

				@Override
				public Set<INodeID> getObservedNodes() {
					return localID;
				}

				@Override
				public SiSInfoProperties getInfoProperties() {
					return new SiSInfoProperties();
				}
			});
		} catch (ComponentNotAvailableException e) {
			// Nothing to do
		}
	}

	/**
	 * Initializes the movement model by executing BonnMotion and parsing the
	 * resulting movement trace.
	 */
	protected void initializeModel() {
		// Schedule first step
		if (!initialized) {
			Event.scheduleWithDelay(timeBetweenMoveOperations, this, null, 0);

			if (sumoExe != null) {
				TraciSimulationController simulationController = TraciSimulationController.createSimulationController(sumoExe, sumoConfigFile);
				_controller = simulationController;
				_controller.setObservedArea(offsetX, offsetY, offsetX + width, offsetY + height);
				_controller.init();
				_controller.nextStep();

				_extractor = simulationController;
			} else {
				XMLSimulationController simulationController;
				if (sumoIntersections == null || sumoIntersections.equals("")) {
					simulationController = new XMLSimulationController(sumoTrace);
				} else {
					simulationController = new XMLSimulationController(sumoTrace, sumoIntersections);
				}

				_controller = simulationController;
				_controller.setObservedArea(offsetX, offsetY, offsetX + width, offsetY + height);
				_controller.init();
				_controller.nextStep();

				_extractor = simulationController;
			}
			intersections = _extractor.getAllIntersections(true);

			scenarioWidth = _extractor.getScenarioWidth();
			scenarioHeight = _extractor.getScenarioHeight();

			System.out.println("Initialization: done.");
		}
	}

	/**
	 * Used for the periodical updates of the vehicle positions
	 * @param content not used in this case, should be null
	 * @param type not used in this case, should be 0
	 */
	@Override
	public void eventOccurred(Object content, int type) {
		/*
		 * One event for all nodes (synchronized movement), as this boosts
		 * simulation performance due to less recalculations in the network
		 * models.
		 */
		long currentTime = Time.getCurrentTime() / timestepConversion;

		while (_controller.getStep() - _controller.getStart() < currentTime) {
			_controller.nextStep();
		}

		List<String> allVehicles = _controller.getAllVehicles();

		for (int i = 0; i < allVehicles.size(); i++) {
			String vehicle = allVehicles.get(i);

			Position position = _controller.getVehiclePosition(vehicle);

			if (position == null) {
				allVehicles.remove(i--);
				continue;
			}

			SimLocationActuator component = requestSimActuator(vehicle);
			if (scenarioHeight != -1) {
				component.updateCurrentLocation(new PositionVector(position.getX(), Math.min(scenarioHeight, height) - position.getY()));
			} else {
				// This would be vertically mirrored
				component.updateCurrentLocation(new PositionVector(position.getX(), position.getY()));
			}

			component.setMovementSpeed(position.getSpeed());

			try {
				RoutedNetLayer routedNetLayer = component.getHost().getComponent(RoutedNetLayer.class);
				for (SimNetInterface netInterface : routedNetLayer.getSimNetworkInterfaces()) {
					if (netInterface.isOffline()) {
						netInterface.goOnline();
					}
				}
			} catch (ComponentNotAvailableException e) {
				e.printStackTrace();
			}
		}

		if (allVehicles.size() != idComponentMatcher.size()) {
			ArrayList<String> registeredVehicles = new ArrayList<>(idComponentMatcher.keySet());
			for (int i = 0; i < registeredVehicles.size(); i++) {
				String vehicle = registeredVehicles.get(i);
				if (!allVehicles.contains(vehicle)) {
					addFreeHost(vehicle);
				}
			}
		}

		freeComponents.forEach((component) -> {
			try {
				RoutedNetLayer routedNetLayer = component.getHost().getComponent(RoutedNetLayer.class);
				for (SimNetInterface netInterface : routedNetLayer.getSimNetworkInterfaces()) {
					if (netInterface.isOnline()) {
						netInterface.goOffline();
					}
				}
			} catch (ComponentNotAvailableException e) {
				e.printStackTrace();
			}
			component.updateCurrentLocation(new PositionVector(Double.NaN, Double.NaN));
		});

		// Reschedule next step
		Event.scheduleWithDelay(timeBetweenMoveOperations, this, null, 0);
	}

	/**
	 * Remove a vehicle from the set of moved components as the vehicle has stopped driving
	 * @param vehicleID The stopped vehicle
	 */
	private void addFreeHost(String vehicleID) {
		if (idComponentMatcher.containsKey(vehicleID)) {
			SimLocationActuator simLocationActuator = idComponentMatcher.remove(vehicleID);
			if (simLocationActuator != null) {
				try {
					VehicleInformationComponent vehicularHostComponent = simLocationActuator.getHost().getComponent(VehicleInformationComponent.class);
					vehicularHostComponent.resetVehicleID();
				} catch (ComponentNotAvailableException e) {
					// Nothing to do here
				}

				freeComponents.add(simLocationActuator);
				hostVehicleIDMatching.remove(simLocationActuator.getHost().getId());
			}
		}
	}

	/**
	 * Request a component for a vehicle. If no component has been assigned to the vehicle yet, a new component is assigned.
	 * @param vehicle The vehicle for which a component is requested.
	 * @throws RuntimeException If no component can be assigned.
	 * @return The component for the vehicle.
	 */
	private SimLocationActuator requestSimActuator(String vehicle) {
		if (!idComponentMatcher.containsKey(vehicle)) {
			SimLocationActuator simLocationActuator = freeComponents.poll();

			if (simLocationActuator != null) {
				VehicleInformationComponent vehicularHostComponent;
				try {
					vehicularHostComponent = simLocationActuator.getHost().getComponent(VehicleInformationComponent.class);
				} catch (ComponentNotAvailableException e) {
					Host host = simLocationActuator.getHost();
					vehicularHostComponent = new DefaultVehicleInformationComponent(host, _controller, _extractor);
					host.registerComponent(vehicularHostComponent);
				}
				vehicularHostComponent.setVehicleID(vehicle);

				idComponentMatcher.put(vehicle, simLocationActuator);
				hostVehicleIDMatching.put(simLocationActuator.getHost().getId(), vehicle);
			} else {
				throw new RuntimeException("Unable to assign new components. Please increase node amount.");
			}
		}
		return idComponentMatcher.get(vehicle);
	}

	public static RoadNetwork getRoadNetwork() {
		return MOVEMENT._extractor.getRoadNetwork();
	}

	/**
	 * Returns the future location of a certain host.
	 * @param pHost The host for which the location is being requested.
	 * @return The future location of the host.
	 */
	@Override
	public Location getFutureLocation(Host pHost) {
		if (hostVehicleIDMatching.containsKey(pHost.getId())) {
			String vehicleID = hostVehicleIDMatching.get(pHost.getId());
			Position vehiclePosition = _controller.getVehiclePosition(_controller.getMaximumAvailablePrediction(), vehicleID);
			if (vehiclePosition != null) {
				return new PositionVector(vehiclePosition.getX(), vehiclePosition.getY());
			}
		}
		return null;
	}

}