feat(fiss): enable VehicleBehavior.wait for FISS (matsim-org#4885)

* Make QLinkI.makeVehicleAvailableToNextDriver accessible

* fix(fiss): fix instantaneous vehicle teleportation in FISS

* test(fiss): add vehicle behavior wait scenario to FISS integration tests

Author: marecabo

contribs/drt-extensions/src/main/java/org/matsim/contrib/drt/extension/fiss/FISS.java

@@ -26,18 +26,23 @@
 import org.matsim.core.gbl.MatsimRandom;
 import org.matsim.core.mobsim.dsim.DistributedDepartureHandler;
 import org.matsim.core.mobsim.framework.MobsimAgent;
+import org.matsim.core.mobsim.framework.MobsimDriverAgent;
 import org.matsim.core.mobsim.framework.PlanAgent;
 import org.matsim.core.mobsim.qsim.InternalInterface;
 import org.matsim.core.mobsim.qsim.TeleportationEngine;
 import org.matsim.core.mobsim.qsim.interfaces.DepartureHandler;
 import org.matsim.core.mobsim.qsim.interfaces.MobsimEngine;
 import org.matsim.core.mobsim.qsim.pt.TransitDriverAgent;
 import org.matsim.core.mobsim.qsim.qnetsimengine.NetworkModeDepartureHandler;
+import org.matsim.core.mobsim.qsim.qnetsimengine.QLinkI;
+import org.matsim.core.mobsim.qsim.qnetsimengine.QNetsimEngineI;
+import org.matsim.core.mobsim.qsim.qnetsimengine.QVehicle;
 import org.matsim.core.population.routes.NetworkRoute;
 import org.matsim.core.router.util.TravelTime;
 import org.matsim.vehicles.Vehicle;
 import org.matsim.vehicles.VehicleType;
 
+import java.util.PriorityQueue;
 import java.util.Random;
 
 /**
@@ -77,12 +82,15 @@ public class FISS implements NetworkModeDepartureHandler, DistributedDepartureHa
 	private final MatsimServices matsimServices;
 	private final QSimConfigGroup qsimConfig;
 	private final Scenario scenario;
+	private final QNetsimEngineI qNetsimEngine;
 
+	private final PriorityQueue<VehicleArrivalEntry> vehicleArrivals = new PriorityQueue<>();
 
 	@Inject
 	FISS(MatsimServices matsimServices, Scenario scenario, FISSConfigGroup fissConfigGroup, Network network, TeleportationEngine teleport,
 		 @Named(TransportMode.car) TravelTime travelTime,
-		 @Named("base-network-mode-departure-handler") NetworkModeDepartureHandler networkModeDepartureHandler) {
+		 @Named("base-network-mode-departure-handler") NetworkModeDepartureHandler networkModeDepartureHandler,
+		 QNetsimEngineI qNetsimEngine) {
 		this.delegate = networkModeDepartureHandler;
 		this.fissConfigGroup = fissConfigGroup;
 		this.network = network;
@@ -92,6 +100,7 @@ public class FISS implements NetworkModeDepartureHandler, DistributedDepartureHa
 		this.scenario = scenario;
 		this.qsimConfig = scenario.getConfig().qsim();
 		this.random = MatsimRandom.getLocalInstance();
+		this.qNetsimEngine = qNetsimEngine;
 	}
 
 	@Override
@@ -109,53 +118,56 @@ public boolean handleDeparture(double now, MobsimAgent agent, Id<Link> linkId) {
 		}
 
 		// This updates the travel time.  Teleportation departure is handled further down.
+		double newTravelTime = 0.;
+		Id<Link> destinationLinkId = agent.getDestinationLinkId();
 		if (agent instanceof PlanAgent planAgent) {
 			Leg currentLeg = (Leg) planAgent.getCurrentPlanElement();
 			NetworkRoute networkRoute = (NetworkRoute) currentLeg.getRoute();
 			Person person = planAgent.getCurrentPlan().getPerson();
 			Vehicle vehicle = this.scenario.getVehicles().getVehicles().get(networkRoute.getVehicleId());
 
-			double newTravelTime = calcQSimTravelTime(networkRoute, now, person, vehicle);
+			newTravelTime = calcQSimTravelTime(networkRoute, now, person, vehicle);
 			LOG.debug("New travelTime: {}, was {}", newTravelTime, networkRoute.getTravelTime().orElseGet(() -> Double.NaN));
 
 			networkRoute.setTravelTime(newTravelTime);
 		}
 
-		// !!!! The following is not a teleportation-pull (as it is in NetworkModeDpHandler...), but a teleportation-push.  yyyy Are we sure
-		// that these are the same?  E.g. if the vehicle needs to be made available for the next driver?  kai, feb'25
-
-//		// remove vehicle of teleported agent from parking spot
-//		QVehicle removedVehicle = null;
-//		if (agent instanceof MobsimDriverAgent driverAgent) {
-//			Id<Vehicle> vehicleId = driverAgent.getPlannedVehicleId();
-//			QLinkI qLinkI = (QLinkI) this.qNetsimEngine.getNetsimNetwork().getNetsimLink(linkId);
-//			removedVehicle = qLinkI.removeParkedVehicle(vehicleId);
-//			if (removedVehicle == null) {
-//				throw new RuntimeException(
-//						"Could not remove parked vehicle with id " + vehicleId + " on the link id "
-// 								+ qNetsimEngine.getVehicles().get(vehicleId).getCurrentLinkId()
-//								+ ".  Maybe it is currently used by someone else?"
-//								+ " (In which case ignoring this exception would lead to duplication of this vehicle.) "
-//								+ "Maybe was never placed onto a link?");
-//			}
-//			// yyyy need to check how this is handled elsewhere.  I think that with some config settings we wait until the vehicle becomes available--???
-//		}
+		// Remove vehicle from departure link and schedule delayed arrival at
+		// destination.
+		if (agent instanceof MobsimDriverAgent driverAgent) {
+			Id<Vehicle> vehicleId = driverAgent.getPlannedVehicleId();
+			QLinkI qLinkI = (QLinkI) this.qNetsimEngine.getNetsimNetwork().getNetsimLink(linkId);
+			QVehicle removedVehicle = qLinkI.removeParkedVehicle(vehicleId);
+			if (removedVehicle == null) {
+				// Vehicle not yet available on this link — may still be in transit from another
+				// teleported agent's trip. Fall back to standard departure handling: with
+				// VehicleBehavior.wait the agent will wait until the vehicle arrives via
+				// doSimStep.
+				LOG.info(
+						"Vehicle {} not found on link {} for agent {} at time {}. Falling back to standard departure (agent will wait for vehicle).",
+						vehicleId, linkId, driverAgent.getId(), now);
+				return delegate.handleDeparture(now, agent, linkId);
+			}
+			double arrivalTime = now + newTravelTime;
+			vehicleArrivals.add(new VehicleArrivalEntry(arrivalTime, removedVehicle, destinationLinkId));
+		}
+
 		boolean result = teleport.handleDeparture(now, agent, linkId);
 		Gbl.assertIf(result); // otherwise we are now confused
 
-		// teleport vehicle right after agent
-//		if (removedVehicle != null) {
-//			Id<Link> destinationLinkId = agent.getDestinationLinkId();
-//			QLinkI qLinkDest = (QLinkI) this.qNetsimEngine.getNetsimNetwork().getNetsimLink(destinationLinkId);
-//			qLinkDest.addParkedVehicle(removedVehicle);
-//		}
-
 		return result;
 
 	}
 
 	@Override
 	public void doSimStep(double time) {
+		// Deliver vehicles that have reached their arrival time.
+		while (!vehicleArrivals.isEmpty() && vehicleArrivals.peek().arrivalTime <= time) {
+			VehicleArrivalEntry entry = vehicleArrivals.poll();
+			QLinkI qLinkDest = (QLinkI) this.qNetsimEngine.getNetsimNetwork().getNetsimLink(entry.destinationLinkId);
+			qLinkDest.addParkedVehicle(entry.vehicle);
+			qLinkDest.makeVehicleAvailableToNextDriver(entry.vehicle);
+		}
 		teleport.doSimStep(time);
 	}
 
@@ -248,4 +260,12 @@ private boolean switchOffFISS() {
 	public void setInternalInterface(InternalInterface internalInterface) {
 		teleport.setInternalInterface(internalInterface);
 	}
+
+	private record VehicleArrivalEntry(double arrivalTime, QVehicle vehicle,
+			Id<Link> destinationLinkId) implements Comparable<VehicleArrivalEntry> {
+		@Override
+		public int compareTo(VehicleArrivalEntry o) {
+			return Double.compare(this.arrivalTime, o.arrivalTime);
+		}
+	}
 }

contribs/drt-extensions/src/main/java/org/matsim/contrib/drt/extension/fiss/FISSConfigGroup.java

@@ -51,14 +51,10 @@ public FISSConfigGroup() {
 		super.checkConsistency( config );
 
 		switch( config.qsim().getVehicleBehavior() ){
-			case teleport -> {
+			case teleport, wait -> {
 			}
 			default -> {
-				throw new RuntimeException( "FISS only works together with vehicle behavior=teleport.  See code for more info." );
-				// This was previously implemented such that it also ran through with other settings.  However, it would teleport the
-				// vehicle immediately to its destination, thus leading to a faulty physical modelling of "wait" or "exception".   I
-				// can't say if a possibly waiting agent would wait for the driver of the vehicle, or for the vehicle itself; this
-				// would need to be checked.  kai, feb'25
+				throw new RuntimeException( "FISS only works together with vehicle behavior teleport or wait." );
 			}
 		}