core: stdcm: fix priority between stop and departure delay

core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMPathfinding.kt

@@ -262,6 +262,7 @@ class STDCMPathfinding(
                             timeOfNextConflictAtLocation = 0.0,
                             totalRunningTime = 0.0,
                             stopTimeData = listOf(),
+                            maxFirstDepartureDelaying = maxDepartureDelay,
                         ),
                         0.0,
                         explorer as InfraExplorerWithEnvelope,

core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/STDCMPostProcessing.kt

@@ -170,14 +170,19 @@ class STDCMPostProcessing(private val graph: STDCMGraph) {
                 nodes.subList(firstPlannedNodeIndex + 1, nodes.size),
                 mutableStopData,
             )
-        val actualStopAddedTime = min(maxAddedTime, timeDiff)
+        var actualStopAddedTime = min(maxAddedTime, timeDiff)
 
-        // Add time to the previous stop, or delay the departure time accordingly
-        if (lastStopIndexBeforeNode == 0)
-            timeData = timeData.copy(departureTime = timeData.departureTime + actualStopAddedTime)
-        else
+        // Add time to the previous stop, or delay the departure time accordingly.
+        // We prefer delaying the departure time when possible.
+        val addedDepartureDelay =
+            min(actualStopAddedTime, timeData.maxDepartureDelayingWithoutConflict)
+        timeData = timeData.copy(departureTime = timeData.departureTime + addedDepartureDelay)
+        actualStopAddedTime -= addedDepartureDelay
+
+        if (actualStopAddedTime > 0) {
             mutableStopData[lastStopIndexBeforeNode - 1] =
                 mutableStopData[lastStopIndexBeforeNode - 1].withAddedStopTime(actualStopAddedTime)
+        }
 
         // Reduce time to the next stops, to keep the change as local as possible
         reduceNextStopDurations(

core/src/main/kotlin/fr/sncf/osrd/stdcm/graph/TimeData.kt

@@ -1,5 +1,7 @@
 package fr.sncf.osrd.stdcm.graph
 
+import com.google.common.primitives.Doubles.min
+import kotlin.Double.Companion.POSITIVE_INFINITY
 import kotlin.math.max
 
 /**
@@ -23,11 +25,19 @@ data class TimeData(
     /**
      * We can delay departure time from either the train start or stops. This value represents how
      * much more delay we can add to the last departure without causing any conflict. The delay
-     * would be added to the departure time of the last stop, or to the global departure time if no
-     * stop has been reached yet.
+     * would be added to the departure time of the last stop, or to the global departure time. We
+     * first delay the departure time whenever possible, then lengthen stop durations if it's not
+     * enough.
      */
     val maxDepartureDelayingWithoutConflict: Double,
 
+    /**
+     * This value describes how much delay we can add to the train departure time without causing
+     * any conflict (from the departure to the current point). This is the preferred method of
+     * delaying when the train reaches the current point.
+     */
+    val maxFirstDepartureDelaying: Double,
+
     /**
      * Time of the next conflict on the given location. Used both to identify edges that go through
      * the same "opening", and to figure out how much delay we can add locally (with margins).
@@ -96,6 +106,8 @@ data class TimeData(
             totalRunningTime = totalRunningTime + extraTravelTime,
             stopTimeData = newStopData,
             maxDepartureDelayingWithoutConflict = maxDepartureDelayingWithoutConflict,
+            maxFirstDepartureDelaying =
+                min(maxFirstDepartureDelaying, (maxAdditionalStopTime ?: POSITIVE_INFINITY)),
         )
     }
 
@@ -123,17 +135,22 @@ data class TimeData(
         assert(timeShift >= delayAddedToLastDeparture)
         var newStopData = stopTimeData
         var newDepartureTime = departureTime
+        var newMaxFirstDepartureDelaying = maxFirstDepartureDelaying
         if (delayAddedToLastDeparture > 0) {
-            if (newStopData.isEmpty()) {
-                newDepartureTime += delayAddedToLastDeparture
-            } else {
+            val firstDepartureTimeDelay = min(maxFirstDepartureDelaying, delayAddedToLastDeparture)
+            val lastStopExtraDuration = delayAddedToLastDeparture - firstDepartureTimeDelay
+            newDepartureTime += firstDepartureTimeDelay
+            newMaxFirstDepartureDelaying -= firstDepartureTimeDelay
+            if (newStopData.isNotEmpty()) {
                 val stopDataCopy = newStopData.toMutableList()
                 val oldLastStopData = stopDataCopy.last()
                 stopDataCopy[stopDataCopy.size - 1] =
-                    oldLastStopData.withAddedStopTime(delayAddedToLastDeparture)
+                    oldLastStopData.withAddedStopTime(lastStopExtraDuration)
                 newStopData = stopDataCopy
             }
         }
+        newMaxFirstDepartureDelaying =
+            min(newMaxFirstDepartureDelaying, maxDepartureDelayingWithoutConflict)
         val extraRunningTime = max(0.0, timeShift - delayAddedToLastDeparture)
         return copy(
             earliestReachableTime = earliestReachableTime + timeShift,
@@ -143,6 +160,7 @@ data class TimeData(
             stopTimeData = newStopData,
             departureTime = newDepartureTime,
             totalRunningTime = totalRunningTime + extraRunningTime,
+            maxFirstDepartureDelaying = newMaxFirstDepartureDelaying,
         )
     }
 

core/src/test/kotlin/fr/sncf/osrd/stdcm/StopTests.kt

@@ -576,8 +576,8 @@ class StopTests {
           |    ____________/_______/  <-- stop
           |   /        /
         b |  /        /
-          | /  ##### /
-        a |/__ #####/___________> time
+          | /  ####  /
+        a |/__ ####_/___________> time
 
          */
         val infra = DummyInfra()
@@ -628,10 +628,60 @@ class StopTests {
         occupancyTest(res, occupancy)
         val arrivalTime =
             res.departureTime + res.envelope.totalTime + res.stopResults.first().duration
-        assertEquals(3_000.0, res.departureTime, timeStep)
+        assertTrue(res.departureTime >= 3_000.0)
         assertEquals(15_000.0, arrivalTime, timeStep)
     }
 
+    /** Checks that we can shift the departure time even when there are stops around. */
+    @Test
+    fun departureTimeShiftWithStops() {
+        /*
+        a --> b --> c --> d
+                 ^
+                stop
+
+        space
+          ^
+        d |########################## /
+          |##########################/
+        c |                         /
+          |    ___(_____x   )   ___/  <-- stop
+          |   /                /
+        b |  /                /
+          | /                /
+        a |/________________/_____________> time
+
+        If we try to lengthen the stop, the running time would be too long
+
+         */
+        val infra = DummyInfra()
+        val timeStep = 2.0
+        val blocks =
+            listOf(
+                infra.addBlock("a", "b"),
+                infra.addBlock("b", "c"),
+                infra.addBlock("c", "d"),
+                infra.addBlock("d", "e"),
+            )
+
+        val builder = ImmutableMultimap.builder<BlockId, OccupancySegment>()
+        builder.put(blocks[2], OccupancySegment(0.0, 10_000.0, 0.meters, 100.meters))
+        val occupancy = builder.build()
+        val res =
+            STDCMPathfindingBuilder()
+                .setInfra(infra.fullInfra())
+                .setUnavailableTimes(occupancy)
+                .addStep(STDCMStep(setOf(EdgeLocation(blocks[0], Offset(0.meters)))))
+                .addStep(STDCMStep(setOf(EdgeLocation(blocks[1], Offset(50.meters))), 1.0, true))
+                .addStep(STDCMStep(setOf(EdgeLocation(blocks[2], Offset(100.meters))), 0.0, true))
+                .setTimeStep(timeStep)
+                .setMaxRunTime(5_000.0)
+                .setMaxDepartureDelay(Double.POSITIVE_INFINITY)
+                .run()!!
+        occupancyTest(res, occupancy)
+        assertEquals(res.stopResults[0].duration, 1.0, 2 * timeStep)
+    }
+
     /**
      * Checks that we properly account for stop durations when looking for conflicts, with two
      * stops.

core/src/test/kotlin/fr/sncf/osrd/stdcm/VisitedNodesTests.kt

@@ -38,6 +38,7 @@ class VisitedNodesTests {
                         totalRunningTime = 0.0,
                         departureTime = 0.0,
                         stopTimeData = listOf(),
+                        maxFirstDepartureDelaying = 42.0,
                     ),
                 maxMarginDuration = 0.0,
             )
@@ -62,6 +63,7 @@ class VisitedNodesTests {
                         totalRunningTime = 0.0,
                         departureTime = 0.0,
                         stopTimeData = listOf(),
+                        maxFirstDepartureDelaying = 42.0,
                     ),
                 maxMarginDuration = 0.0,
             )
@@ -94,6 +96,7 @@ class VisitedNodesTests {
                         totalRunningTime = 0.0,
                         departureTime = 0.0,
                         stopTimeData = listOf(),
+                        maxFirstDepartureDelaying = 100.0,
                     ),
                 maxMarginDuration = 100.0,
             )
@@ -136,6 +139,7 @@ class VisitedNodesTests {
                         totalRunningTime = 0.0,
                         departureTime = 0.0,
                         stopTimeData = listOf(),
+                        maxFirstDepartureDelaying = 100.0,
                     ),
                 maxMarginDuration = 100.0,
             )
@@ -171,6 +175,7 @@ class VisitedNodesTests {
                         totalRunningTime = 0.0,
                         departureTime = 0.0,
                         stopTimeData = listOf(),
+                        maxFirstDepartureDelaying = 0.0,
                     ),
                 maxMarginDuration = 100.0,
             )
@@ -209,9 +214,10 @@ class VisitedNodesTests {
                                 StopTimeData(
                                     currentDuration = 120.0,
                                     minDuration = 120.0,
-                                    maxDepartureDelayBeforeStop = 0.0
+                                    maxDepartureDelayBeforeStop = 0.0,
                                 )
                             ),
+                        maxFirstDepartureDelaying = 100.0,
                     ),
                 maxMarginDuration = 100.0,
             )
@@ -258,6 +264,7 @@ class VisitedNodesTests {
                                     maxDepartureDelayBeforeStop = 0.0
                                 )
                             ),
+                        maxFirstDepartureDelaying = 100.0,
                     ),
                 maxMarginDuration = 100.0,
                 remainingTimeEstimation = 300.0,

core/src/test/kotlin/fr/sncf/osrd/stdcm/preprocessing/STDCMHeuristicTests.kt

@@ -225,6 +225,7 @@ class STDCMHeuristicTests {
                 timeOfNextConflictAtLocation = 0.0,
                 totalRunningTime = 0.0,
                 stopTimeData = listOf(),
+                maxFirstDepartureDelaying = 0.0,
             )
         val defaultNode =
             STDCMNode(