core: stdcm: handle stops directly in infra explorer

Signed-off-by: Eloi Charpentier <[email protected]>

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

@@ -7,6 +7,7 @@ import fr.sncf.osrd.stdcm.preprocessing.interfaces.BlockAvailabilityInterface
 import fr.sncf.osrd.stdcm.preprocessing.interfaces.BlockAvailabilityInterface.Availability
 import fr.sncf.osrd.utils.units.Distance.Companion.fromMeters
 import fr.sncf.osrd.utils.units.Offset
+import fr.sncf.osrd.utils.units.meters
 import java.util.*
 
 /**
@@ -130,4 +131,20 @@ internal constructor(
             startTime
         )
     }
+
+    /** Return how much time we can add to the stop at the end of the explorer */
+    fun getMaxAdditionalStopDuration(
+        explorerWithNewEnvelope: InfraExplorerWithEnvelope,
+        endTime: Double,
+    ): Double {
+        val availability =
+            blockAvailability.getAvailability(
+                explorerWithNewEnvelope,
+                explorerWithNewEnvelope.getSimulatedLength() - 10.meters,
+                explorerWithNewEnvelope.getSimulatedLength(),
+                endTime,
+            )
+        if (availability is BlockAvailabilityInterface.Available) return availability.maximumDelay
+        return 0.0
+    }
 }

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

@@ -12,8 +12,6 @@ import fr.sncf.osrd.envelope_sim.allowances.utils.AllowanceValue
 import fr.sncf.osrd.railjson.schema.rollingstock.Comfort
 import fr.sncf.osrd.reporting.exceptions.ErrorType
 import fr.sncf.osrd.reporting.exceptions.OSRDError
-import fr.sncf.osrd.standalone_sim.EnvelopeStopWrapper
-import fr.sncf.osrd.stdcm.infra_exploration.withEnvelope
 import fr.sncf.osrd.stdcm.preprocessing.interfaces.BlockAvailabilityInterface
 import fr.sncf.osrd.train.RollingStock
 import fr.sncf.osrd.train.TrainStop
