feat: add complete jerk-limited stopping math

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs

@@ -60,9 +60,13 @@ public sealed class PlanningHorizonSelector
         }
 
         double remainingSegment = Math.Max(0d, segment.LengthMeters - currentSegmentReferenceS);
-        LongitudinalStoppingProfile initialStop = LongitudinalStoppingMath.Calculate(initialProgressSpeedMetersPerSecond,
-            initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared,
-            longitudinal.MaximumJerkMetersPerSecondCubed);
+        if (!JerkLimitedStoppingMath.TryCalculate(initialProgressSpeedMetersPerSecond,
+                initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile initialStop,
+                out failureReason))
+        {
+            return EmPlanningStatus.InvalidInput;
+        }
         if (initialStop.DistanceMeters + BoundaryTolerance > remainingSegment)
         {
             failureReason = "The current segment lacks the jerk-limited stopping distance.";
@@ -88,8 +92,13 @@ public sealed class PlanningHorizonSelector
     private static double CalculateReachableDistance(double initialSpeed, double initialAcceleration, double maximumSpeed,
         LongitudinalConfiguration configuration, double timeHorizonSeconds)
     {
-        LongitudinalStoppingProfile stopAtMaximumSpeed = LongitudinalStoppingMath.Calculate(maximumSpeed, 0d,
-            configuration.MaximumDecelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed);
+        if (!JerkLimitedStoppingMath.TryCalculate(maximumSpeed, 0d,
+                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);