feat: optimize lateral paths with SQP

ClumsyPilot/tests/EMPlannerVerificationHost/FakeQpSolver.cs

@@ -8,11 +8,24 @@ namespace EMPlannerVerificationHost;
 
 internal sealed class FakeQpSolver : IQpSolver
 {
-    private readonly QpSolveResult _result;
+    private readonly Queue<QpSolveResult> _results;
+    private readonly List<QuadraticProgram> _problems = new List<QuadraticProgram>();
+    private readonly List<IReadOnlyList<double>> _warmStarts = new List<IReadOnlyList<double>>();
 
     public FakeQpSolver(QpSolveResult result)
+        : this(new[] { result })
     {
-        _result = result ?? throw new ArgumentNullException(nameof(result));
+    }
+
+    public FakeQpSolver(IEnumerable<QpSolveResult> results)
+    {
+        if (results == null)
+            throw new ArgumentNullException(nameof(results));
+        _results = new Queue<QpSolveResult>();
+        foreach (QpSolveResult result in results)
+            _results.Enqueue(result ?? throw new ArgumentException("Fake solver results cannot contain null values.", nameof(results)));
+        if (_results.Count == 0)
+            throw new ArgumentException("At least one fake solver result is required.", nameof(results));
         LastWarmStart = Array.Empty<double>();
     }
 
@@ -22,6 +35,12 @@ internal sealed class FakeQpSolver : IQpSolver
 
     public IReadOnlyList<double> LastWarmStart { get; private set; }
 
+    public IReadOnlyList<QuadraticProgram> Problems => new ReadOnlyCollection<QuadraticProgram>(_problems);
+
+    public IReadOnlyList<IReadOnlyList<double>> WarmStarts => new ReadOnlyCollection<IReadOnlyList<double>>(_warmStarts);
+
+    public int SolveCallCount => _problems.Count;
+
     public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
         CancellationToken cancellationToken)
     {
@@ -34,6 +53,10 @@ internal sealed class FakeQpSolver : IQpSolver
                 copy.Add(warmStart[index]);
         }
         LastWarmStart = new ReadOnlyCollection<double>(copy);
-        return _result;
+        _problems.Add(LastProblem);
+        _warmStarts.Add(LastWarmStart);
+        if (_results.Count == 0)
+            throw new InvalidOperationException("Fake solver was called more often than its scripted result sequence.");
+        return _results.Dequeue();
     }
 }