feat: derive adaptive full-segment ST schedule

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs

@@ -14,6 +14,16 @@ public sealed class LongitudinalPlanningInput
         double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
         EmPlannerConfiguration configuration,
         IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
+        : this(path, direction, initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared,
+            terminalType, mode, configuration, EmPlanningScope.RollingHorizon,
+            CreateRollingSchedule(configuration), previousPathS, previousProgressSpeedMetersPerSecond)
+    {
+    }
+
+    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
+        EmPlannerConfiguration configuration, EmPlanningScope planningScope, LongitudinalKnotSchedule knotSchedule,
+        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
     {
         if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
             throw new ArgumentException("Longitudinal planning requires an independently validated lateral path with at least two points.",
@@ -34,6 +44,13 @@ public sealed class LongitudinalPlanningInput
             throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch.");
         if (configuration == null)
             throw new ArgumentNullException(nameof(configuration));
+        if (!Enum.IsDefined(typeof(EmPlanningScope), planningScope))
+            throw new ArgumentOutOfRangeException(nameof(planningScope));
+        if (knotSchedule == null)
+            throw new ArgumentNullException(nameof(knotSchedule));
+        if ((planningScope == EmPlanningScope.FullDirectionSegment) != knotSchedule.IsAdaptive)
+            throw new ArgumentException("Full-direction planning requires an adaptive schedule and rolling planning requires a rolling schedule.",
+                nameof(knotSchedule));
 
         Path = CopyAndValidatePath(path);
         Direction = direction;
@@ -42,6 +59,8 @@ public sealed class LongitudinalPlanningInput
         TerminalType = terminalType;
         Mode = mode;
         Configuration = configuration.Copy();
+        PlanningScope = planningScope;
+        KnotSchedule = knotSchedule.Copy();
         PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
         PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
             nameof(previousProgressSpeedMetersPerSecond));
@@ -75,6 +94,10 @@ public sealed class LongitudinalPlanningInput
 
     public EmPlannerConfiguration Configuration { get; }
 
+    public EmPlanningScope PlanningScope { get; }
+
+    public LongitudinalKnotSchedule KnotSchedule { get; }
+
     public IReadOnlyList<double> PreviousPathS { get; }
 
     public IReadOnlyList<double> PreviousProgressSpeedMetersPerSecond { get; }
@@ -143,6 +166,14 @@ public sealed class LongitudinalPlanningInput
         return new ReadOnlyCollection<double>(copy);
     }
 
+    private static LongitudinalKnotSchedule CreateRollingSchedule(EmPlannerConfiguration configuration)
+    {
+        if (configuration == null || configuration.Scheduling == null)
+            throw new ArgumentNullException(nameof(configuration));
+        return LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds,
+            configuration.Scheduling.OutputTimeStepSeconds);
+    }
+
     private static bool IsFinite(double value)
     {
         return !double.IsNaN(value) && !double.IsInfinity(value);