@@ -88,11 +86,9 @@ fun buildFinalEnvelope(
                 runSimulationWithFixedPoints(maxSpeedEnvelope, fixedPoints, context, isMareco)
             val conflictOffset =
                 findConflictOffsets(
-                    graph,
                     newEnvelope,
                     blockAvailability,
                     edges,
-                    stops,
                     updatedTimeData,
                 ) ?: return newEnvelope
             if (fixedPoints.any { it.offset == conflictOffset })
@@ -254,14 +250,11 @@ private fun getTimeOnEdges(
  * found, returns its offset. Otherwise, returns null.
  */
 private fun findConflictOffsets(
-    graph: STDCMGraph,
     envelope: Envelope,
     blockAvailability: BlockAvailabilityInterface,
     edges: List<STDCMEdge>,
-    stops: List<TrainStop>,
     updatedTimeData: TimeData,
 ): Offset<TravelledPath>? {
-    val envelopeWithStops = EnvelopeStopWrapper(envelope, stops)
     val startOffset = edges[0].envelopeStartOffset
     val endOffset =
         startOffset +
@@ -273,17 +266,12 @@ private fun findConflictOffsets(
         edges
             .last()
             .infraExplorer
-            .withEnvelope(
-                envelopeWithStops,
-                graph.fullInfra,
-                graph.rollingStock,
-                isSimulationComplete = true
+            .withNewEnvelope(
+                envelope,
             )
+            .updateStopDurations(updatedTimeData)
     assert(
-        TrainPhysicsIntegrator.arePositionsEqual(
-            envelopeWithStops.endPos,
-            (endOffset - startOffset).meters
-        )
+        TrainPhysicsIntegrator.arePositionsEqual(envelope.endPos, (endOffset - startOffset).meters)
     )
     val availability =
         blockAvailability.getAvailability(

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

@@ -55,7 +55,7 @@ data class STDCMEdge(
         return if (!endAtStop) {
             // We move on to the next block
             STDCMNode(
-                timeData.withAddedTime(totalTime, null),
+                timeData.withAddedTime(totalTime, null, null),
                 endSpeed,
                 infraExplorerWithNewEnvelope,
                 this,
@@ -71,8 +71,16 @@ data class STDCMEdge(
             val firstStopAfterIndex = graph.getFirstStopAfterIndex(waypointIndex)!!
             val stopDuration = firstStopAfterIndex.duration!!
             val locationOnEdge = envelopeStartOffset + length.distance
+
             STDCMNode(
-                timeData.withAddedTime(totalTime, stopDuration),
+                timeData.withAddedTime(
+                    totalTime,
+                    stopDuration,
+                    graph.delayManager.getMaxAdditionalStopDuration(
+                        infraExplorerWithNewEnvelope,
+                        timeData.earliestReachableTime + totalTime
+                    )
+                ),
                 endSpeed,
                 infraExplorerWithNewEnvelope,
                 this,

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

@@ -1,13 +1,10 @@
 package fr.sncf.osrd.stdcm.graph
 
 import fr.sncf.osrd.envelope.Envelope
-import fr.sncf.osrd.envelope_sim.TrainPhysicsIntegrator.isTimeStrictlyPositive
 import fr.sncf.osrd.envelope_sim.allowances.LinearAllowance
 import fr.sncf.osrd.sim_infra.api.Block
-import fr.sncf.osrd.standalone_sim.EnvelopeStopWrapper
 import fr.sncf.osrd.stdcm.infra_exploration.InfraExplorerWithEnvelope
 import fr.sncf.osrd.stdcm.preprocessing.interfaces.BlockAvailabilityInterface
-import fr.sncf.osrd.train.TrainStop
 import fr.sncf.osrd.utils.units.Distance.Companion.fromMeters
 import fr.sncf.osrd.utils.units.Length
 import fr.sncf.osrd.utils.units.Offset
@@ -121,21 +118,8 @@ internal constructor(
                 if (envelope.endPos == 0.0) envelope
                 else LinearAllowance.scaleEnvelope(envelope, speedRatio)
             val stopDuration = getEndStopDuration()
-            val envelopeWithStop =
-                if (stopDuration == null) scaledEnvelope
-                else
-                    EnvelopeStopWrapper(
-                        scaledEnvelope,
-                        listOf(
-                            TrainStop(
-                                envelope.endPos,
-                                stopDuration,
-                                // TODO: forward and use onStopSignal param from request
-                                isTimeStrictlyPositive(stopDuration)
-                            )
-                        )
-                    )
-            explorerWithNewEnvelope = infraExplorer.clone().addEnvelope(envelopeWithStop)
+            explorerWithNewEnvelope = infraExplorer.clone().addEnvelope(scaledEnvelope)
+            if (stopDuration != null) explorerWithNewEnvelope!!.addStop(stopDuration)
         }
         return explorerWithNewEnvelope
     }

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

@@ -68,9 +68,12 @@ data class TimeData(
     fun withAddedTime(
         extraTravelTime: Double,
         extraStopTime: Double?,
+        maxAdditionalStopTime: Double?
     ): TimeData {
         var newStopData = stopTimeData
         var maxDepartureDelayingWithoutConflict = maxDepartureDelayingWithoutConflict
+        val nextEarliestReachableTime =
+            earliestReachableTime + extraTravelTime + (extraStopTime ?: 0.0)
         if (extraStopTime != null) {
             val stopDataCopy = newStopData.toMutableList()
             stopDataCopy.add(
@@ -81,11 +84,10 @@ data class TimeData(
                 )
             )
             newStopData = stopDataCopy
-            maxDepartureDelayingWithoutConflict = Double.POSITIVE_INFINITY
+            maxDepartureDelayingWithoutConflict = maxAdditionalStopTime!!
         }
         return copy(
-            earliestReachableTime =
-                earliestReachableTime + extraTravelTime + (extraStopTime ?: 0.0),
+            earliestReachableTime = nextEarliestReachableTime,
             totalRunningTime = totalRunningTime + extraTravelTime,
             stopTimeData = newStopData,
             maxDepartureDelayingWithoutConflict = maxDepartureDelayingWithoutConflict,

core/src/main/kotlin/fr/sncf/osrd/stdcm/infra_exploration/InfraExplorerWithEnvelope.kt

@@ -3,15 +3,16 @@ package fr.sncf.osrd.stdcm.infra_exploration
 import fr.sncf.osrd.api.FullInfra
 import fr.sncf.osrd.conflicts.IncrementalRequirementEnvelopeAdapter
 import fr.sncf.osrd.conflicts.SpacingRequirementAutomaton
-import fr.sncf.osrd.envelope.EnvelopeConcat
-import fr.sncf.osrd.envelope.EnvelopeInterpolate
+import fr.sncf.osrd.envelope.Envelope
 import fr.sncf.osrd.envelope.EnvelopeTimeInterpolate
 import fr.sncf.osrd.envelope_sim.PhysicsRollingStock
 import fr.sncf.osrd.graph.PathfindingConstraint
 import fr.sncf.osrd.graph.PathfindingEdgeLocationId
 import fr.sncf.osrd.sim_infra.api.Block
 import fr.sncf.osrd.sim_infra.api.Path
 import fr.sncf.osrd.standalone_sim.result.ResultTrain
+import fr.sncf.osrd.stdcm.graph.TimeData
+import fr.sncf.osrd.train.TrainStop
 import fr.sncf.osrd.utils.appendOnlyLinkedListOf
 import fr.sncf.osrd.utils.units.Length
 import fr.sncf.osrd.utils.units.Offset
@@ -41,7 +42,21 @@ interface InfraExplorerWithEnvelope : InfraExplorer {
     fun getFullEnvelope(): EnvelopeTimeInterpolate
 
     /** Adds an envelope. This is done in-place. */
-    fun addEnvelope(envelope: EnvelopeInterpolate): InfraExplorerWithEnvelope
+    fun addEnvelope(envelope: Envelope): InfraExplorerWithEnvelope
+
+    /**
+     * Returns a copy with the given envelope, the previous one is ignored. This keeps stop data.
+     */
+    fun withNewEnvelope(envelope: Envelope): InfraExplorerWithEnvelope
+
+    /** Add a stop to the end of the last simulated envelope */
+    fun addStop(stopDuration: Double)
+
+    /** Just for debugging purposes. */
+    fun getStops(): List<TrainStop>
+
+    /** Update the stop durations, following the updated time data */
+    fun updateStopDurations(updatedTimeData: TimeData): InfraExplorerWithEnvelope
 
     /**
      * Calls `InterpolateDepartureFromClamp` on the underlying envelope, taking the travelled path
@@ -99,29 +114,3 @@ fun initInfraExplorerWithEnvelope(
             )
         }
 }
-
-/** Add an envelope to a simple InfraExplorer. */
-fun InfraExplorer.withEnvelope(
-    envelope: EnvelopeInterpolate,
-    fullInfra: FullInfra,
-    rollingStock: PhysicsRollingStock,
-    isSimulationComplete: Boolean = false,
-): InfraExplorerWithEnvelope {
-    return InfraExplorerWithEnvelopeImpl(
-        this,
-        appendOnlyLinkedListOf(EnvelopeConcat.LocatedEnvelope(envelope, 0.0, 0.0)),
-        SpacingRequirementAutomaton(
-            fullInfra.rawInfra,
-            fullInfra.loadedSignalInfra,
-            fullInfra.blockInfra,
-            fullInfra.signalingSimulator,
-            IncrementalRequirementEnvelopeAdapter(
-                rollingStock,
-                EnvelopeConcat.from(listOf(envelope)),
-                isSimulationComplete
-            ),
-            getIncrementalPath(),
-        ),
-        rollingStock,
-    )
-}

core/src/main/kotlin/fr/sncf/osrd/stdcm/infra_exploration/InfraExplorerWithEnvelopeImpl.kt

@@ -3,14 +3,19 @@ package fr.sncf.osrd.stdcm.infra_exploration
 import fr.sncf.osrd.conflicts.IncrementalRequirementEnvelopeAdapter
 import fr.sncf.osrd.conflicts.SpacingRequirementAutomaton
 import fr.sncf.osrd.conflicts.SpacingRequirements
+import fr.sncf.osrd.envelope.Envelope
 import fr.sncf.osrd.envelope.EnvelopeConcat
 import fr.sncf.osrd.envelope.EnvelopeConcat.LocatedEnvelope
 import fr.sncf.osrd.envelope.EnvelopeInterpolate
 import fr.sncf.osrd.envelope_sim.PhysicsRollingStock
 import fr.sncf.osrd.sim_infra.api.Path
+import fr.sncf.osrd.standalone_sim.EnvelopeStopWrapper
 import fr.sncf.osrd.standalone_sim.result.ResultTrain.SpacingRequirement
+import fr.sncf.osrd.stdcm.graph.TimeData
 import fr.sncf.osrd.stdcm.preprocessing.interfaces.BlockAvailabilityInterface
+import fr.sncf.osrd.train.TrainStop
 import fr.sncf.osrd.utils.AppendOnlyLinkedList
+import fr.sncf.osrd.utils.appendOnlyLinkedListOf
 import fr.sncf.osrd.utils.units.Distance
 import fr.sncf.osrd.utils.units.Length
 import fr.sncf.osrd.utils.units.Offset
@@ -22,6 +27,7 @@ data class InfraExplorerWithEnvelopeImpl(
     private val envelopes: AppendOnlyLinkedList<LocatedEnvelope>,
     private val spacingRequirementAutomaton: SpacingRequirementAutomaton,
     private val rollingStock: PhysicsRollingStock,
+    private var stops: MutableList<TrainStop> = mutableListOf(),
 
     // Soft references tell the JVM that the values may be cleared when running out of memory
     private var spacingRequirementsCache: SoftReference<List<SpacingRequirement>>? = null,
@@ -35,6 +41,7 @@ data class InfraExplorerWithEnvelopeImpl(
                 envelopes.shallowCopy(),
                 spacingRequirementAutomaton.clone(),
                 rollingStock,
+                stops.toMutableList(),
                 spacingRequirementsCache,
             )
         }
@@ -44,11 +51,12 @@ data class InfraExplorerWithEnvelopeImpl(
         val cached = envelopeCache?.get()
         if (cached != null) return cached
         val res = EnvelopeConcat.fromLocated(envelopes.toList())
-        envelopeCache = SoftReference(res)
-        return res
+        val withStops = EnvelopeStopWrapper(res, stops)
+        envelopeCache = SoftReference(withStops)
+        return withStops
     }
 
-    override fun addEnvelope(envelope: EnvelopeInterpolate): InfraExplorerWithEnvelope {
+    override fun addEnvelope(envelope: Envelope): InfraExplorerWithEnvelope {
         var prevEndOffset = 0.0
         var prevEndTime = 0.0
         if (envelopes.isNotEmpty()) {
@@ -62,6 +70,68 @@ data class InfraExplorerWithEnvelopeImpl(
         return this
     }
 
+    override fun withNewEnvelope(envelope: Envelope): InfraExplorerWithEnvelope {
+        return copy(
+            envelopes =
+                appendOnlyLinkedListOf(
+                    LocatedEnvelope(envelope, 0.0, 0.0),
+                ),
+            spacingRequirementAutomaton =
+                SpacingRequirementAutomaton(
+                    spacingRequirementAutomaton.rawInfra,
+                    spacingRequirementAutomaton.loadedSignalInfra,
+                    spacingRequirementAutomaton.blockInfra,
+                    spacingRequirementAutomaton.simulator,
+                    IncrementalRequirementEnvelopeAdapter(
+                        rollingStock,
+                        EnvelopeConcat.from(listOf(envelope)),
+                        true
+                    ),
+                    getIncrementalPath(),
+                ),
+            spacingRequirementsCache = null,
+            envelopeCache = null,
+        )
+    }
+
+    override fun addStop(stopDuration: Double) {
+        val position = getFullEnvelope().endPos
+        // We tolerate duplicates and filter them
+        if (stops.isEmpty() || stops.last().position != position) {
+            stops.add(
+                TrainStop(
+                    position,
+                    stopDuration,
+                    true,
+                )
+            )
+            envelopeCache = null
+            spacingRequirementsCache = null
+        } else {
+            assert(stops.isEmpty() || stops.last().duration == stopDuration)
+        }
+    }
+
+    override fun getStops(): List<TrainStop> {
+        return stops
+    }
+
+    override fun updateStopDurations(updatedTimeData: TimeData): InfraExplorerWithEnvelope {
+        for ((i, stop) in stops.withIndex()) {
+            assert(i < updatedTimeData.stopTimeData.size)
+            val updatedStop = updatedTimeData.stopTimeData[i]
+            if (updatedStop.currentDuration != stop.duration) {
+                stops[i] =
+                    TrainStop(
+                        stops[i].position,
+                        updatedStop.currentDuration,
+                        stops[i].onStopSignal,
+                    )
+            }
+        }
+        return this
+    }
+
     override fun interpolateDepartureFromClamp(pathOffset: Offset<Path>): Double {
         return getFullEnvelope()
             .interpolateDepartureFromClamp(
@@ -133,6 +203,7 @@ data class InfraExplorerWithEnvelopeImpl(
             envelopes.shallowCopy(),
             spacingRequirementAutomaton.clone(),
             rollingStock,
+            stops.toMutableList(),
             spacingRequirementsCache
         )
     }