core: add etcs loa and svl 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

@@ -6,6 +6,41 @@ const val mNvavadh = 0.0
 /** National Default Value: Emergency Brake Confidence Level in % */
 const val mNvebcl = 1 - 1E-9
 
+/**
+ * National Default Value: permission to inhibit the compensation of the speed measurement accuracy
+ */
+const val qNvinhsmicperm = false
+
+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 vMin = 4.0 / 3.6 // m/s, corresponds to dvWarning, is used as a min ceiling speed for SVL
+
+// Estimated acceleration during tBerem, worst case scenario (aEst2 is between 0 and 0.4),
+// expressed in m/s²
+const val aEst2 = 0.4
+
+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
+}
+
+fun vDelta0(speed: Double): Double {
+    return if (!qNvinhsmicperm) vUra(speed) else 0.0
+}
+
+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

@@ -10,6 +10,7 @@ import fr.sncf.osrd.envelope.part.constraints.EnvelopePartConstraintType
 import fr.sncf.osrd.envelope.part.constraints.SpeedConstraint
 import fr.sncf.osrd.envelope_sim.*
 import fr.sncf.osrd.envelope_sim.overlays.EnvelopeDeceleration
+import fr.sncf.osrd.utils.SelfTypeHolder
 import kotlin.math.max
 import kotlin.math.min
 
@@ -66,12 +67,90 @@ fun addBrakingCurvesAtEOAs(
             )
         val fullIndicationCurve =
             computeIndicationBrakingCurveFromRef(context, sbdCurve, BrakingCurveType.SBD, guiCurve)
+
+        if (endOfAuthority.offsetSVL != null) {
+            // TODO: Make ebd go until envelope.maxSpeed + deltaVbec(envelope.maxSpeed, minGrade,
+            // maxTractiveEffortCurve)
+            // so that ebi will intersect with mrsp
+            val ebdCurve =
+                computeBrakingCurve(
+                    context,
+                    envelope,
+                    endOfAuthority.offsetSVL.distance.meters,
+                    targetSpeed,
+                    BrakingType.ETCS_EBD
+                )
+            val ebiCurve = computeEbiBrakingCurveFromEbd(context, ebdCurve)
+            val indicationCurveSvl =
+                computeIndicationBrakingCurveFromRef(
+                    context,
+                    ebiCurve,
+                    BrakingCurveType.SBD,
+                    guiCurve
+                )
+            // val intersectionVmin = indicationCurve.interpolateSpeed(vMin)
+            // TODO: return min of the 2 indication curves: indicationCurveEOA and
+            // indicationCurveSVL.
+            //  If the foot of the min curve is reached before EOA, follow the min curve until
+            // Constants.vMin,
+            //  then maintain speed at Constants.vMin, then follow indicationCurveEOA whose foot is
+            // the EOA.
+            // indicationCurve = min(indicationCurve, indicationCurveSvl)
+        }
         val indicationCurve = keepBrakingCurveUnderOverlay(fullIndicationCurve, envelope)
         builder.addPart(indicationCurve)
     }
     return builder.build()
 }
 
