core: add etcs loa and svl logic to etcs braking simulator

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

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope/part/EnvelopePart.java

@@ -1,6 +1,7 @@
 package fr.sncf.osrd.envelope.part;
 
 import static fr.sncf.osrd.envelope.EnvelopePhysics.intersectStepWithSpeed;
+import static fr.sncf.osrd.envelope_sim.TrainPhysicsIntegrator.areSpeedsEqual;
 
 import com.carrotsearch.hppc.DoubleArrayList;
 import edu.umd.cs.findbugs.annotations.SuppressFBWarnings;
@@ -429,6 +430,13 @@ private static double[] computeTimes(double[] positions, double[] speeds) {
      */
     public Double interpolatePosition(int startIndex, double speed) {
         assert strictlyMonotonicSpeeds;
+        var minSpeed = getMinSpeed();
+        var maxSpeed = getMaxSpeed();
+        if (areSpeedsEqual(speed, minSpeed)) {
+            speed = minSpeed;
+        } else if (areSpeedsEqual(speed, maxSpeed)) {
+            speed = maxSpeed;
+        }
         assert isBetween(speed, getMinSpeed(), getMaxSpeed());
 
         for (int i = startIndex; i < positions.length - 1; i++) {

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

@@ -78,12 +78,119 @@ fun addBrakingCurvesAtEOAs(
                 beginPos
             )
         val indicationCurve = keepBrakingCurveUnderOverlay(fullIndicationCurve, envelope, beginPos)
+
+        if (endOfAuthority.offsetSVL != null && false) {
+            // 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 }
+    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)
+        }
+        envelopeWithLoaBrakingCurves = builder.build()
+        builder = OverlayEnvelopeBuilder.forward(envelopeWithLoaBrakingCurves)
+    }
+    return envelopeWithLoaBrakingCurves
+}
+
 /** Compute braking curve: used to compute EBD, SBD or GUI. */
 private fun computeBrakingCurve(
     context: EnvelopeSimContext,
@@ -114,7 +221,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 +232,72 @@ 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.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. */
+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, ebdCurve.beginSpeed)
+        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.
@@ -226,6 +395,60 @@ 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. */
 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

@@ -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(