core: add etcs loa logic to etcs braking simulator

Signed-off-by: Erashin <[email protected]>

core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/etcs/Constants.kt

@@ -1,6 +1,43 @@
 package fr.sncf.osrd.envelope_sim.etcs
 
+/**
+ * National Default Value: permission to inhibit the compensation of the speed measurement accuracy.
+ * See Subset referenced in ETCSBrakingSimulator: table in Appendix A.3.2.
+ */
+const val qNvinhsmicperm = false
+
+/**
+ * Estimated acceleration during tBerem, worst case scenario (aEst2 is between 0 and 0.4), expressed
+ * in m/s². See Subset referenced in ETCSBrakingSimulator: §3.13.9.3.2.9.
+ */
+const val aEst2 = 0.4
+
 /** See Subset referenced in ETCSBrakingSimulator: table in Appendix A.3.1. */
+const val dvEbiMin = 7.5 / 3.6 // m/s
+const val dvEbiMax = 15.0 / 3.6 // m/s
+const val vEbiMin = 110.0 / 3.6 // m/s
+const val vEbiMax = 210.0 / 3.6 // m/s
+const val tWarning = 2.0 // s
 const val tDriver = 4.0 // s
 const val mRotatingMax = 15.0 // %
 const val mRotatingMin = 2.0 // %
+
+fun vUra(speed: Double): Double {
+    return if (speed <= 30 / 3.6) 2 / 3.6
+    // vUra(30km/h) = 2km/h & vUra(500km/h) = 12km/h with a linear interpolation in between
+    // this gives the following equation : y = (x + 64) / 47, still in km/h
+    else ((speed + 64) / 47) / 3.6
+}
+
+/** See Subset referenced in ETCSBrakingSimulator: §3.13.9.3.2.10. */
+fun vDelta0(speed: Double): Double {
+    return if (!qNvinhsmicperm) vUra(speed) else 0.0
+}
+
+/** See Subset referenced in ETCSBrakingSimulator: §3.13.9.2.3. */
+fun dvEbi(speed: Double): Double {
+    return if (speed <= vEbiMin) dvEbiMin
+    else if (speed < vEbiMax)
+        (dvEbiMax - dvEbiMin) / (vEbiMax - vEbiMin) * (speed - vEbiMin) + dvEbiMin
+    else dvEbiMax
+}

core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/etcs/ETCSBrakingCurves.kt

@@ -79,11 +79,95 @@ fun addBrakingCurvesAtEOAs(
             )
         val indicationCurve = keepBrakingCurveUnderOverlay(fullIndicationCurve, envelope, beginPos)
         builder.addPart(indicationCurve)
+        // We build EOAs along the path. We need to handle overlaps with the next EOA. To do so, we
+        // shift the left position constraint, beginPos, to this EOA's target position.
         beginPos = targetPosition
     }
     return builder.build()
 }
 
