chore: save current workspace progress

ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs

@@ -16,6 +16,8 @@ internal static class LateralIntegrationChecks
     {
         VerifiesValidatedCandidateSurvivesLaterTimeout();
         VerifiesInvalidVectorsAndInaccurateResidualsNeverBecomeFallbacks();
+        VerifiesCancellationAfterStrictCandidateNeverPublishesFallback();
+        VerifiesRejectedCandidateAdvancesTheNextLateralLinearization();
         VerifiesTrustRegionWarmStartAndOuterIterationLimit();
         VerifiesCancellationAndTimeoutWithoutCandidate();
         VerifiesLateralPlannerDelegatesToTheSequentialOptimizer();
@@ -150,6 +152,47 @@ internal static class LateralIntegrationChecks
             "SolvedInaccurate still requires full independent lateral validation");
     }
 
+    private static void VerifiesCancellationAfterStrictCandidateNeverPublishesFallback()
+    {
+        LateralPlanningInput input = CreateInput();
+        double[] valid = CreatePrimal(input, 0.02d);
+        using var cancellation = new CancellationTokenSource();
+        var solver = new CancellingQpSolver(
+            new FakeQpSolver(Result(QpSolveStatus.Solved, valid, 10d)), cancellation);
+
+        LateralPlanningResult result = new SequentialConvexOptimizer(solver).Optimize(input, cancellation.Token);
+
+        Verification.Equal(EmPlanningStatus.Cancelled, result.Status,
+            "cancellation after a strict LS candidate is never fallback success");
+        Verification.True(result.Path == null, "cancelled LS result exposes no path");
+    }
+
+    private static void VerifiesRejectedCandidateAdvancesTheNextLateralLinearization()
+    {
+        LateralPlanningInput input = CreateInput(0.10d);
+        double[] strict = CreatePrimal(input, 0.01d);
+        double[] rejected = CreatePrimal(input, 0.02d);
+        var solver = new FakeQpSolver(new[]
+        {
+            Result(QpSolveStatus.Solved, strict, 10d),
+            Result(QpSolveStatus.Solved, rejected, 9d),
+            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 9d),
+        });
+
+        LateralPlanningResult result = new SequentialConvexOptimizer(solver).Optimize(input, CancellationToken.None);
+
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
+            "later timeout preserves the earlier strict candidate");
+        var layout = new LateralVariableLayout(input.ReferenceStations.Count);
+        Verification.NearlyEqual(0.02d, solver.WarmStarts[2][layout.L(1)],
+            "the next LS QP warm-starts from the rejected but parseable candidate");
+        FindSingleVariableBounds(solver.Problems[2], layout.L(1), out double lower, out double upper);
+        Verification.NearlyEqual(-0.03d, lower,
+            "the next LS trust region is centered on the rejected candidate");
+        Verification.NearlyEqual(0.07d, upper,
+            "the next LS trust region is centered on the rejected candidate");
+    }
+
     private static void VerifiesTrustRegionWarmStartAndOuterIterationLimit()
     {
         LateralPlanningInput input = CreateInput();
@@ -178,7 +221,10 @@ internal static class LateralIntegrationChecks
         var limitSolver = new FakeQpSolver(limitResults);
         LateralPlanningResult limited = new SequentialConvexOptimizer(limitSolver).Optimize(input, CancellationToken.None);
         Verification.Equal(5, limitSolver.SolveCallCount, "outer loop stops after at most five QP calls");
-        Verification.Equal(EmPlanningStatus.Success, limited.Status, "last feasible candidate succeeds at outer iteration limit");
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, limited.Status,
+            "last strict candidate is reported as fallback at outer iteration limit");
+        Verification.True(limited.FailureReason.Length > 0,
+            "outer iteration fallback preserves a non-empty diagnostic");
     }
 
     private static void VerifiesCancellationAndTimeoutWithoutCandidate()
@@ -319,7 +365,7 @@ internal static class LateralIntegrationChecks
             Verification.True(Math.Abs(leftValues[index] - rightValues[index]) <= 1e-10d, name + " value " + index);
     }
 
-    private static LateralPlanningInput CreateInput()
+    private static LateralPlanningInput CreateInput(double maximumVehicleCurvature = 1d)
     {
         var points = new List<SmoothedPathPoint>
         {
@@ -341,7 +387,7 @@ internal static class LateralIntegrationChecks
             LengthMeters = 0.1d,
             WidthMeters = 0.1d,
             SafetyMarginMeters = 0d,
-            MaximumCurvaturePerMeter = 1d,
+            MaximumCurvaturePerMeter = maximumVehicleCurvature,
         };
         return new LateralPlanningInput(segment, corridor,
             new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0d, 0d, 0d),