core: take step planned timing data into account in block availability

core/osrd-reporting/src/main/java/fr/sncf/osrd/reporting/exceptions/ErrorType.java

@@ -153,7 +153,9 @@ public enum ErrorType {
             "speed_section",
             "Speed section definition is nonsensical and cannot be used for simulation",
             ErrorCause.USER),
-    MissingLastSTDCMStop("missing_last_stdcm_stop", "Last step of stdcm request needs to be a stop", ErrorCause.USER);
+    MissingLastSTDCMStop("missing_last_stdcm_stop", "Last step of stdcm request needs to be a stop", ErrorCause.USER),
+    MissingStepTimingData(
+            "missing_step_timing_data", "At least one path item must contain a step timing data", ErrorCause.USER);
 
     public final String type;
     public final String message;

core/src/main/kotlin/fr/sncf/osrd/api/api_v2/stdcm/STDCMEndpointV2.kt

@@ -25,6 +25,7 @@ import fr.sncf.osrd.standalone_sim.makeElectricalProfiles
 import fr.sncf.osrd.standalone_sim.makeMRSPResponse
 import fr.sncf.osrd.standalone_sim.result.ElectrificationRange
 import fr.sncf.osrd.standalone_sim.runScheduleMetadataExtractor
+import fr.sncf.osrd.stdcm.PlannedTimingData
 import fr.sncf.osrd.stdcm.STDCMResult
 import fr.sncf.osrd.stdcm.STDCMStep
 import fr.sncf.osrd.stdcm.graph.findPath
@@ -33,9 +34,12 @@ import fr.sncf.osrd.train.RollingStock
 import fr.sncf.osrd.train.TrainStop
 import fr.sncf.osrd.utils.Direction
 import fr.sncf.osrd.utils.units.Offset
+import fr.sncf.osrd.utils.units.TimeDelta
 import fr.sncf.osrd.utils.units.meters
 import fr.sncf.osrd.utils.units.seconds
+import java.time.Duration.between
 import java.time.Duration.ofMillis
+import java.time.ZonedDateTime
 import org.takes.Request
 import org.takes.Response
 import org.takes.Take
