# EM FullDirection Correctness Fixes Implementation Plan

> **For agentic workers:** REQUIRED SUB-SKILL: Use test-driven-development for every task. Execute tasks serially because they share the EM solver pipeline.

**Goal:** Fix confirmed FullDirection projection, cancellation, curvature-constraint, timeout-budget, and trajectory-coordinate defects without replacing the existing LS/ST planner.

**Architecture:** Keep `IEmPlanningService`, `EmPlanningRequest`, `LateralPlanner`, `LongitudinalPlanner`, and trajectory contracts compatible. Apply local fixes where one component owns the invariant; introduce only a shared internal lateral-curvature affine model and internal explicit-budget overloads where the same invariant necessarily crosses components.

**Tech Stack:** C# 10, .NET Standard 2.0 production assembly, .NET 8 verification host, solver-neutral `IQpSolver` tests.

## Global Constraints

- `FullDirectionSegment` plans exactly one complete direction segment; `RollingHorizon` behavior is not redesigned in this plan.
- FullDirection ego admission is restricted to the segment-start prefix `[0, min(L, MaximumProjectionDistanceMeters)]`; it must not select a later U-shape/self-overlap branch.
- A start heading error with magnitude greater than or equal to `π/2` is rejected before `tan(headingError)` is evaluated.
- `Cancelled` always carries a null trajectory/path/candidate, even if a strict fallback candidate exists.
- `SolverTimeoutSeconds` is one combined LS+ST solve budget for a service call, not a fresh budget for each optimizer.
- Every LS QP has a finite linearized vehicle-curvature hard-bound row at every station; nonlinear validation remains authoritative.
- `SegmentLocalS` stores interpolated direction-segment reference S; `PathS` stores optimized lateral-path arc length.
- Do not edit or revert the user's existing `EmPlannerConfiguration.cs` change, PathSmoothing work, Map work, or `ClumsyPilot.csproj` changes.
- Do not add actuator calls, change public EM request/result signatures, or commit/stage files from the dirty shared worktree.

---

### Task 1: Anchor FullDirection start projection and reject folded headings

**Files:**
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`

**Interfaces:**
- Consumes: existing `FrenetProjector.TryProject` bounded-window overload.
- Produces: service-local FullDirection start-prefix admission; Rolling continues using `[0,L]`.

- [ ] **Step 1: Write failing service tests.** Add a FullDirection U-shaped all-forward reference whose later arm is closer to the measured pose, and assert `ProjectionFailed` rather than accepting a later `ReferenceS`. Add a same-position start pose with yaw `π`, and assert `ProjectionFailed` with a null trajectory.

```csharp
EmPlanningResult wrongBranch = service.Plan(fullURequest, CancellationToken.None);
Verification.Equal(EmPlanningStatus.ProjectionFailed, wrongBranch.Status,
    "FullDirection cannot enter through a later U branch");

EmPlanningResult reversedHeading = service.Plan(oppositeHeadingRequest, CancellationToken.None);
Verification.Equal(EmPlanningStatus.ProjectionFailed, reversedHeading.Status,
    "opposite start heading is rejected before slope conversion");
```

- [ ] **Step 2: Run the focused test and verify RED.**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-planning-service
```

Expected: the later U branch and/or opposite-heading assertion fails under the current global `[0,L]`, seed-zero projection.

- [ ] **Step 3: Implement the local admission rule.** In `EmPlanningService.Plan`, choose the projection upper bound from scope and validate heading before constructing lateral input.

```csharp
double startProjectionUpperS = request.PlanningScope == EmPlanningScope.FullDirectionSegment
    ? Math.Min(segment.LengthMeters, configuration.Frenet.MaximumProjectionDistanceMeters)
    : segment.LengthMeters;
if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, startProjectionUpperS,
        configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection) ||
    Math.Abs(startProjection.HeadingError) >= Math.PI / 2d)
{
    return Failure(EmPlanningStatus.ProjectionFailed, request,
        "Vehicle pose is not an admissible start state for the selected direction segment.");
}
```

- [ ] **Step 4: Re-run `em-planning-service` and verify GREEN.** Existing Rolling projection behavior must remain green.

### Task 2: Make cancellation terminal and non-publishable

**Files:**
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`

**Interfaces:**
- Produces: `Cancelled` results with null candidate/path/trajectory at every layer.

- [ ] **Step 1: Write failing optimizer tests.** Use a solver that returns one valid candidate and cancels the supplied source before the next iteration. Assert both optimizers return `Cancelled`, not `SuccessWithFallback`, and expose no candidate.

```csharp
Verification.Equal(EmPlanningStatus.Cancelled, result.Status,
    "cancellation is never converted to fallback success");
Verification.True(result.Path == null, "cancelled lateral result has no path");
```

- [ ] **Step 2: Verify RED with the focused groups.**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- lateral-integration
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-integration
```

Expected: at least one optimizer currently returns `SuccessWithFallback`.

- [ ] **Step 3: Implement minimal cancellation precedence.** Special-case cancellation in each `FallbackOrFailure`, and check the token after LS, after ST, after assembly, and immediately before service success publication.

```csharp
if (failureStatus == EmPlanningStatus.Cancelled)
    return Failed(EmPlanningStatus.Cancelled, failureReason);
```

- [ ] **Step 4: Re-run both optimizer groups and `em-planning-service`; verify GREEN.**

