feat: separate rolling horizons from stop boundaries

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs

@@ -0,0 +1,8 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum EmLongitudinalMode
+{
+    RollingContinuation,
+    ApproachStopBoundary,
+    ExactStopAtBoundary,
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

@@ -58,11 +58,11 @@ public sealed class EmPlanningService : IEmPlanningService
                 initialAcceleration, configuration, out PlanningHorizonSelection horizon, out string horizonReason);
             if (horizonStatus != EmPlanningStatus.Success)
                 return Failure(horizonStatus, request, horizonReason);
-            ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.TerminalReferenceS);
+            ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS);
             EmitDebug(request, "exact horizon and terminal selection succeeded");
 
             IReadOnlyList<FrenetProjection> previousSeed = ProjectPreviousTrajectorySeed(request.PreviousTrajectory, segment,
-                startProjection.ReferenceS, horizon.TerminalReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
+                startProjection.ReferenceS, horizon.WindowEndReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
             var corridorSeed = new List<FrenetProjection>(previousSeed.Count + 1) { startProjection };
             for (int index = 0; index < previousSeed.Count; index++) corridorSeed.Add(previousSeed[index]);
             EmitDebug(request, "previous-trajectory seed projection completed");
@@ -83,7 +83,8 @@ public sealed class EmPlanningService : IEmPlanningService
             EmitDebug(request, "LS optimization and validation succeeded");
 
             var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
-                initialAcceleration, horizon.TerminalType, configuration, Array.Empty<double>(), Array.Empty<double>());
+                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
+                Array.Empty<double>(), Array.Empty<double>());
             EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason);
             if (envelopeStatus != EmPlanningStatus.Success)
                 return Failure(envelopeStatus, request, envelopeReason);

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs

@@ -11,7 +11,8 @@ public sealed class LongitudinalPlanningInput
     private const double PathSTolerance = 1e-12d;
 
     public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
-        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
+        EmPlannerConfiguration configuration,
         IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
     {
         if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
@@ -25,6 +26,12 @@ public sealed class LongitudinalPlanningInput
             throw new ArgumentOutOfRangeException(nameof(initialAccelerationMetersPerSecondSquared));
         if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
             throw new ArgumentOutOfRangeException(nameof(terminalType));
+        if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode))
+            throw new ArgumentOutOfRangeException(nameof(mode));
+        if (mode == EmLongitudinalMode.RollingContinuation && terminalType != EmTerminalType.RollingSafetyStop)
+            throw new ArgumentException("Rolling continuation requires a rolling window boundary.");
+        if (mode != EmLongitudinalMode.RollingContinuation && terminalType == EmTerminalType.RollingSafetyStop)
+            throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch.");
         if (configuration == null)
             throw new ArgumentNullException(nameof(configuration));
 
@@ -33,6 +40,7 @@ public sealed class LongitudinalPlanningInput
         InitialProgressSpeedMetersPerSecond = initialProgressSpeedMetersPerSecond;
         InitialAccelerationMetersPerSecondSquared = initialAccelerationMetersPerSecondSquared;
         TerminalType = terminalType;
+        Mode = mode;
         Configuration = configuration.Copy();
         PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
         PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
@@ -42,6 +50,17 @@ public sealed class LongitudinalPlanningInput
                 nameof(previousProgressSpeedMetersPerSecond));
     }
 
+    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
+        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
+        : this(path, direction, initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, terminalType,
+            terminalType == EmTerminalType.RollingSafetyStop
+                ? EmLongitudinalMode.RollingContinuation
+                : EmLongitudinalMode.ExactStopAtBoundary,
+            configuration, previousPathS, previousProgressSpeedMetersPerSecond)
+    {
+    }
+
     public LateralPath Path { get; }
 
     public TravelDirection Direction { get; }
@@ -52,13 +71,33 @@ public sealed class LongitudinalPlanningInput
 
     public EmTerminalType TerminalType { get; }
 