+/** Compute braking curves at every limit of authority. */
+fun addBrakingCurvesAtLOAs(
+    envelope: Envelope,
+    context: EnvelopeSimContext,
+    limitsOfAuthority: Collection<LimitOfAuthority>
+): Envelope {
+    val builder = OverlayEnvelopeBuilder.backward(envelope)
+    // TODO: same as for svl, adapt ebd calculation, and use this part of the code.
+    for (limitOfAuthority in limitsOfAuthority) {
+        val targetPosition = limitOfAuthority.offset.distance.meters
+        val targetSpeed = limitOfAuthority.speed
+        val ebdCurve =
+            computeBrakingCurve(
+                context,
+                envelope,
+                targetPosition,
+                targetSpeed,
+                BrakingType.ETCS_EBD
+            )
+        val guiCurve =
+            computeBrakingCurve(
+                context,
+                envelope,
+                targetPosition,
+                targetSpeed,
+                BrakingType.ETCS_GUI
+            )
+        val ebiCurve = computeEbiBrakingCurveFromEbd(context, ebdCurve)
+        val indicationCurve =
+            computeIndicationBrakingCurveFromRef(context, ebiCurve, BrakingCurveType.EBI, guiCurve)
+        val intersection = indicationCurve.interpolateSpeed(targetSpeed)
+        val slicedIndicationCurve = indicationCurve.slice(indicationCurve.beginPos, intersection)
+        val targetSpeedCurve =
+            EnvelopePart(
+                mapOf<Class<out SelfTypeHolder>, SelfTypeHolder>(
+                    Pair(EnvelopeProfile::class.java, EnvelopeProfile.CONSTANT_SPEED)
+                ),
+                indicationCurve.clonePositions(),
+                indicationCurve.cloneTimes(),
+                DoubleArray(indicationCurve.cloneSpeeds().size) { targetSpeed }
+            )
+        val slicedTargetSpeedCurve = targetSpeedCurve.slice(intersection, targetSpeedCurve.endPos)
+        builder.addPart(slicedIndicationCurve)
+        builder.addPart(slicedTargetSpeedCurve)
+    }
+    return builder.build()
+}
+
 /** Compute braking curve: used to compute EBD, SBD or GUI. */
 private fun computeBrakingCurve(
     context: EnvelopeSimContext,
@@ -104,7 +183,65 @@ 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
+        val dvEbi = dvEbi(targetSpeed)
+        val intersection = brakingCurve.interpolateSpeed(targetSpeed + dvEbi)
+        brakingCurve =
+            brakingCurve.copyAndShift(
+                targetPosition - intersection,
+                0.0,
+                Double.POSITIVE_INFINITY,
+                0.0
+            )
+    }
+
+    return brakingCurve
+}
+
+/** Compute EBI curve from EBD curve. */
+private fun computeEbiBrakingCurveFromEbd(
+    context: EnvelopeSimContext,
+    ebdCurve: EnvelopePart
+): EnvelopePart {
+    val pos = ebdCurve.clonePositions()
+    val speeds = ebdCurve.cloneSpeeds()
+    val newPositions = ArrayList<Double>()
+    val newSpeeds = ArrayList<Double>()
+    for (i in ebdCurve.pointCount() - 1 downTo 0) {
+        val deltaPosAndDeltaSpeed =
+            computeBecDeltaPosAndSpeed(context, ebdCurve, speeds[i], ebdCurve.beginSpeed)
+        val deltaPos = deltaPosAndDeltaSpeed.component1()
+        val deltaSpeed = deltaPosAndDeltaSpeed.component2()
+        val newPos = pos[i] - deltaPos
+        val newSpeed = speeds[i] - deltaSpeed
+        if (pos[i] - deltaPos >= 0) {
+            newPositions.add(newPos)
+            newSpeeds.add(newSpeed)
+        } else if (i != ebdCurve.pointCount() - 1 && newPositions[i + 1] > 0) {
+            val prevPos = newPositions[i + 1]
+            val prevSpeed = newSpeeds[i + 1]
+            val speedAtZero = prevSpeed - prevPos * (newSpeed - prevSpeed) / (newPos - prevPos)
+            newPositions.add(0.0)
+            newSpeeds.add(speedAtZero)
+            break
+        }
+    }
+
+    val nbPoints = newPositions.size
+    val newPosArray = DoubleArray(newPositions.size)
+    val newSpeedsArray = DoubleArray(newSpeeds.size)
+    for (i in newPosArray.indices) {
+        newPosArray[i] = newPositions[nbPoints - 1 - i]
+        newSpeedsArray[i] = newSpeeds[nbPoints - 1 - i]
+    }
+    val brakingCurve =
+        EnvelopePart.generateTimes(listOf(EnvelopeProfile.BRAKING), newPosArray, newSpeedsArray)
+
     return brakingCurve
 }
 
@@ -187,6 +324,50 @@ private fun keepBrakingCurveUnderOverlay(
     return partBuilder.build()
 }
 
+/** Compute the position and speed offsets between EBD and EBI curves, for a given speed. */
+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
+    val tTraction = max(rollingStock.tractionCutOff - (tWarning + rollingStock.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
+    val vDelta1 = aEst1 * tTraction
+
+    // The remaining time during which the traction effort is not present
+    val tBerem = max(rollingStock.tBe - tTraction, 0.0)
+    // Speed correction due to the braking system not being active yet
+    val vDelta2 = aEst2 * tBerem
+
+    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)
+}
+
 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

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