### Task 3: Add shared linearized curvature hard constraints

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralCurvatureLinearization.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralObjectiveBuilder.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralConstraintBuilder.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralModelChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`

**Interfaces:**
- Produces: `LateralCurvatureLinearization.Create(input, layout, iterate)` returning immutable station affines with indices, gradient, and constant.
- Consumers: objective terms and hard constraints use the exact same affine coefficients.

- [ ] **Step 1: Write a failing QP-shape test.** For a straight reference and zero iterate, set vehicle maximum curvature to `0.25 1/m`; assert every station has a row equivalent to `-0.25 <= DDL(i) <= 0.25`. Also assert the total constraint count increases by the station count.

```csharp
Verification.Equal(expectedOldRows + layout.StationCount, problem.ConstraintCount,
    "one curvature hard-bound row is emitted per station");
Verification.True(HasBound(problem,
    new Dictionary<int, double> { { layout.DDL(station), 1d } }, -0.25d, 0.25d),
    "straight-path curvature affine is hard bounded");
```

- [ ] **Step 2: Run `lateral-model` and verify RED.**

- [ ] **Step 3: Extract the existing affine calculation without changing its formula.** Move `CreateCurvatureAffines` and its value type from `LateralObjectiveBuilder` to the new internal file. Keep the nonlinear formula in `LateralGeometryEvaluator`/independent validator unchanged.

- [ ] **Step 4: Add one curvature row per station in `LateralConstraintBuilder`.** Bounds are `[-maximumVehicleCurvature, +maximumVehicleCurvature]` after subtracting the affine constant.

```csharp
AddRow(constraints, lower, upper, ref row,
    -maximumCurvature - affine.Constant,
     maximumCurvature - affine.Constant,
    affine.Indices, affine.Gradient);
```

- [ ] **Step 5: Update test solver problem classification so the added lateral rows are not mistaken for ST rows, then run `lateral-model`, `lateral-integration`, and `em-planning-service` GREEN.**

### Task 4: Relinearize rejected solved vectors and report iteration exhaustion honestly

**Files:**
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LateralIntegrationChecks.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs`

**Interfaces:**
- Produces: rejected, parseable solver candidates may advance only the SQP iterate/warm start; they never replace `lastValidatedPath`.

- [ ] **Step 1: Write failing tests.** Configure a low curvature limit so the first candidate is strict and the second parseable candidate fails nonlinear validation. Assert the third QP/warm start is based on the second candidate, while a later timeout still returns the first strict path. Change the outer-limit assertion from ordinary `Success` to `SuccessWithFallback` with a non-empty reason.

- [ ] **Step 2: Run `lateral-integration` and verify RED.**

- [ ] **Step 3: Move iterate/warm-start advancement to immediately after a parseable solved candidate, while updating `lastValidatedPath` only after independent validation.** Return `SuccessWithFallback` when the outer loop ends with a strict candidate but without satisfying convergence.

- [ ] **Step 4: Run `lateral-integration` and `lateral-real-osqp` GREEN.**

### Task 5: Share one LS/ST solver timeout budget

**Files:**
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanner.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/SequentialConvexOptimizer.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanner.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`

**Interfaces:**
- Keeps: existing public `Plan(input, token)` and `Optimize(input, token)` entry points.
- Adds: internal overloads accepting a finite positive `TimeSpan solveBudget`.

- [ ] **Step 1: Write a failing service test.** Delay a lateral fake solve by at least 100 ms under a 2 s configured timeout, record all `QpSolverSettings.MaximumSolveDuration` values, and assert the first ST call receives less than 1.95 s rather than a fresh 2 s.

- [ ] **Step 2: Run `em-planning-service` and verify RED.**

- [ ] **Step 3: Add internal explicit-budget overloads.** Default public overloads continue deriving budget from configuration; service starts one monotonic `Stopwatch` immediately before LS and passes `configuredBudget - elapsed` to LS and then ST. Non-positive remaining time returns `SolverTimedOut` with no trajectory.

- [ ] **Step 4: Add a final elapsed/cancellation check before publication and run `lateral-integration`, `longitudinal-integration`, and `em-planning-service` GREEN.**

### Task 6: Preserve reference S separately from optimized PathS

**Files:**
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/LateralPathInterpolator.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs`

**Interfaces:**
- Extends internal `InterpolatedLateralPathPoint` with `ReferenceS`.
- Keeps public `EmTrajectoryPoint` shape unchanged.

- [ ] **Step 1: Write a failing curved/offset-path assembly test.** Construct a validated lateral path where reference-S and chord PathS differ; assert each output point's `SegmentLocalS` is the interpolated reference-S and `PathS` remains the ST progress value.

- [ ] **Step 2: Run `trajectory` and verify RED.** Current assembly writes `sample.PathS` into both fields.

- [ ] **Step 3: Interpolate `ReferenceS` using the same PathS bracket and pass `geometry.ReferenceS` as `SegmentLocalS`.** Update publication bounds so segment-local S is checked against direction-segment length/reference bound, while PathS is checked against the optimized path upper bound.

- [ ] **Step 4: Run `trajectory` and `em-core-all` GREEN.**

### Task 7: Core regression and diff hygiene

**Files:**
- Verify only; no broad formatting.

- [ ] **Step 1: Run the complete EM verification set.**

```powershell
dotnet build ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj --no-restore
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj --no-build -- em-all
```

- [ ] **Step 2: Check only scoped diffs.**

```powershell
git diff --check -- ClumsyPilot/ParkrobTrajplanner/EMPlanner ClumsyPilot/tests/EMPlannerVerificationHost
git status --short -- ClumsyPilot/ParkrobTrajplanner/EMPlanner ClumsyPilot/tests/EMPlannerVerificationHost
```

Expected: all groups pass; no whitespace errors; `EmPlannerConfiguration.cs` remains exactly the user's pre-existing modification.