+    public EmLongitudinalMode Mode { get; }
+
     public EmPlannerConfiguration Configuration { get; }
 
     public IReadOnlyList<double> PreviousPathS { get; }
 
     public IReadOnlyList<double> PreviousProgressSpeedMetersPerSecond { get; }
 
-    public double TerminalPathS { get { return Path.Points[Path.Points.Count - 1].PathS; } }
+    public double PathUpperBoundS { get { return Path.Points[Path.Points.Count - 1].PathS; } }
+
+    public bool HasStopBoundary { get { return TerminalType != EmTerminalType.RollingSafetyStop; } }
+
+    public double StopBoundaryPathS { get { return HasStopBoundary ? PathUpperBoundS : double.NaN; } }
+
+    public EmBoundaryType StopBoundaryType
+    {
+        get
+        {
+            return TerminalType == EmTerminalType.Goal
+                ? EmBoundaryType.Goal
+                : TerminalType == EmTerminalType.GearSwitch
+                    ? EmBoundaryType.GearSwitchApproach
+                    : EmBoundaryType.None;
+        }
+    }
+
+    public double TerminalPathS { get { return PathUpperBoundS; } }
 
     public double DirectionMaximumSpeedMetersPerSecond
     {

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalTerminalSchedule.cs

@@ -0,0 +1,39 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public static class LongitudinalTerminalSchedule
+{
+    public static int GetStabilizationStartIndex(IReadOnlyList<double> knotTimes,
+        double minimumStabilizationDurationSeconds)
+    {
+        if (knotTimes == null || knotTimes.Count < 3 ||
+            double.IsNaN(minimumStabilizationDurationSeconds) ||
+            double.IsInfinity(minimumStabilizationDurationSeconds) ||
+            minimumStabilizationDurationSeconds <= 0d)
+        {
+            throw new ArgumentException("A positive stabilization tail and at least three knots are required.");
+        }
+
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < knotTimes.Count; index++)
+        {
+            if (double.IsNaN(knotTimes[index]) || double.IsInfinity(knotTimes[index]) ||
+                knotTimes[index] <= previous)
+            {
+                throw new ArgumentException("Terminal-schedule knots must be finite and strictly increasing.");
+            }
+            previous = knotTimes[index];
+        }
+
+        double finalTime = knotTimes[knotTimes.Count - 1];
+        for (int index = knotTimes.Count - 2; index >= 1; index--)
+        {
+            if (finalTime - knotTimes[index] >= minimumStabilizationDurationSeconds - 1e-12d)
+                return index;
+        }
+
+        throw new ArgumentException("The time horizon cannot contain a full terminal stabilization interval.");
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs

@@ -1,4 +1,5 @@
 using System;
+using System.Collections.Generic;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
@@ -6,15 +7,31 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 /// <summary>Reference-distance terminal chosen before LS without crossing the current direction segment.</summary>
 public sealed class PlanningHorizonSelection
 {
-    internal PlanningHorizonSelection(double terminalReferenceS, EmTerminalType terminalType)
+    internal PlanningHorizonSelection(double windowEndReferenceS, EmBoundaryType windowEndBoundaryType,
+        EmTerminalType terminalType, EmLongitudinalMode longitudinalMode,
+        double stopBoundaryReferenceS, bool hasStopBoundary)
     {
-        TerminalReferenceS = terminalReferenceS;
+        WindowEndReferenceS = windowEndReferenceS;
+        WindowEndBoundaryType = windowEndBoundaryType;
         TerminalType = terminalType;
+        LongitudinalMode = longitudinalMode;
+        StopBoundaryReferenceS = stopBoundaryReferenceS;
+        HasStopBoundary = hasStopBoundary;
     }
 
-    public double TerminalReferenceS { get; }
+    public double WindowEndReferenceS { get; }
+
+    public double TerminalReferenceS => WindowEndReferenceS;
+
+    public EmBoundaryType WindowEndBoundaryType { get; }
 
     public EmTerminalType TerminalType { get; }
+
+    public EmLongitudinalMode LongitudinalMode { get; }
+
+    public double StopBoundaryReferenceS { get; }
+
+    public bool HasStopBoundary { get; }
 }
 
 public sealed class PlanningHorizonSelector
@@ -73,75 +90,41 @@ public sealed class PlanningHorizonSelector
             return EmPlanningStatus.StoppingDistanceInsufficient;
         }
 
-        double timeReachable = CalculateReachableDistance(initialProgressSpeedMetersPerSecond,
+        double windowEnd = Math.Min(currentSegmentReferenceS + scheduling.DistanceHorizonMeters, segment.LengthMeters);
-            initialAccelerationMetersPerSecondSquared, directionMaximum, longitudinal, scheduling.TimeHorizonSeconds);
+        bool windowReachesSegmentEnd = windowEnd >= segment.LengthMeters - BoundaryTolerance;
-        double terminalReferenceS = currentSegmentReferenceS + Math.Min(remainingSegment,
+        bool hasStopBoundary = windowReachesSegmentEnd && IsStopBoundary(segment.EndBoundary.BoundaryType);
-            Math.Min(scheduling.DistanceHorizonMeters, timeReachable));
+        EmBoundaryType windowEndBoundaryType = windowReachesSegmentEnd
-        if (terminalReferenceS >= segment.LengthMeters - BoundaryTolerance)
+            ? segment.EndBoundary.BoundaryType
+            : EmBoundaryType.RollingSafetyStop;
+        if (!hasStopBoundary)
         {
-            terminalReferenceS = segment.LengthMeters;
+            selection = new PlanningHorizonSelection(windowEnd, windowEndBoundaryType,
-            selection = new PlanningHorizonSelection(terminalReferenceS, ToTerminalType(segment.EndBoundary.BoundaryType));
+                EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation,
-        }
+                segment.LengthMeters, false);
-        else
+            return EmPlanningStatus.Success;
-        {
-            selection = new PlanningHorizonSelection(terminalReferenceS, EmTerminalType.RollingSafetyStop);
         }
+
+        IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
+            scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
+        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+            knotTimes, scheduling.OutputTimeStepSeconds);
+        double availableMotionTime = knotTimes[stabilizationStart];
+        double maximumStoppedDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
+            initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, directionMaximum,
+            longitudinal.MaximumAccelerationMetersPerSecondSquared,
+            longitudinal.MaximumDecelerationMetersPerSecondSquared,
+            longitudinal.MaximumJerkMetersPerSecondCubed, availableMotionTime);
+        EmLongitudinalMode mode = remainingSegment <= maximumStoppedDistance + BoundaryTolerance
+            ? EmLongitudinalMode.ExactStopAtBoundary
+            : EmLongitudinalMode.ApproachStopBoundary;
+        selection = new PlanningHorizonSelection(segment.LengthMeters, windowEndBoundaryType,
+            ToTerminalType(segment.EndBoundary.BoundaryType), mode, segment.LengthMeters, true);
         return EmPlanningStatus.Success;
     }
 