+/** Compute braking curves at every limit of authority. */
+fun addBrakingCurvesAtLOAs(
+    envelope: Envelope,
+    context: EnvelopeSimContext,
+    limitsOfAuthority: Collection<LimitOfAuthority>
+): Envelope {
+    val sortedLimitsOfAuthority = limitsOfAuthority.sortedBy { it.offset }
+    val beginPos = 0.0
+    var envelopeWithLoaBrakingCurves = envelope
+    var builder = OverlayEnvelopeBuilder.forward(envelopeWithLoaBrakingCurves)
+    for (limitOfAuthority in sortedLimitsOfAuthority) {
+        val targetPosition = limitOfAuthority.offset.distance.meters
+        val targetSpeed = limitOfAuthority.speed
+        val maxBecDeltaSpeed = maxBecDeltaSpeed()
+        val overhead =
+            Envelope.make(
+                EnvelopePart.generateTimes(
+                    listOf(EnvelopeProfile.CONSTANT_SPEED),
+                    doubleArrayOf(beginPos, targetPosition),
+                    doubleArrayOf(
+                        envelope.maxSpeed + maxBecDeltaSpeed,
+                        envelope.maxSpeed + maxBecDeltaSpeed
+                    )
+                )
+            )
+        val ebdCurve =
+            computeBrakingCurve(
+                context,
+                overhead,
+                beginPos,
+                targetPosition,
+                targetSpeed,
+                BrakingType.ETCS_EBD
+            )
+        val guiCurve =
+            computeBrakingCurve(
+                context,
+                overhead,
+                beginPos,
+                targetPosition,
+                targetSpeed,
+                BrakingType.ETCS_GUI
+            )
+
+        val ebiCurve = computeEbiBrakingCurveFromEbd(context, ebdCurve, beginPos, targetSpeed)
+        assert(ebiCurve.beginPos == beginPos || ebiCurve.maxSpeed >= envelope.maxSpeed)
+
+        val fullIndicationCurve =
+            computeIndicationBrakingCurveFromRef(
+                context,
+                ebiCurve,
+                BrakingCurveType.EBI,
+                guiCurve,
+                beginPos
+            )
+        val indicationCurve =
+            keepBrakingCurveUnderOverlay(
+                fullIndicationCurve,
+                envelopeWithLoaBrakingCurves,
+                beginPos
+            )
+        builder.addPart(indicationCurve)
+        if (indicationCurve.endPos < targetPosition) {
+            // Maintain target speed until target position, i.e. LOA.
+            val maintainTargetSpeedCurve =
+                EnvelopePart.generateTimes(
+                    listOf(EnvelopeProfile.CONSTANT_SPEED),
+                    doubleArrayOf(indicationCurve.endPos, targetPosition),
+                    doubleArrayOf(targetSpeed, targetSpeed)
+                )
+            builder.addPart(maintainTargetSpeedCurve)
+        }
+        // We build the LOAs along the path, and they don't all have the same target speeds. To
+        // handle
+        // intersections with the next LOA, it is needed to add this LOA braking curve to the
+        // overlay builder that will be used to compute the following LOAs.
+        envelopeWithLoaBrakingCurves = builder.build()
+        builder = OverlayEnvelopeBuilder.forward(envelopeWithLoaBrakingCurves)
+    }
+    return envelopeWithLoaBrakingCurves
+}
+
 /** Compute braking curve: used to compute EBD, SBD or GUI. */
 private fun computeBrakingCurve(
     context: EnvelopeSimContext,
@@ -93,9 +177,14 @@ private fun computeBrakingCurve(
     targetSpeed: Double,
     brakingType: BrakingType
 ): EnvelopePart {
-    // If the stopPosition is below begin position, the input is invalid
+    assert(
+        brakingType == BrakingType.ETCS_EBD ||
+            brakingType == BrakingType.ETCS_SBD ||
+            brakingType == BrakingType.ETCS_GUI
+    )
+    // If the stopPosition is below begin position, the input is invalid.
     // If the stopPosition is after the end of the path, the input is invalid except if it is an
-    // SVL, i.e. the target speed is 0 and the curve to compute is an EBD
+    // SVL, i.e. the target speed is 0 and the curve to compute is an EBD.
     if (
         targetPosition <= beginPos ||
             (targetPosition > context.path.length &&
@@ -114,7 +203,7 @@ private fun computeBrakingCurve(
         ConstrainedEnvelopePartBuilder(
             partBuilder,
             PositionConstraint(beginPos, targetPosition),
-            SpeedConstraint(0.0, EnvelopePartConstraintType.FLOOR),
+            SpeedConstraint(targetSpeed, EnvelopePartConstraintType.FLOOR),
             EnvelopeConstraint(envelope, EnvelopePartConstraintType.CEILING)
         )
     EnvelopeDeceleration.decelerate(
@@ -125,10 +214,81 @@ private fun computeBrakingCurve(
         -1.0,
         brakingType
     )
-    val brakingCurve = partBuilder.build()
+    var brakingCurve = partBuilder.build()
+
+    if (brakingType == BrakingType.ETCS_EBD && targetSpeed != 0.0) {
+        // TODO: by doing this, there is an approximation on the gradient used. TBD at a later date.
+        // When target is an LOA, EBD reaches target position at target speed + dVEbi: shift
+        // envelope to make it so. See Subset referenced in ETCSBrakingSimulator: §3.13.8.3.1,
+        // figure 40.
+        val dvEbi = dvEbi(targetSpeed)
+        val intersection = brakingCurve.interpolatePosition(targetSpeed + dvEbi)
+        brakingCurve =
+            brakingCurve.copyAndShift(targetPosition - intersection, 0.0, Double.POSITIVE_INFINITY)
+    }
+
     return brakingCurve
 }
 
+/**
+ * Compute EBI curve from EBD curve. Resulting EBI stops at target speed. See Subset referenced in
+ * ETCSBrakingSimulator: figure 45.
+ */
+private fun computeEbiBrakingCurveFromEbd(
+    context: EnvelopeSimContext,
+    ebdCurve: EnvelopePart,
+    beginPos: Double,
+    targetSpeed: Double
+): EnvelopePart {
+    val reversedNewPos = ArrayList<Double>()
+    val reversedNewSpeeds = ArrayList<Double>()
+    var reversedNewPosIndex = 0
+    for (i in ebdCurve.pointCount() - 1 downTo 0) {
+        val speed = ebdCurve.getPointSpeed(i)
+        val deltaPosAndDeltaSpeed =
+            computeBecDeltaPosAndSpeed(context, ebdCurve, speed, targetSpeed)
+        val deltaPos = deltaPosAndDeltaSpeed.component1()
+        val deltaSpeed = deltaPosAndDeltaSpeed.component2()
+        val newPos = ebdCurve.getPointPos(i) - deltaPos
+        val newSpeed = speed - deltaSpeed
+        if (newSpeed < 0) continue
+        if (newPos >= beginPos) {
+            reversedNewPos.add(newPos)
+            reversedNewSpeeds.add(newSpeed)
+            reversedNewPosIndex++
+        } else {
+            assert(reversedNewPosIndex > 0 && reversedNewPos[reversedNewPosIndex - 1] > beginPos)
+            // Interpolate to begin position if reaching a position before it.
+            val prevPos = reversedNewPos[reversedNewPosIndex - 1]
+            val prevSpeed = reversedNewSpeeds[reversedNewPosIndex - 1]
+            val speedAtBeginPos =
+                prevSpeed + (beginPos - prevPos) * (newSpeed - prevSpeed) / (newPos - prevPos)
+            reversedNewPos.add(beginPos)
+            reversedNewSpeeds.add(speedAtBeginPos)
+            break
+        }
+    }
+
+    val nbPoints = reversedNewPos.size
+    val newPosArray = DoubleArray(nbPoints)
+    val newSpeedsArray = DoubleArray(nbPoints)
+    for (i in newPosArray.indices) {
+        newPosArray[i] = reversedNewPos[nbPoints - 1 - i]
+        newSpeedsArray[i] = reversedNewSpeeds[nbPoints - 1 - i]
+    }
+    val fullBrakingCurve =
+        EnvelopePart.generateTimes(listOf(EnvelopeProfile.BRAKING), newPosArray, newSpeedsArray)
+
+    // Make EBI stop at target speed.
+    val intersection = fullBrakingCurve.interpolatePosition(targetSpeed)
+    return fullBrakingCurve.sliceWithSpeeds(
+        fullBrakingCurve.beginPos,
+        fullBrakingCurve.beginSpeed,
+        intersection,
+        targetSpeed
+    )
+}
+
 /**
  * Compute Indication curve: EBI/SBD -> SBI -> PS -> IND. See Subset referenced in
  * ETCSBrakingSimulator: figures 45 and 46.
@@ -170,7 +330,7 @@ private fun computeIndicationBrakingCurveFromRef(
             reversedNewSpeeds.add(speed)
         } else {
             assert(reversedNewPosIndex > 0 && reversedNewPos[reversedNewPosIndex - 1] > beginPos)
-            // Interpolate to begin position if reaching a position before it
+            // Interpolate to begin position if reaching a position before it.
             val prevPos = reversedNewPos[reversedNewPosIndex - 1]
             val prevSpeed = reversedNewSpeeds[reversedNewPosIndex - 1]
             val speedAtBeginPos =
@@ -226,6 +386,61 @@ private fun keepBrakingCurveUnderOverlay(
     return partBuilder.build()
 }
 
+/** Compute the position and speed offsets between EBD and EBI curves, for a given speed. See Subset referenced in ETCSBrakingSimulator: 3.13.9.3.2. */
+private fun computeBecDeltaPosAndSpeed(
+    context: EnvelopeSimContext,
+    ebd: EnvelopePart,
+    speed: Double,
+    targetSpeed: Double
+): Pair<Double, Double> {
+    val position = ebd.interpolatePosition(speed)
+    val rollingStock = context.rollingStock
+
+    val vDelta0 = vDelta0(speed)
+
+    val minGrade = TrainPhysicsIntegrator.getMinGrade(rollingStock, context.path, position)
+    val weightForce = TrainPhysicsIntegrator.getWeightForce(rollingStock, minGrade)
+    // The time during which the traction effort is still present. See Subset: §3.13.9.3.2.3.
+    val tTraction =
+        max(
+            rollingStock.rjsEtcsBrakeParams.tTractionCutOff -
+                (tWarning + rollingStock.rjsEtcsBrakeParams.tBs2),
+            0.0
+        )
+    // Estimated acceleration during tTraction, worst case scenario (the train accelerates as much
+    // as possible).
+    val aEst1 =
+        TrainPhysicsIntegrator.computeAcceleration(
+            rollingStock,
+            rollingStock.getRollingResistance(speed),
+            weightForce,
+            speed,
+            PhysicsRollingStock.getMaxEffort(speed, context.tractiveEffortCurveMap.get(position)),
+            1.0
+        )
+    // Speed correction due to the traction staying active during tTraction. See Subset: §3.13.9.3.2.10.
+    val vDelta1 = aEst1 * tTraction
+
+    // The remaining time during which the traction effort is not present. See Subset: §3.13.9.3.2.6.
+    val tBerem = max(rollingStock.rjsEtcsBrakeParams.tBe - tTraction, 0.0)
+    // Speed correction due to the braking system not being active yet. See Subset: §3.13.9.3.2.10.
+    val vDelta2 = aEst2 * tBerem
+
+    // Compute dBec and vBec. See Subset: §3.13.9.3.2.10.
+    val maxV = max(speed + vDelta0 + vDelta1, targetSpeed)
+    val dBec =
+        max(speed + vDelta0 + vDelta1 / 2, targetSpeed) * tTraction + (maxV + vDelta1 / 2) * tBerem
+    val vBec = maxV + vDelta2
+
+    val deltaSpeed = vBec - speed
+    return Pair(dBec, deltaSpeed)
+}
+
+private fun maxBecDeltaSpeed(): Double {
+    // TODO: correctly compute maxBecDeltaSpeed. TBD at a later date.
+    return 50.0 / 3.6
+}
+
 /** See Subset referenced in ETCSBrakingSimulator: §3.13.9.3.3.1 and §3.13.9.3.3.2. */
 private fun getSbiPosition(ebiOrSbdPosition: Double, speed: Double, tbs: Double): Double {
     return getPreviousPosition(ebiOrSbdPosition, speed, tbs)

core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/etcs/ETCSBrakingSimulator.kt

@@ -51,8 +51,7 @@ class ETCSBrakingSimulatorImpl(override val context: EnvelopeSimContext) : ETCSB
         limitsOfAuthority: Collection<LimitOfAuthority>
     ): Envelope {
         if (limitsOfAuthority.isEmpty()) return envelope
-        // TODO: implement braking at LOAs CORRECTLY
-        return envelope
+        return addBrakingCurvesAtLOAs(envelope, context, limitsOfAuthority)
     }
 
     override fun addStopBrakingCurves(