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

@@ -1,6 +1,41 @@
 package fr.sncf.osrd.envelope_sim.etcs
 
+/**
+ * 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
+
 /** See Subset referenced in ETCSBrakingSimulator: table in Appendix A.3.1. */
+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

@@ -11,6 +11,7 @@ import fr.sncf.osrd.envelope.part.constraints.PositionConstraint
 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
 
@@ -78,7 +79,102 @@ fun addBrakingCurvesAtEOAs(
                 beginPos
             )
         val indicationCurve = keepBrakingCurveUnderOverlay(fullIndicationCurve, envelope, beginPos)
+
+        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,
+                    beginPos,
+                    endOfAuthority.offsetSVL.distance.meters,
+                    targetSpeed,
+                    BrakingType.ETCS_EBD
+                )
+            val ebiCurve = computeEbiBrakingCurveFromEbd(context, ebdCurve, beginPos, targetSpeed)
+            val fullIndicationCurveSvl =
+                computeIndicationBrakingCurveFromRef(
+                    context,
+                    ebiCurve,
+                    BrakingCurveType.SBD,
+                    guiCurve,
+                    beginPos
+                )
+            val indicationCurveSvl = keepBrakingCurveUnderOverlay(fullIndicationCurveSvl, envelope, beginPos)
+            // 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)
+        }
+        builder.addPart(indicationCurve)
+        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 }
+    var beginPos = 0.0
+    val builder = OverlayEnvelopeBuilder.forward(envelope)
+    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, envelope, 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)
+        }
         beginPos = targetPosition
     }
     return builder.build()
@@ -114,7 +210,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 +221,76 @@ 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
+            )
+    }
+
     return brakingCurve
 }
 
+/** Compute EBI curve from EBD curve. Resulting EBI stops at target speed. */
+private fun computeEbiBrakingCurveFromEbd(
+    context: EnvelopeSimContext,
+    ebdCurve: EnvelopePart,
+    beginPos: Double,
+    targetSpeed: Double
+): EnvelopePart {
+    val pos = ebdCurve.clonePositions()
+    val speeds = ebdCurve.cloneSpeeds()
+    val reversedNewPos = ArrayList<Double>()
+    val reversedNewSpeeds = ArrayList<Double>()
+    for (i in ebdCurve.pointCount() - 1 downTo 0) {
+        val reversedNewPosIndex = ebdCurve.pointCount() - 1 - i
+        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 (newPos >= beginPos) {
+            reversedNewPos.add(newPos)
+            reversedNewSpeeds.add(newSpeed)
+        } 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.slice(fullBrakingCurve.beginPos, intersection)
+}
+
 /**
  * Compute Indication curve: EBI/SBD -> SBI -> PS -> IND. See Subset referenced in
  * ETCSBrakingSimulator: figures 45 and 46.
@@ -224,6 +386,55 @@ 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.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
+    val vDelta1 = aEst1 * tTraction
+
+    // The remaining time during which the traction effort is not present
+    val tBerem = max(rollingStock.rjsEtcsBrakeParams.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)
+}
+
+private fun maxBecDeltaSpeed(): Double {
+    // TODO: correctly compute maxBecDeltaSpeed
+    return 50.0 / 3.6
+}
+
 /** See Subset referenced in ETCSBrakingSimulator: §3.13.9.3.3.1 and §3.13.9.3.3.2. */
 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

@@ -52,7 +52,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(