@@ -67,6 +71,7 @@ class STDCMEndpointV2(private val infraManager: InfraManager) : Take {
             val trainsRequirements =
                 parseRawTrainsRequirements(request.trainsRequirements, request.startTime)
             val spacingRequirements = trainsRequirements.flatMap { it.spacingRequirements }
+            val steps = parseSteps(infra, request.pathItems, request.startTime)
 
             // Run the STDCM pathfinding
             val path =
@@ -75,11 +80,12 @@ class STDCMEndpointV2(private val infraManager: InfraManager) : Take {
                     rollingStock,
                     request.comfort,
                     0.0,
-                    parseSteps(infra, request.pathItems),
+                    steps,
                     makeBlockAvailability(
                         infra,
                         spacingRequirements,
                         workSchedules = request.workSchedules,
+                        steps,
                         gridMarginBeforeTrain = request.timeGapBefore.seconds,
                         gridMarginAfterTrain = request.timeGapAfter.seconds
                     ),
@@ -173,16 +179,30 @@ class STDCMEndpointV2(private val infraManager: InfraManager) : Take {
     }
 }
 
-private fun parseSteps(infra: FullInfra, pathItems: List<STDCMPathItem>): List<STDCMStep> {
+private fun parseSteps(
+    infra: FullInfra,
+    pathItems: List<STDCMPathItem>,
+    startTime: ZonedDateTime
+): List<STDCMStep> {
     if (pathItems.last().stopDuration == null) {
         throw OSRDError(ErrorType.MissingLastSTDCMStop)
     }
+    if (pathItems.none { it.stepTimingData != null }) {
+        throw OSRDError(ErrorType.MissingStepTimingData)
+    }
     return pathItems
         .map {
             STDCMStep(
                 findWaypointBlocks(infra, it.locations),
                 it.stopDuration?.seconds,
-                it.stopDuration != null
+                it.stopDuration != null,
+                if (it.stepTimingData != null)
+                    PlannedTimingData(
+                        TimeDelta(between(startTime, it.stepTimingData.arrivalTime).toMillis()),
+                        it.stepTimingData.arrivalTimeToleranceBefore,
+                        it.stepTimingData.arrivalTimeToleranceAfter
+                    )
+                else null
             )
         }
         .toList()

core/src/main/kotlin/fr/sncf/osrd/api/api_v2/stdcm/STDCMRequestV2.kt

@@ -12,11 +12,9 @@ import fr.sncf.osrd.api.api_v2.standalone_sim.MarginValueAdapter
 import fr.sncf.osrd.api.api_v2.standalone_sim.PhysicsRollingStockModel
 import fr.sncf.osrd.railjson.schema.rollingstock.RJSLoadingGaugeType
 import fr.sncf.osrd.railjson.schema.rollingstock.RJSRollingResistance
-import fr.sncf.osrd.sim_infra.api.TrackSection
 import fr.sncf.osrd.train.RollingStock.Comfort
 import fr.sncf.osrd.utils.json.UnitAdapterFactory
 import fr.sncf.osrd.utils.units.Duration
-import fr.sncf.osrd.utils.units.Offset
 import fr.sncf.osrd.utils.units.TimeDelta
 import fr.sncf.osrd.utils.units.seconds
 import java.time.ZonedDateTime
@@ -69,8 +67,6 @@ data class StepTimingData(
     @Json(name = "arrival_time_tolerance_after") val arrivalTimeToleranceAfter: Duration,
 )
 
-class TrackOffset(val track: String, val offset: Offset<TrackSection>)
-
 data class WorkSchedule(
     /** List of affected track ranges */
     @Json(name = "track_ranges") val trackRanges: Collection<UndirectedTrackRange> = listOf(),

core/src/main/kotlin/fr/sncf/osrd/stdcm/STDCMStep.kt

@@ -2,9 +2,18 @@ package fr.sncf.osrd.stdcm
 
 import fr.sncf.osrd.graph.PathfindingEdgeLocationId
 import fr.sncf.osrd.sim_infra.api.Block
+import fr.sncf.osrd.utils.units.Duration
+import fr.sncf.osrd.utils.units.TimeDelta
 
 data class STDCMStep(
     val locations: Collection<PathfindingEdgeLocationId<Block>>,
     val duration: Double?,
-    val stop: Boolean
+    val stop: Boolean,
+    val plannedTimingData: PlannedTimingData? = null,
+)
+
+data class PlannedTimingData(
+    val arrivalTime: TimeDelta,
+    val arrivalTimeToleranceBefore: Duration,
+    val arrivalTimeToleranceAfter: Duration,
 )

core/src/main/kotlin/fr/sncf/osrd/stdcm/preprocessing/implementation/BlockAvailability.kt

@@ -10,6 +10,7 @@ import fr.sncf.osrd.envelope_utils.DoubleBinarySearch
 import fr.sncf.osrd.sim_infra.api.Path
 import fr.sncf.osrd.sim_infra.api.RawSignalingInfra
 import fr.sncf.osrd.standalone_sim.result.ResultTrain.SpacingRequirement
+import fr.sncf.osrd.stdcm.STDCMStep
 import fr.sncf.osrd.stdcm.infra_exploration.InfraExplorerWithEnvelope
 import fr.sncf.osrd.stdcm.preprocessing.interfaces.BlockAvailabilityInterface
 import fr.sncf.osrd.utils.units.Distance
@@ -25,6 +26,7 @@ val blockAvailabilityLogger: Logger = LoggerFactory.getLogger("BlockAvailability
 data class BlockAvailability(
     val fullInfra: FullInfra,
     val incrementalConflictDetector: IncrementalConflictDetector,
+    val plannedSteps: List<STDCMStep>,
     val gridMarginBeforeTrain: Double,
     val gridMarginAfterTrain: Double,
 ) : BlockAvailabilityInterface {
@@ -34,13 +36,154 @@ data class BlockAvailability(
         startOffset: Offset<Path>,
         endOffset: Offset<Path>,
         startTime: Double
+    ): BlockAvailabilityInterface.Availability {
+        var timeShift = 0.0
+        var firstConflictOffset = infraExplorer.getIncrementalPath().toTravelledPath(startOffset)
+        while (timeShift.isFinite()) {
+            val shiftedStartTime = startTime + timeShift
+            val pathStartTime =
+                shiftedStartTime - infraExplorer.interpolateDepartureFromClamp(startOffset)
+            val endTime = infraExplorer.interpolateDepartureFromClamp(endOffset) + pathStartTime
+
+            val stepAvailability =
+                getStepAvailability(
+                    infraExplorer,
+                    startOffset,
+                    endOffset,
+                    pathStartTime,
+                    shiftedStartTime
+                )
+            val conflictAvailability =
+                getConflictAvailability(
+                    infraExplorer,
+                    startOffset,
+                    pathStartTime,
+                    shiftedStartTime,
+                    endTime
+                )
+            val availability =
+                getMostRestrictiveAvailability(stepAvailability, conflictAvailability)
+
+            if (availability !is BlockAvailabilityInterface.Unavailable) {
+                if (timeShift > 0.0) { // Availability is available due to adding a delay: timeShift
+                    return BlockAvailabilityInterface.Unavailable(timeShift, firstConflictOffset)
+                }
+                // Availability is directly available without adding any delay
+                return availability
+            }
+            timeShift += availability.duration
+            firstConflictOffset = availability.firstConflictOffset
+        }
+        // No available solution with a finite delay was found
+        return BlockAvailabilityInterface.Unavailable(Double.POSITIVE_INFINITY, firstConflictOffset)
+    }
+
+    /**
+     * Check that the planned step timing data is respected, and return the corresponding
+     * availability.
+     * - If some steps are not respected, find the minimumDelayToBecomeAvailable and return
+     *   Unavailable(minimumDelayToBecomeAvailable, ...). In case this delay fails other steps,
+     *   return Unavailable(Infinity, ...).
+     * - If all steps are respected, find the maximumDelayToStayAvailable and return
+     *   Available(maximumDelayToStayAvailable, ...).
+     */
+    private fun getStepAvailability(
+        infraExplorer: InfraExplorerWithEnvelope,
+        startOffset: Offset<Path>,
+        endOffset: Offset<Path>,
+        pathStartTime: Double,
+        startTime: Double,
+    ): BlockAvailabilityInterface.Availability {
+        var maximumDelayToStayAvailable = Double.POSITIVE_INFINITY
+        var minimumDelayToBecomeAvailable = 0.0
+        var firstConflictOffset = Offset<TravelledPath>(Double.POSITIVE_INFINITY.meters)
+
+        val incrementalPath = infraExplorer.getIncrementalPath()
+        // Iterate over all the predecessor blocks + current block
+        for (i in 0 until infraExplorer.getPredecessorBlocks().size + 1) {
+            // Only consider the blocks within the range formed by given offsets
+            if (
+                !(incrementalPath.getBlockStartOffset(i) >= endOffset &&
+                    incrementalPath.getBlockEndOffset(i) <= startOffset)
+            ) {
+                val block = incrementalPath.getBlock(i)
+                first@ for (step in plannedSteps) {
+                    for (location in step.locations) {
+                        if (location.edge == block) {
+                            val stepOffsetOnPath =
+                                incrementalPath.getBlockStartOffset(i) + location.offset.distance
+                            val timeAtStep =
+                                infraExplorer.interpolateDepartureFromClamp(stepOffsetOnPath) +
+                                    pathStartTime
+                            val plannedMinTimeAtStep =
+                                (step.plannedTimingData!!.arrivalTime -
+                                        step.plannedTimingData.arrivalTimeToleranceBefore)
+                                    .seconds - pathStartTime
+                            val plannedMaxTimeAtStep =
+                                (step.plannedTimingData.arrivalTime +
+                                        step.plannedTimingData.arrivalTimeToleranceAfter)
+                                    .seconds - pathStartTime
+                            if (plannedMinTimeAtStep - timeAtStep > minimumDelayToBecomeAvailable) {
+                                // Train passes through planned timing data before it is available
+                                // and the minimum delay to become available is higher than the
+                                // current one
+                                minimumDelayToBecomeAvailable = plannedMinTimeAtStep - timeAtStep
+                                firstConflictOffset =
+                                    incrementalPath.toTravelledPath(stepOffsetOnPath)
+                            } else if (timeAtStep > plannedMaxTimeAtStep) {
+                                // Train passes through planned timing data after it is available:
+                                // block is forever unavailable
+                                return BlockAvailabilityInterface.Unavailable(
+                                    Double.POSITIVE_INFINITY,
+                                    infraExplorer
+                                        .getIncrementalPath()
+                                        .toTravelledPath(stepOffsetOnPath)
+                                )
+                            }
+                            // Planned timing data respected: update maximumDelayToStayAvailable if
+                            // necessary
+                            maximumDelayToStayAvailable =
+                                min(plannedMaxTimeAtStep - timeAtStep, maximumDelayToStayAvailable)
+                            break@first
+                        }
+                    }
+                }
+            }
+        }
+        if (minimumDelayToBecomeAvailable > 0.0) { // At least one planned step was not respected
+            if (minimumDelayToBecomeAvailable > maximumDelayToStayAvailable) {
+                // Adding minimumDelayToBecomeAvailable will make us step out of another planned
+                // step
+                return BlockAvailabilityInterface.Unavailable(
+                    Double.POSITIVE_INFINITY,
+                    firstConflictOffset
+                )
+            }
+            // Adding minimumDelayToBecomeAvailable solves every planned step problem
+            return BlockAvailabilityInterface.Unavailable(
+                minimumDelayToBecomeAvailable,
+                firstConflictOffset
+            )
+        }
+        // Every planned step was respected
+        return BlockAvailabilityInterface.Available(
+            maximumDelayToStayAvailable,
+            startTime + maximumDelayToStayAvailable
+        )
+    }
+
+    /** Check the conflicts on the given path and return the corresponding availability. */
+    private fun getConflictAvailability(
+        infraExplorer: InfraExplorerWithEnvelope,
+        startOffset: Offset<Path>,
+        pathStartTime: Double,
+        startTime: Double,
+        endTime: Double
     ): BlockAvailabilityInterface.Availability {
         val needFullRequirements = startOffset < infraExplorer.getPredecessorLength()
         val spacingRequirements =
             if (needFullRequirements) infraExplorer.getFullSpacingRequirements()
             else infraExplorer.getSpacingRequirements()
-        val pathStartTime = startTime - infraExplorer.interpolateDepartureFromClamp(startOffset)
-        val endTime = infraExplorer.interpolateDepartureFromClamp(endOffset) + pathStartTime
 
         // Modify the spacing requirements to adjust for the start time,
         // and filter out the ones that are outside the relevant time range
@@ -77,6 +220,36 @@ data class BlockAvailability(
         }
     }
 
+    /**
+     * Given 2 availabilities, return the most restrictive one:
+     * - Unavailable > Available
+     * - both are unavailable: take the one with the highest duration necessary to become available
+     * - both are available: take the one with the lowest duration necessary to become unavailable
+     */
+    private fun getMostRestrictiveAvailability(
+        firstAvailability: BlockAvailabilityInterface.Availability,
+        secondAvailability: BlockAvailabilityInterface.Availability
+    ): BlockAvailabilityInterface.Availability {
+        if (
+            firstAvailability is BlockAvailabilityInterface.Unavailable &&
+                secondAvailability is BlockAvailabilityInterface.Unavailable
+        ) {
+            if (firstAvailability.duration >= secondAvailability.duration) return firstAvailability
+            return secondAvailability
+        } else if (firstAvailability is BlockAvailabilityInterface.Unavailable) {
+            return firstAvailability
+        } else if (secondAvailability is BlockAvailabilityInterface.Unavailable) {
+            return secondAvailability
+        } else {
+            if (
+                (firstAvailability as BlockAvailabilityInterface.Available).maximumDelay <=
+                    (secondAvailability as BlockAvailabilityInterface.Available).maximumDelay
+            )
+                return firstAvailability
+            return secondAvailability
+        }
+    }
+
     /**
      * Turns a time into an offset on an envelope with a binary search. Can be optimized if needed.
      */
@@ -104,9 +277,11 @@ fun makeBlockAvailability(
     infra: FullInfra,
     requirements: Collection<SpacingRequirement>,
     workSchedules: Collection<WorkSchedule> = listOf(),
+    steps: List<STDCMStep> = listOf(),
     gridMarginBeforeTrain: Double = 0.0,
     gridMarginAfterTrain: Double = 0.0,
 ): BlockAvailabilityInterface {
+    // Merge work schedules into train requirements
     val convertedWorkSchedules = convertWorkSchedules(infra.rawInfra, workSchedules)
     var allRequirements = requirements + convertedWorkSchedules
     if (gridMarginAfterTrain != 0.0 || gridMarginBeforeTrain != 0.0) {
@@ -122,9 +297,13 @@ fun makeBlockAvailability(
             }
     }
     val trainRequirements = listOf(TrainRequirements(0L, allRequirements, listOf()))
+
+    // Only keep steps with planned timing data
+    val plannedSteps = steps.filter { it.plannedTimingData != null }
     return BlockAvailability(
         infra,
         incrementalConflictDetector(trainRequirements),
+        plannedSteps,
         gridMarginBeforeTrain,
         gridMarginAfterTrain,
     )