core: stdcm: fix how we access the next stop duration

Author: eckter

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

@@ -110,7 +110,7 @@ data class STDCMEdge(
             )
         } else {
             // New edge on the same block, after a stop
-            val stopDuration = graph.steps[waypointIndex + 1].duration!!
+            val stopDuration = graph.getFirstStopAfterIndex(waypointIndex)!!.duration!!
             STDCMNode(
                 totalTime + timeStart + stopDuration,
                 endSpeed,

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

@@ -224,7 +224,7 @@ internal constructor(
             getStopOnBlock(graph, infraExplorer.getCurrentBlock(), startOffset, waypointIndex) !=
                 null
         if (!endAtStop) return null
-        return graph.steps[waypointIndex + 1].duration!!
+        return graph.getFirstStopAfterIndex(waypointIndex)!!.duration!!
     }
 
     /** Creates a single STDCM edge, adding the given amount of delay */

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

@@ -106,4 +106,12 @@ class STDCMGraph(
         }
         return res
     }
+
+    /** Returns the first step with a stop after the given index, if any. */
+    fun getFirstStopAfterIndex(i: Int): STDCMStep? {
+        return steps
+            .withIndex()
+            .firstOrNull { it.index > i && it.value.stop }
+            ?.value
+    }
 }