# EM PathS Monotonic Resampling Fix Implementation Plan

> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.

**Goal:** Normalize solver-sized `PathS` residuals during full-direction EM trajectory publication while rejecting material regressions.

**Architecture:** `EmTrajectoryAssembler` passes `Validation.KinematicTolerance` into `TrajectorySampleSchedule`. Each output sample is normalized into `[previousPathS, terminalPathS]` only when the required correction is within that tolerance; larger violations retain fail-closed behavior with numeric diagnostics.

**Tech Stack:** C#, .NET 10 verification host, netstandard2.0 plugin, `EMPlannerVerificationHost`.

## Global Constraints

- Do not modify `ClumsyPilot/Control`, `Shared`, `ClumsyPilot/StateEstimation`, `ClumsyPilot/Trajectory`, or controller Movement sources.
- Reuse `Validation.KinematicTolerance`; add no independent tolerance setting.
- Published `PathS` must be nondecreasing and no greater than terminal `PathS`.
- Violations greater than tolerance must still be rejected.

---

### Task 1: Add PathS resampling regression coverage

**Files:**
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs`

**Interfaces:**
- Consumes: `EmTrajectoryAssembler.Assemble(...)` with `EmPlanningScope.FullDirectionSegment`.
- Produces: regression checks invoked by `TrajectoryChecks.Run()`.

- [ ] **Step 1: Write the failing tolerance-sized regression test**

Add `VerifiesFullScopeNormalizesToleranceSizedPathSResiduals()` to `Run()`. Construct four knots at `0.0, 0.1, 0.2, 0.3` seconds with `S = 0.0, 0.05, 0.05 - 0.5 * KinematicTolerance, 0.10`, use `RollingContinuation` plus `FullDirectionSegment`, assemble it, and assert every published `PathS` is at least its predecessor and no greater than `candidate.S[^1]`.

Use this metadata helper signature so existing calls remain valid:

```csharp
private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType,
    EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary,
    EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
{
    return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L,
        "reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode, planningScope);
}
```

- [ ] **Step 2: Write the material-regression protection test**

Add `VerifiesFullScopeRejectsMaterialPathSRegressionWithDiagnostics()`. Use the same time knots with `S = 0.0, 0.05, 0.04, 0.10`; assert assembly throws `ArgumentException` and its message contains `sampleIndex=2`, `previousPathS=`, `candidatePathS=`, and `difference=`.

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

Run:

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
```

Expected: FAIL with `Trajectory PathS cannot decrease. (Parameter 'candidate')` in the tolerance-sized regression test.

- [ ] **Step 4: Commit the red tests**

```powershell
git add -- ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
git commit -m "test: reproduce EM PathS resampling regression"
```

### Task 2: Implement bounded monotonic normalization

**Files:**
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs`

**Interfaces:**
- Consumes: `EmPlannerConfiguration.Validation.KinematicTolerance`.
- Produces: `TrajectorySampleSchedule(..., bool resampleMotion, double pathSTolerance)`.

- [ ] **Step 1: Pass the configured tolerance into the schedule**

In `EmTrajectoryAssembler`, store and validate:

```csharp
private readonly double pathSTolerance;

pathSTolerance = configuration.Validation.KinematicTolerance;
if (!IsFinite(pathSTolerance) || pathSTolerance < 0d)
    throw new ArgumentOutOfRangeException(nameof(configuration));
```

Construct the schedule with:

```csharp
var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds,
    holdDurationSeconds, metadata.LongitudinalMode, isFullDirectionSegment, pathSTolerance);
```

- [ ] **Step 2: Normalize only tolerance-sized deviations**

Add `using System.Globalization;`. Validate `pathSTolerance`, set `terminalPathS = candidate.S[candidate.S.Count - 1]`, and pass `samples.Count` into each `AddSample` call. Implement the core behavior:

```csharp
double lowerDifference = previousPathS - sample.PathS;
double upperDifference = sample.PathS - terminalPathS;
if (lowerDifference > pathSTolerance || upperDifference > pathSTolerance)
    throw PathSFailure(sampleIndex, previousPathS, sample.PathS,
        Math.Max(lowerDifference, upperDifference), candidate);

double normalizedPathS = Math.Min(terminalPathS, Math.Max(previousPathS, sample.PathS));
if (normalizedPathS != sample.PathS)
    sample = new TrajectorySample(sample.TimeFromStart, normalizedPathS, sample.ProgressSpeed,
        sample.Acceleration, sample.Jerk, sample.IsHoldSample);
```

Retain the existing negative progress-speed check. Create diagnostics with invariant round-trip formatting:

```csharp
private static ArgumentException PathSFailure(int sampleIndex, double previousPathS,
    double candidatePathS, double difference, LongitudinalCandidate candidate)
{
    return new ArgumentException("Trajectory PathS violates monotonic publication bounds: sampleIndex=" +
        sampleIndex + ";previousPathS=" + previousPathS.ToString("R", CultureInfo.InvariantCulture) +
        ";candidatePathS=" + candidatePathS.ToString("R", CultureInfo.InvariantCulture) +
        ";difference=" + difference.ToString("R", CultureInfo.InvariantCulture) + ".", nameof(candidate));
}
```

- [ ] **Step 3: Run focused tests and verify GREEN**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
```

Expected: `PASS trajectory`.

- [ ] **Step 4: Commit the minimal production fix**

```powershell
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs
git commit -m "fix: normalize EM PathS resampling residuals"
```

### Task 3: Run integration regression and scoped verification

**Files:**
- Verify only; no production edits expected.

**Interfaces:**
- Consumes: the normalized `EmTrajectory` publication behavior.
- Produces: evidence that planning, observation, adapter, and plugin compilation remain valid.

- [ ] **Step 1: Run EM integration checks**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-planning-service
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-control-adapter
```

Expected: all three commands print `PASS`.

- [ ] **Step 2: Build and inspect only the scoped files**

```powershell
dotnet build ClumsyPilot/ClumsyPilot.csproj -c Release --no-restore
git diff --check -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
```

Expected: zero build errors; only the two known obsolete-API warnings may remain; diff check emits no errors.