-    private static double CalculateReachableDistance(double initialSpeed, double initialAcceleration, double maximumSpeed,
+    private static bool IsStopBoundary(EmBoundaryType boundaryType)
-        LongitudinalConfiguration configuration, double timeHorizonSeconds)
     {
-        if (!JerkLimitedStoppingMath.TryCalculate(maximumSpeed, 0d,
+        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
-                configuration.MaximumDecelerationMetersPerSecondSquared,
-                configuration.MaximumJerkMetersPerSecondCubed,
-                out JerkLimitedStoppingProfile stopAtMaximumSpeed, out _))
-        {
-            throw new ArgumentOutOfRangeException(nameof(configuration));
-        }
-        double drivingDuration = timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds - stopAtMaximumSpeed.DurationSeconds;
-        if (drivingDuration <= 0d)
-            return Math.Min(initialSpeed, maximumSpeed) * Math.Max(0d, timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds);
-
-        double speed = initialSpeed;
-        double acceleration = initialAcceleration;
-        double distance = 0d;
-        const double simulationStepSeconds = 0.001d;
-        while (drivingDuration > 0d)
-        {
-            double step = Math.Min(simulationStepSeconds, drivingDuration);
-            double jerk = ChooseAccelerationJerk(speed, acceleration, maximumSpeed,
-                configuration.MaximumAccelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed);
-            double nextSpeed = speed + acceleration * step + 0.5d * jerk * step * step;
-            if (nextSpeed > maximumSpeed)
-            {
-                nextSpeed = maximumSpeed;
-                acceleration = 0d;
-                jerk = 0d;
-            }
-            distance += speed * step + 0.5d * acceleration * step * step + jerk * step * step * step / 6d;
-            acceleration += jerk * step;
-            speed = Math.Max(0d, nextSpeed);
-            drivingDuration -= step;
-        }
-        return Math.Max(0d, distance + stopAtMaximumSpeed.DistanceMeters);
-    }
-
-    private static double ChooseAccelerationJerk(double speed, double acceleration, double maximumSpeed,
-        double maximumAcceleration, double maximumJerk)
-    {
-        if (speed >= maximumSpeed - BoundaryTolerance)
-        {
-            if (acceleration > 0d)
-                return -maximumJerk;
-            return acceleration < 0d ? maximumJerk : 0d;
-        }
-        double speedToReduceAccelerationToZero = acceleration > 0d
-            ? acceleration * acceleration / (2d * maximumJerk)
-            : 0d;
-        if (speed + speedToReduceAccelerationToZero >= maximumSpeed - BoundaryTolerance)
-            return acceleration > 0d ? -maximumJerk : 0d;
-        return acceleration < maximumAcceleration - BoundaryTolerance ? maximumJerk : 0d;
     }
 
     private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs

