core: fix max speed in concatenated envelopes

Signed-off-by: Eloi Charpentier <[email protected]>

Author: eckter

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

@@ -1,6 +1,8 @@
 package fr.sncf.osrd.envelope;
 
 import static fr.sncf.osrd.envelope_utils.DoubleUtils.clamp;
+import static java.lang.Math.max;
+import static java.lang.Math.min;
 
 import java.util.ArrayList;
 import java.util.List;
@@ -158,9 +160,11 @@ public double maxSpeedInRange(double beginPos, double endPos) {
         assert firstEnvelopeIndex != -1 : "Trying to interpolate time outside of the envelope";
         var firstEnvelope = envelopes.get(firstEnvelopeIndex);
         var beginSpeed = firstEnvelope.envelope.maxSpeedInRange(
-                beginPos - firstEnvelope.startOffset, firstEnvelope.envelope.getEndPos());
+                beginPos - firstEnvelope.startOffset,
+                min(firstEnvelope.envelope.getEndPos(), endPos - firstEnvelope.startOffset));
 
         var lastEnvelopeIndex = findEnvelopeIndexAt(endPos, true);
+        if (lastEnvelopeIndex == firstEnvelopeIndex) return beginSpeed;
         assert lastEnvelopeIndex != -1 : "Trying to interpolate time outside of the envelope";
         var lastEnvelope = envelopes.get(lastEnvelopeIndex);
         var endOffset = endPos - lastEnvelope.startOffset;
@@ -169,14 +173,13 @@ public double maxSpeedInRange(double beginPos, double endPos) {
         }
         var endSpeed = lastEnvelope.envelope.maxSpeedInRange(0, endOffset);
 
-        var maxSpeed = beginSpeed;
+        var maxSpeed = max(beginSpeed, endSpeed);
         for (var i = firstEnvelopeIndex + 1; i < lastEnvelopeIndex; i++) {
             var envelope = envelopes.get(i);
             var speed = envelope.envelope.maxSpeedInRange(0, envelope.envelope.getEndPos());
             if (speed > maxSpeed) maxSpeed = speed;
         }
 
-        if (endSpeed > maxSpeed) maxSpeed = endSpeed;
         return maxSpeed;
     }