diff --git a/core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/EnvelopeSimContext.java b/core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/EnvelopeSimContext.java deleted file mode 100644 index 2a4f64beb27..00000000000 --- a/core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/EnvelopeSimContext.java +++ /dev/null @@ -1,28 +0,0 @@ -package fr.sncf.osrd.envelope_sim; - -import com.google.common.collect.RangeMap; - -public final class EnvelopeSimContext { - public final PhysicsRollingStock rollingStock; - public final PhysicsPath path; - public final double timeStep; - public final RangeMap tractiveEffortCurveMap; - - /** Creates a context suitable to run simulations on envelopes */ - public EnvelopeSimContext( - PhysicsRollingStock rollingStock, - PhysicsPath path, - double timeStep, - RangeMap tractiveEffortCurveMap) { - this.rollingStock = rollingStock; - this.path = path; - this.timeStep = timeStep; - assert tractiveEffortCurveMap != null; - this.tractiveEffortCurveMap = tractiveEffortCurveMap; - } - - public EnvelopeSimContext updateCurves( - RangeMap tractiveEffortCurveMap) { - return new EnvelopeSimContext(rollingStock, path, timeStep, tractiveEffortCurveMap); - } -} diff --git a/core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/pipelines/MaxEffortEnvelope.java b/core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/pipelines/MaxEffortEnvelope.java deleted file mode 100644 index 1485fd06314..00000000000 --- a/core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/pipelines/MaxEffortEnvelope.java +++ /dev/null @@ -1,146 +0,0 @@ -package fr.sncf.osrd.envelope_sim.pipelines; - -import static fr.sncf.osrd.envelope.part.constraints.EnvelopePartConstraintType.*; - -import fr.sncf.osrd.envelope.Envelope; -import fr.sncf.osrd.envelope.EnvelopeCursor; -import fr.sncf.osrd.envelope.OverlayEnvelopeBuilder; -import fr.sncf.osrd.envelope.part.ConstrainedEnvelopePartBuilder; -import fr.sncf.osrd.envelope.part.EnvelopePart; -import fr.sncf.osrd.envelope.part.EnvelopePartBuilder; -import fr.sncf.osrd.envelope.part.constraints.EnvelopeConstraint; -import fr.sncf.osrd.envelope.part.constraints.PositionConstraint; -import fr.sncf.osrd.envelope.part.constraints.SpeedConstraint; -import fr.sncf.osrd.envelope_sim.EnvelopeProfile; -import fr.sncf.osrd.envelope_sim.EnvelopeSimContext; -import fr.sncf.osrd.envelope_sim.overlays.EnvelopeAcceleration; -import fr.sncf.osrd.envelope_sim.overlays.EnvelopeMaintain; -import fr.sncf.osrd.reporting.exceptions.ErrorType; -import fr.sncf.osrd.reporting.exceptions.OSRDError; - -/** - * Max effort envelope = Max speed envelope + acceleration curves + check maintain speed It is the - * max physical speed at any given point, ignoring allowances - */ -public class MaxEffortEnvelope { - /** Detects if an envelope parts is a plateau */ - public static boolean maxEffortPlateau(EnvelopePart part) { - return part.getMinSpeed() == part.getMaxSpeed(); - } - - /** - * Generate acceleration curves overlay everywhere the max speed envelope increase with a - * discontinuity and compute the constant speed parts to check whether the train can physically - * maintain its speed - */ - public static Envelope addAccelerationAndConstantSpeedParts( - EnvelopeSimContext context, Envelope maxSpeedProfile, double initialPosition, double initialSpeed) { - var builder = OverlayEnvelopeBuilder.forward(maxSpeedProfile); - var cursor = EnvelopeCursor.forward(maxSpeedProfile); - var maxSpeed = maxSpeedProfile.interpolateSpeedRightDir(initialPosition, 1); - if (initialSpeed < maxSpeed) { - accelerate(context, maxSpeedProfile, initialSpeed, initialPosition, builder, cursor); - } - while (!cursor.hasReachedEnd()) { - if (cursor.checkPart(MaxEffortEnvelope::maxEffortPlateau)) { - var partBuilder = new EnvelopePartBuilder(); - partBuilder.setAttr(EnvelopeProfile.CONSTANT_SPEED); - var startSpeed = cursor.getStepBeginSpeed(); - var startPosition = cursor.getPosition(); - var overlayBuilder = new ConstrainedEnvelopePartBuilder( - partBuilder, - new SpeedConstraint(startSpeed, EQUAL), - new PositionConstraint( - cursor.getPart().getBeginPos(), cursor.getPart().getEndPos())); - EnvelopeMaintain.maintain(context, startPosition, startSpeed, overlayBuilder, 1); - - // check if the speed can be maintained from the first position before adding the - // part, - // otherwise it would only be a single point - if (partBuilder.stepCount() > 1) { - builder.addPart(partBuilder.build()); - cursor.findPosition(overlayBuilder.getLastPos()); - } - - // if the cursor didn't reach the end of the constant speed part, - // that means the train was slowed down by a steep ramp - if (cursor.getPosition() < cursor.getPart().getEndPos()) { - partBuilder = new EnvelopePartBuilder(); - partBuilder.setAttr(EnvelopeProfile.CATCHING_UP); - overlayBuilder = new ConstrainedEnvelopePartBuilder( - partBuilder, - new SpeedConstraint(0, FLOOR), - new EnvelopeConstraint(maxSpeedProfile, CEILING)); - startPosition = cursor.getPosition(); - startSpeed = maxSpeedProfile.interpolateSpeedLeftDir(startPosition, 1); - EnvelopeAcceleration.accelerate(context, startPosition, startSpeed, overlayBuilder, 1); - cursor.findPosition(overlayBuilder.getLastPos()); - - if (overlayBuilder.lastIntersection == 0) { - // Train stopped while trying to catch up - throw new OSRDError(ErrorType.ImpossibleSimulationError); - } - - // check that the train was actually slowed down by the ramp - if (partBuilder.stepCount() > 0) - // if the part has more than one point, add it - builder.addPart(partBuilder.build()); - else { - // otherwise skip this position as the train isn't really being slowed down - // and step 1m further - var maxPosition = cursor.getPart().getEndPos(); // We don't want to skip further than the part - if (cursor.getPosition() < maxPosition) - cursor.findPosition(Math.min(maxPosition, cursor.getPosition() + 1)); - } - } - } else if (cursor.checkPartTransition(MaxSpeedEnvelope::increase)) { - var startSpeed = maxSpeedProfile.interpolateSpeedLeftDir(cursor.getPosition(), 1); - accelerate(context, maxSpeedProfile, startSpeed, cursor.getPosition(), builder, cursor); - } else cursor.nextPart(); - } - return builder.build(); - } - - /** - * Accelerates starting at the given speed and position. Simple code factorization, it's called - * when starting up and at part transitions. - */ - private static void accelerate( - EnvelopeSimContext context, - Envelope maxSpeedProfile, - double initialSpeed, - double startPosition, - OverlayEnvelopeBuilder builder, - EnvelopeCursor cursor) { - var partBuilder = new EnvelopePartBuilder(); - partBuilder.setAttr(EnvelopeProfile.ACCELERATING); - var overlayBuilder = new ConstrainedEnvelopePartBuilder( - partBuilder, new SpeedConstraint(0, FLOOR), new EnvelopeConstraint(maxSpeedProfile, CEILING)); - EnvelopeAcceleration.accelerate(context, startPosition, initialSpeed, overlayBuilder, 1); - cursor.findPosition(overlayBuilder.getLastPos()); - if (overlayBuilder.lastIntersection == 0) { - // The train stopped before reaching the end - var err = new OSRDError(ErrorType.ImpossibleSimulationError); - var offset = cursor.getPosition(); - err.context.put("offset", String.format("%.0fm", offset)); - var headPosition = Math.min(Math.max(0, offset), context.path.getLength()); - var tailPosition = - Math.min(Math.max(0, headPosition - context.rollingStock.getLength()), context.path.getLength()); - var grade = context.path.getAverageGrade(headPosition, tailPosition); - err.context.put("grade", String.format("%.2fm/km", grade)); - var map = context.tractiveEffortCurveMap.get(cursor.getPosition()); - assert map != null; - err.context.put("traction_force", String.format("%.2fN", map[0].maxEffort())); - throw err; - } - builder.addPart(partBuilder.build()); - } - - /** Generate a max effort envelope given a max speed envelope */ - public static Envelope from(EnvelopeSimContext context, double initialSpeed, Envelope maxSpeedProfile) { - var maxEffortEnvelope = addAccelerationAndConstantSpeedParts(context, maxSpeedProfile, 0, initialSpeed); - assert maxEffortEnvelope.continuous : "Discontinuity in max effort envelope"; - assert maxEffortEnvelope.getBeginPos() == 0; - return maxEffortEnvelope; - } -} diff --git a/core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/pipelines/MaxSpeedEnvelope.java b/core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/pipelines/MaxSpeedEnvelope.java deleted file mode 100644 index 6151e4fc7d7..00000000000 --- a/core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/pipelines/MaxSpeedEnvelope.java +++ /dev/null @@ -1,93 +0,0 @@ -package fr.sncf.osrd.envelope_sim.pipelines; - -import static fr.sncf.osrd.envelope.part.constraints.EnvelopePartConstraintType.CEILING; -import static fr.sncf.osrd.envelope.part.constraints.EnvelopePartConstraintType.FLOOR; -import static fr.sncf.osrd.envelope_sim.TrainPhysicsIntegrator.arePositionsEqual; - -import fr.sncf.osrd.envelope.Envelope; -import fr.sncf.osrd.envelope.EnvelopeCursor; -import fr.sncf.osrd.envelope.OverlayEnvelopeBuilder; -import fr.sncf.osrd.envelope.part.ConstrainedEnvelopePartBuilder; -import fr.sncf.osrd.envelope.part.EnvelopePartBuilder; -import fr.sncf.osrd.envelope.part.constraints.EnvelopeConstraint; -import fr.sncf.osrd.envelope.part.constraints.SpeedConstraint; -import fr.sncf.osrd.envelope_sim.EnvelopeProfile; -import fr.sncf.osrd.envelope_sim.EnvelopeSimContext; -import fr.sncf.osrd.envelope_sim.StopMeta; -import fr.sncf.osrd.envelope_sim.overlays.EnvelopeDeceleration; -import fr.sncf.osrd.reporting.exceptions.OSRDError; - -/** - * Max speed envelope = MRSP + braking curves It is the max speed allowed at any given point, - * ignoring allowances - */ -public class MaxSpeedEnvelope { - static boolean increase(double prevPos, double prevSpeed, double nextPos, double nextSpeed) { - // Works for both accelerations (forwards) and decelerations (backwards) - return prevSpeed < nextSpeed; - } - - /** - * Generate braking curves overlay everywhere the mrsp decrease (increase backwards) with a - * discontinuity - */ - private static Envelope addBrakingCurves(EnvelopeSimContext context, Envelope mrsp) { - var builder = OverlayEnvelopeBuilder.backward(mrsp); - var cursor = EnvelopeCursor.backward(mrsp); - var lastPosition = mrsp.getEndPos(); - while (cursor.findPartTransition(MaxSpeedEnvelope::increase)) { - if (cursor.getPosition() > lastPosition) { - // The next braking curve already covers this point, this braking curve is hidden - cursor.nextPart(); - continue; - } - var partBuilder = new EnvelopePartBuilder(); - partBuilder.setAttr(EnvelopeProfile.BRAKING); - var overlayBuilder = new ConstrainedEnvelopePartBuilder( - partBuilder, new SpeedConstraint(0, FLOOR), new EnvelopeConstraint(mrsp, CEILING)); - var startSpeed = cursor.getSpeed(); - var startPosition = cursor.getPosition(); - // TODO: link directionSign to cursor boolean reverse - EnvelopeDeceleration.decelerate(context, startPosition, startSpeed, overlayBuilder, -1); - builder.addPart(partBuilder.build()); - cursor.nextPart(); - lastPosition = overlayBuilder.getLastPos(); - } - return builder.build(); - } - - /** Generate braking curves overlay at every stop position */ - private static Envelope addStopBrakingCurves( - EnvelopeSimContext context, double[] stopPositions, Envelope curveWithDecelerations) { - for (int i = 0; i < stopPositions.length; i++) { - var stopPosition = stopPositions[i]; - // if the stopPosition is zero, no need to build a deceleration curve - if (stopPosition == 0.0) continue; - if (stopPosition > curveWithDecelerations.getEndPos()) { - if (arePositionsEqual(stopPosition, curveWithDecelerations.getEndPos())) - stopPosition = curveWithDecelerations.getEndPos(); - else throw OSRDError.newEnvelopeError(i, stopPosition, curveWithDecelerations.getEndPos()); - } - var partBuilder = new EnvelopePartBuilder(); - partBuilder.setAttr(EnvelopeProfile.BRAKING); - partBuilder.setAttr(new StopMeta(i)); - var overlayBuilder = new ConstrainedEnvelopePartBuilder( - partBuilder, - new SpeedConstraint(0, FLOOR), - new EnvelopeConstraint(curveWithDecelerations, CEILING)); - EnvelopeDeceleration.decelerate(context, stopPosition, 0, overlayBuilder, -1); - - var builder = OverlayEnvelopeBuilder.backward(curveWithDecelerations); - builder.addPart(partBuilder.build()); - curveWithDecelerations = builder.build(); - } - return curveWithDecelerations; - } - - /** Generate a max speed envelope given a mrsp */ - public static Envelope from(EnvelopeSimContext context, double[] stopPositions, Envelope mrsp) { - var maxSpeedEnvelope = addBrakingCurves(context, mrsp); - maxSpeedEnvelope = addStopBrakingCurves(context, stopPositions, maxSpeedEnvelope); - return maxSpeedEnvelope; - } -} diff --git a/core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/EnvelopeSimContext.kt b/core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/EnvelopeSimContext.kt new file mode 100644 index 00000000000..8359a1a09fe --- /dev/null +++ b/core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/EnvelopeSimContext.kt @@ -0,0 +1,25 @@ +package fr.sncf.osrd.envelope_sim + +import com.google.common.collect.RangeMap + +class EnvelopeSimContext( + @JvmField val rollingStock: PhysicsRollingStock, + @JvmField val path: PhysicsPath, + @JvmField val timeStep: Double, + tractiveEffortCurveMap: RangeMap> +) { + @JvmField + val tractiveEffortCurveMap: RangeMap> + + /** Creates a context suitable to run simulations on envelopes */ + init { + checkNotNull(tractiveEffortCurveMap) + this.tractiveEffortCurveMap = tractiveEffortCurveMap + } + + fun updateCurves( + tractiveEffortCurveMap: RangeMap> + ): EnvelopeSimContext { + return EnvelopeSimContext(rollingStock, path, timeStep, tractiveEffortCurveMap) + } +} diff --git a/core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/pipelines/MaxEffortEnvelope.kt b/core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/pipelines/MaxEffortEnvelope.kt new file mode 100644 index 00000000000..50221ef015a --- /dev/null +++ b/core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/pipelines/MaxEffortEnvelope.kt @@ -0,0 +1,182 @@ +package fr.sncf.osrd.envelope_sim.pipelines + +import fr.sncf.osrd.envelope.Envelope +import fr.sncf.osrd.envelope.EnvelopeCursor +import fr.sncf.osrd.envelope.OverlayEnvelopeBuilder +import fr.sncf.osrd.envelope.part.ConstrainedEnvelopePartBuilder +import fr.sncf.osrd.envelope.part.EnvelopePart +import fr.sncf.osrd.envelope.part.EnvelopePartBuilder +import fr.sncf.osrd.envelope.part.constraints.EnvelopeConstraint +import fr.sncf.osrd.envelope.part.constraints.EnvelopePartConstraintType +import fr.sncf.osrd.envelope.part.constraints.PositionConstraint +import fr.sncf.osrd.envelope.part.constraints.SpeedConstraint +import fr.sncf.osrd.envelope_sim.EnvelopeProfile +import fr.sncf.osrd.envelope_sim.EnvelopeSimContext +import fr.sncf.osrd.envelope_sim.overlays.EnvelopeAcceleration +import fr.sncf.osrd.envelope_sim.overlays.EnvelopeMaintain +import fr.sncf.osrd.reporting.exceptions.ErrorType +import fr.sncf.osrd.reporting.exceptions.OSRDError +import kotlin.math.max +import kotlin.math.min + +/** + * Max effort envelope = Max speed envelope + acceleration curves + check maintain speed It is the + * max physical speed at any given point, ignoring allowances + */ +object MaxEffortEnvelope { + /** Detects if an envelope parts is a plateau */ + @JvmStatic + fun maxEffortPlateau(part: EnvelopePart): Boolean { + return part.getMinSpeed() == part.getMaxSpeed() + } + + /** + * Generate acceleration curves overlay everywhere the max speed envelope increase with a + * discontinuity and compute the constant speed parts to check whether the train can physically + * maintain its speed + */ + @JvmStatic + fun addAccelerationAndConstantSpeedParts( + context: EnvelopeSimContext, + maxSpeedProfile: Envelope, + initialPosition: Double, + initialSpeed: Double + ): Envelope { + val builder = OverlayEnvelopeBuilder.forward(maxSpeedProfile) + val cursor = EnvelopeCursor.forward(maxSpeedProfile) + val maxSpeed = maxSpeedProfile.interpolateSpeedRightDir(initialPosition, 1.0) + if (initialSpeed < maxSpeed) { + accelerate(context, maxSpeedProfile, initialSpeed, initialPosition, builder, cursor) + } + while (!cursor.hasReachedEnd()) { + if (cursor.checkPart(MaxEffortEnvelope::maxEffortPlateau)) { + var partBuilder = EnvelopePartBuilder() + partBuilder.setAttr(EnvelopeProfile.CONSTANT_SPEED) + var startSpeed = cursor.stepBeginSpeed + var startPosition = cursor.getPosition() + var overlayBuilder = + ConstrainedEnvelopePartBuilder( + partBuilder, + SpeedConstraint(startSpeed, EnvelopePartConstraintType.EQUAL), + PositionConstraint(cursor.getPart().beginPos, cursor.getPart().endPos) + ) + EnvelopeMaintain.maintain(context, startPosition, startSpeed, overlayBuilder, 1.0) + + // check if the speed can be maintained from the first position before adding the + // part, + // otherwise it would only be a single point + if (partBuilder.stepCount() > 1) { + builder.addPart(partBuilder.build()) + cursor.findPosition(overlayBuilder.lastPos) + } + + // if the cursor didn't reach the end of the constant speed part, + // that means the train was slowed down by a steep ramp + if (cursor.getPosition() < cursor.getPart().endPos) { + partBuilder = EnvelopePartBuilder() + partBuilder.setAttr(EnvelopeProfile.CATCHING_UP) + overlayBuilder = + ConstrainedEnvelopePartBuilder( + partBuilder, + SpeedConstraint(0.0, EnvelopePartConstraintType.FLOOR), + EnvelopeConstraint(maxSpeedProfile, EnvelopePartConstraintType.CEILING) + ) + startPosition = cursor.getPosition() + startSpeed = maxSpeedProfile.interpolateSpeedLeftDir(startPosition, 1.0) + EnvelopeAcceleration.accelerate( + context, + startPosition, + startSpeed, + overlayBuilder, + 1.0 + ) + cursor.findPosition(overlayBuilder.lastPos) + + if (overlayBuilder.lastIntersection == 0) { + // Train stopped while trying to catch up + throw OSRDError(ErrorType.ImpossibleSimulationError) + } + + // check that the train was actually slowed down by the ramp + if (partBuilder.stepCount() > 0) // if the part has more than one point, add it + builder.addPart(partBuilder.build()) + else { + // otherwise skip this position as the train isn't really being slowed down + // and step 1m further + val maxPosition = + cursor.getPart().endPos // We don't want to skip further than the part + if (cursor.getPosition() < maxPosition) + cursor.findPosition(min(maxPosition, cursor.getPosition() + 1)) + } + } + } else if (cursor.checkPartTransition(MaxSpeedEnvelope::increase)) { + val startSpeed = maxSpeedProfile.interpolateSpeedLeftDir(cursor.getPosition(), 1.0) + accelerate( + context, + maxSpeedProfile, + startSpeed, + cursor.getPosition(), + builder, + cursor + ) + } else cursor.nextPart() + } + return builder.build() + } + + /** + * Accelerates starting at the given speed and position. Simple code factorization, it's called + * when starting up and at part transitions. + */ + private fun accelerate( + context: EnvelopeSimContext, + maxSpeedProfile: Envelope?, + initialSpeed: Double, + startPosition: Double, + builder: OverlayEnvelopeBuilder, + cursor: EnvelopeCursor + ) { + val partBuilder = EnvelopePartBuilder() + partBuilder.setAttr(EnvelopeProfile.ACCELERATING) + val overlayBuilder = + ConstrainedEnvelopePartBuilder( + partBuilder, + SpeedConstraint(0.0, EnvelopePartConstraintType.FLOOR), + EnvelopeConstraint(maxSpeedProfile, EnvelopePartConstraintType.CEILING) + ) + EnvelopeAcceleration.accelerate(context, startPosition, initialSpeed, overlayBuilder, 1.0) + cursor.findPosition(overlayBuilder.lastPos) + if (overlayBuilder.lastIntersection == 0) { + // The train stopped before reaching the end + val err = OSRDError(ErrorType.ImpossibleSimulationError) + val offset = cursor.getPosition() + err.context.put("offset", String.format("%.0fm", offset)) + val headPosition = min(max(0.0, offset), context.path.getLength()) + val tailPosition = + min( + max(0.0, headPosition - context.rollingStock.getLength()), + context.path.getLength() + ) + val grade = context.path.getAverageGrade(headPosition, tailPosition) + err.context.put("grade", String.format("%.2fm/km", grade)) + val map = context.tractiveEffortCurveMap[cursor.getPosition()]!! + err.context.put("traction_force", String.format("%.2fN", map[0].maxEffort)) + throw err + } + builder.addPart(partBuilder.build()) + } + + /** Generate a max effort envelope given a max speed envelope */ + @JvmStatic + fun from( + context: EnvelopeSimContext, + initialSpeed: Double, + maxSpeedProfile: Envelope + ): Envelope { + val maxEffortEnvelope = + addAccelerationAndConstantSpeedParts(context, maxSpeedProfile, 0.0, initialSpeed) + assert(maxEffortEnvelope.continuous) { "Discontinuity in max effort envelope" } + assert(maxEffortEnvelope.beginPos == 0.0) + return maxEffortEnvelope + } +} diff --git a/core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/pipelines/MaxSpeedEnvelope.kt b/core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/pipelines/MaxSpeedEnvelope.kt new file mode 100644 index 00000000000..969987c7e02 --- /dev/null +++ b/core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/pipelines/MaxSpeedEnvelope.kt @@ -0,0 +1,114 @@ +package fr.sncf.osrd.envelope_sim.pipelines + +import fr.sncf.osrd.envelope.Envelope +import fr.sncf.osrd.envelope.EnvelopeCursor +import fr.sncf.osrd.envelope.OverlayEnvelopeBuilder +import fr.sncf.osrd.envelope.part.ConstrainedEnvelopePartBuilder +import fr.sncf.osrd.envelope.part.EnvelopePartBuilder +import fr.sncf.osrd.envelope.part.constraints.EnvelopeConstraint +import fr.sncf.osrd.envelope.part.constraints.EnvelopePartConstraintType +import fr.sncf.osrd.envelope.part.constraints.SpeedConstraint +import fr.sncf.osrd.envelope_sim.EnvelopeProfile +import fr.sncf.osrd.envelope_sim.EnvelopeSimContext +import fr.sncf.osrd.envelope_sim.StopMeta +import fr.sncf.osrd.envelope_sim.TrainPhysicsIntegrator +import fr.sncf.osrd.envelope_sim.overlays.EnvelopeDeceleration +import fr.sncf.osrd.reporting.exceptions.OSRDError + +/** + * Max speed envelope = MRSP + braking curves It is the max speed allowed at any given point, + * ignoring allowances + */ +object MaxSpeedEnvelope { + fun increase(prevPos: Double, prevSpeed: Double, nextPos: Double, nextSpeed: Double): Boolean { + // Works for both accelerations (forwards) and decelerations (backwards) + return prevSpeed < nextSpeed + } + + /** + * Generate braking curves overlay everywhere the mrsp decrease (increase backwards) with a + * discontinuity + */ + private fun addBrakingCurves(context: EnvelopeSimContext?, mrsp: Envelope): Envelope { + val builder = OverlayEnvelopeBuilder.backward(mrsp) + val cursor = EnvelopeCursor.backward(mrsp) + var lastPosition = mrsp.endPos + while (cursor.findPartTransition(MaxSpeedEnvelope::increase)) { + if (cursor.getPosition() > lastPosition) { + // The next braking curve already covers this point, this braking curve is hidden + cursor.nextPart() + continue + } + val partBuilder = EnvelopePartBuilder() + partBuilder.setAttr(EnvelopeProfile.BRAKING) + val overlayBuilder = + ConstrainedEnvelopePartBuilder( + partBuilder, + SpeedConstraint(0.0, EnvelopePartConstraintType.FLOOR), + EnvelopeConstraint(mrsp, EnvelopePartConstraintType.CEILING) + ) + val startSpeed = cursor.getSpeed() + val startPosition = cursor.getPosition() + // TODO: link directionSign to cursor boolean reverse + EnvelopeDeceleration.decelerate( + context, + startPosition, + startSpeed, + overlayBuilder, + -1.0 + ) + builder.addPart(partBuilder.build()) + cursor.nextPart() + lastPosition = overlayBuilder.lastPos + } + return builder.build() + } + + /** Generate braking curves overlay at every stop position */ + private fun addStopBrakingCurves( + context: EnvelopeSimContext?, + stopPositions: DoubleArray, + curveWithDecelerations: Envelope + ): Envelope { + var curveWithDecelerations = curveWithDecelerations + for (i in stopPositions.indices) { + var stopPosition = stopPositions[i] + // if the stopPosition is zero, no need to build a deceleration curve + if (stopPosition == 0.0) continue + if (stopPosition > curveWithDecelerations.endPos) { + if ( + TrainPhysicsIntegrator.arePositionsEqual( + stopPosition, + curveWithDecelerations.endPos + ) + ) + stopPosition = curveWithDecelerations.endPos + else + throw OSRDError.newEnvelopeError(i, stopPosition, curveWithDecelerations.endPos) + } + val partBuilder = EnvelopePartBuilder() + partBuilder.setAttr(EnvelopeProfile.BRAKING) + partBuilder.setAttr(StopMeta(i)) + val overlayBuilder = + ConstrainedEnvelopePartBuilder( + partBuilder, + SpeedConstraint(0.0, EnvelopePartConstraintType.FLOOR), + EnvelopeConstraint(curveWithDecelerations, EnvelopePartConstraintType.CEILING) + ) + EnvelopeDeceleration.decelerate(context, stopPosition, 0.0, overlayBuilder, -1.0) + + val builder = OverlayEnvelopeBuilder.backward(curveWithDecelerations) + builder.addPart(partBuilder.build()) + curveWithDecelerations = builder.build() + } + return curveWithDecelerations + } + + /** Generate a max speed envelope given a mrsp */ + @JvmStatic + fun from(context: EnvelopeSimContext?, stopPositions: DoubleArray, mrsp: Envelope): Envelope { + var maxSpeedEnvelope = addBrakingCurves(context, mrsp) + maxSpeedEnvelope = addStopBrakingCurves(context, stopPositions, maxSpeedEnvelope) + return maxSpeedEnvelope + } +}