@@ -1,4 +1,5 @@
 using System;
+using System.Collections.Generic;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing;
 using MultiWheelC.TrajectoryPlanning.Utils;
@@ -142,6 +143,45 @@ public static class EmPlanningRequestValidator
             !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights))
             return false;
 
+        if (!JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumForwardSpeedMetersPerSecond,
+                longitudinal.MaximumAccelerationMetersPerSecondSquared,
+                longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile forwardStop,
+                out _) ||
+            !JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumReverseSpeedMetersPerSecond,
+                longitudinal.MaximumAccelerationMetersPerSecondSquared,
+                longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile reverseStop,
+                out _))
+        {
+            failureReason = "Configuration cannot construct the jerk-limited stopping model.";
+            return false;
+        }
+
+        double maximumSpeed = Math.Max(longitudinal.MaximumForwardSpeedMetersPerSecond,
+            longitudinal.MaximumReverseSpeedMetersPerSecond);
+        double requiredDistanceHorizon = Math.Max(forwardStop.DistanceMeters, reverseStop.DistanceMeters) +
+            maximumSpeed * scheduling.ReplanPeriodSeconds;
+        if (scheduling.DistanceHorizonMeters + 1e-12d < requiredDistanceHorizon)
+        {
+            failureReason = "DistanceHorizonMeters is insufficient: required=" + requiredDistanceHorizon +
+                ";configured=" + scheduling.DistanceHorizonMeters;
+            return false;
+        }
+
+        try
+        {
+            IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
+                scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
+            LongitudinalTerminalSchedule.GetStabilizationStartIndex(knotTimes,
+                scheduling.OutputTimeStepSeconds);
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = exception.Message;
+            return false;
+        }
+
         return true;
     }
 

ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs

@@ -106,6 +106,17 @@ internal static class FoundationChecks
         VerifyLateralWeights(configuration.Lateral.Weights);
         VerifyLongitudinalWeights(configuration.Longitudinal.Weights);
 
+        EmPlannerConfiguration insufficientPreview = EmPlannerConfiguration.CreateDefault();
+        insufficientPreview.Scheduling.DistanceHorizonMeters = 0.01d;
+        EmPlanningRequestValidationResult insufficientPreviewValidation = EmPlanningRequestValidator.Validate(
+            CreateValidRequest(insufficientPreview));
+        AssertStatus(EmPlanningStatus.InvalidInput, insufficientPreviewValidation, "insufficient stopping preview");
+        EMPlannerVerificationHost.Verification.True(
+            insufficientPreviewValidation.FailureReason.Contains("DistanceHorizonMeters") &&
+            insufficientPreviewValidation.FailureReason.Contains("required") &&
+            insufficientPreviewValidation.FailureReason.Contains("configured"),
+            "insufficient stopping preview describes configured and required distance");
+
         EmPlanningRequest valid = CreateValidRequest(configuration);
         AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
             CreateRequest(null!, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null reference path");

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs

@@ -170,7 +170,7 @@ internal static class LongitudinalIntegrationChecks
         }, true);
         var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
             baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
-            baseline.TerminalType, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
+            baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
         EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
             out string speedFailure);
         Verification.Equal(EmPlanningStatus.Success, speedStatus, "curved-envelope setup: " + speedFailure);
@@ -212,7 +212,7 @@ internal static class LongitudinalIntegrationChecks
         }, true);
         var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
             baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
-            baseline.TerminalType, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
+            baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
         var solver = new FakeQpSolver(new[]
         {
             Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d),
@@ -335,7 +335,8 @@ internal static class LongitudinalIntegrationChecks
             Point(2d, terminalPathS, 0d),
         };
         return new LongitudinalScenario(name, new LongitudinalPlanningInput(new LateralPath(points, true), direction,
-            initialSpeed, initialAcceleration, EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>()));
+            initialSpeed, initialAcceleration, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
+            configuration, Array.Empty<double>(), Array.Empty<double>()));
     }
 
     private static void VerifyRealScenario(LongitudinalScenario scenario, LongitudinalPlanningResult result)
@@ -392,7 +393,7 @@ internal static class LongitudinalIntegrationChecks
             Point(2d, terminalPathS, 0d),
         }, true);
         return new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d, EmTerminalType.Goal,
-            configuration, Array.Empty<double>(), Array.Empty<double>());
+            EmLongitudinalMode.ExactStopAtBoundary, configuration, Array.Empty<double>(), Array.Empty<double>());
     }
 
     private static LateralPathPoint Point(double referenceS, double pathS, double curvature)

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -16,7 +16,7 @@ internal static class LongitudinalModelChecks
         VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
         VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations();
         VerifiesStoppingPrecheckBeforeQpAssembly();
-        VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary();
+        VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
         VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
     }
 
@@ -64,7 +64,8 @@ internal static class LongitudinalModelChecks
             new PathFixture(1d, 1d, 0d, 0d),
         });
         var directionInput = new LongitudinalPlanningInput(directionPath, TravelDirection.Forward, 0d, 0d,
-            EmTerminalType.Goal, EmPlannerConfiguration.CreateDefault(), Array.Empty<double>(), Array.Empty<double>());
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, EmPlannerConfiguration.CreateDefault(),
+            Array.Empty<double>(), Array.Empty<double>());
         EmPlanningStatus directionStatus = new PathSpeedLimitBuilder().Build(directionInput,
             out PathSpeedLimit directionEnvelope, out string directionFailureReason);
         Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " +
@@ -83,7 +84,8 @@ internal static class LongitudinalModelChecks
             new PathFixture(21d, 5d, 0d, 0d),
         });
         var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
-            EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
 
         EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
             out string failureReason);
@@ -124,7 +126,8 @@ internal static class LongitudinalModelChecks
             new PathFixture(100d, 2d, 0d, 0d),
         });
         var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
-            EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
 
         EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
             out string failureReason);
@@ -145,7 +148,8 @@ internal static class LongitudinalModelChecks
             new PathFixture(100d, 2d, 0d, 0d),
         });
         var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
-            EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
 
         EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
             out string failureReason);
@@ -173,7 +177,8 @@ internal static class LongitudinalModelChecks
             new PathFixture(100d, 0.01d, 0d, 0d),
         });
         var input = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
-            EmTerminalType.RollingSafetyStop, configuration, Array.Empty<double>(), Array.Empty<double>());
+            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
+            Array.Empty<double>(), Array.Empty<double>());
 
         EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
             out string failureReason);
@@ -183,24 +188,46 @@ internal static class LongitudinalModelChecks
         Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
     }
 
-    private static void VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary()
+    private static void VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime()
     {
         EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
-        DirectionSegmentView shortGoal = CreateSegment(0.25d, EmBoundaryType.Goal);
+        configuration.Scheduling.DistanceHorizonMeters = 1d;
-        EmPlanningStatus status = new PlanningHorizonSelector().Select(shortGoal, 0d, 0d, 0d, configuration,
+        configuration.Scheduling.TimeHorizonSeconds = 2d;
-            out PlanningHorizonSelection goalSelection, out string goalFailure);
 
-        Verification.Equal(EmPlanningStatus.Success, status, "goal horizon status: " + goalFailure);
+        DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.Goal);
-        Verification.Equal(EmTerminalType.Goal, goalSelection.TerminalType, "goal terminal type");
+        EmPlanningStatus status = new PlanningHorizonSelector().Select(
-        Verification.NearlyEqual(0.25d, goalSelection.TerminalReferenceS, "goal terminal reference S");
+            longSegment, 0d, 0d, 0d, configuration,
+            out PlanningHorizonSelection rolling, out string failure);
+        Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
+        Verification.NearlyEqual(1d, rolling.WindowEndReferenceS,
+            "distance horizon defines the LS window");
+        Verification.Equal(EmLongitudinalMode.RollingContinuation,
+            rolling.LongitudinalMode, "far boundary rolls");
 
-        DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.GearSwitchApproach);
+        DirectionSegmentView visibleButFar = CreateSegment(0.35d, EmBoundaryType.Goal);
-        status = new PlanningHorizonSelector().Select(longSegment, 0d, 0d, 0d, configuration,
+        status = new PlanningHorizonSelector().Select(
-            out PlanningHorizonSelection rollingSelection, out string rollingFailure);
+            visibleButFar, 0d, 0d, 0d, configuration,
-        Verification.Equal(EmPlanningStatus.Success, status, "rolling horizon status: " + rollingFailure);
+            out PlanningHorizonSelection approach, out failure);
-        Verification.Equal(EmTerminalType.RollingSafetyStop, rollingSelection.TerminalType, "rolling terminal type");
+        Verification.Equal(EmPlanningStatus.Success, status, "approach selection: " + failure);
-        Verification.True(rollingSelection.TerminalReferenceS >= 0d && rollingSelection.TerminalReferenceS < 4d,
+        Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
-            "rolling horizon remains within the current segment");
+            approach.LongitudinalMode, "visible unreachable boundary approaches");
+
+        DirectionSegmentView reachableGoal = CreateSegment(0.10d, EmBoundaryType.Goal);
+        status = new PlanningHorizonSelector().Select(
+            reachableGoal, 0d, 0d, 0d, configuration,
+            out PlanningHorizonSelection exact, out failure);
+        Verification.Equal(EmPlanningStatus.Success, status, "exact selection: " + failure);
+        Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
+            exact.LongitudinalMode, "reachable goal stops exactly");
+        Verification.Equal(EmTerminalType.Goal, exact.TerminalType,
+            "exact stop preserves Goal identity");
+
+        IReadOnlyList<double> regularTimes = LongitudinalCandidate.CreateKnotTimes(2d, 0.1d);
+        Verification.Equal(19, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+            regularTimes, 0.1d), "regular exact stop reserves t=1.9..2.0");
+        Verification.Equal(1, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+            new[] { 0d, 0.1d, 0.2d, 0.25d }, 0.1d),
+            "short final interval moves the stop anchor earlier");
     }
 
     private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
@@ -240,7 +267,8 @@ internal static class LongitudinalModelChecks
             new PathFixture(2d, 2d, 0d, 0d),
         });
         var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
-            EmTerminalType.Goal, configuration, new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
             new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
         EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
             out string speedFailure);