# EM Planner Rolling Execution and Plugin Deployment 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:** Add version-safe rolling replanning, trajectory handoff, gear-switch execution states, controller adaptation, and deterministic packaging of `ClumsyPilot.dll` with `osqp.dll` and licenses.

**Architecture:** Keep `EmPlanningService` pure and place scheduling, cancellation, stale-result suppression, previous-trajectory reuse, and command generation in a sibling `TrajectoryExecution` module. The coordinator publishes only a fully validated current-version trajectory; the executor samples that immutable trajectory, manages zero-speed gear transitions, and converts its fields into a generic controller command without coupling EM optimization to the hardware API.

**Tech Stack:** C# 10, .NET Standard 2.0, `Task`/`CancellationToken`, immutable EM trajectories, PowerShell packaging, Windows x64 plugin layout.

## Global Constraints

- This plan depends on all four preceding implementation plans.
- Default replan period is `0.20 s`; time horizon `6.0 s`; distance horizon `5.0 m`; handoff lookahead `0.30 s`.
- The coordinator accepts captured `VehicleMotionState`; it does not read localization, wheel speed, UI, or hardware directly.
- Each cycle binds `MapSnapshotId`, `ReferencePathId`, `VehicleState.SequenceId`, `PreviousTrajectoryId`, and `SegmentIndex`.
- A result is publishable only if all bound identities still match the latest cycle and the cycle version is current.
- Normal replans hand off from the previous trajectory only when tracking error and age are within configuration limits and no gear boundary is crossed.
- If a replan fails, the previously published trajectory remains executable and ends in its own zero-speed safety tail.
- Gear change occurs only after measured speed remains below `0.01 m/s` for at least `0.20 s`.
- The executor never commands lateral body velocity or in-place rotation.
- The generic control command uses signed longitudinal velocity and yaw rate; world `vx/vy`, speed, curvature, and pose remain available for monitoring.
- No dynamic-obstacle prediction or dynamic behavior state is introduced.
- Plugin runtime layout is `plugins/ClumsyPilot.dll`, `plugins/osqp.dll`, and `plugins/licenses/*`.
- Deployment refuses a non-x64 host/runtime package or an OSQP hash mismatch.

---

## Locked File Structure

```text
ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/
├── EmPlanningCoordinator.cs
├── GearSwitchStateMachine.cs
├── GearSwitchState.cs
├── IEmPlanningCycleSink.cs
├── IVehicleStateProvider.cs
├── PlanningCycleIdentity.cs
├── PlanningCycleInput.cs
├── PlanningCycleResult.cs
├── TrajectoryControlAdapter.cs
├── TrajectoryControlCommand.cs
├── TrajectoryExecutionState.cs
├── TrajectoryExecutor.cs
├── TrajectoryHandoffSelector.cs
└── TrajectorySampler.cs

ClumsyPilot/scripts/
└── Publish-ClumsyPilotPlugin.ps1

ClumsyPilot/tests/EMPlannerVerificationHost/
├── CoordinatorChecks.cs
├── ExecutorChecks.cs
└── PluginPackagingChecks.cs
```

## Shared Interfaces

```csharp
public interface IVehicleStateProvider
{
    VehicleMotionState Capture();
}

public sealed class EmPlanningCoordinator
{
    public EmPlanningCoordinator(IEmPlanningService planningService,
        IEmPlanningCycleSink sink = null);
    public Task<PlanningCycleResult> PlanLatestAsync(PlanningCycleInput input,
        CancellationToken cancellationToken);
    public EmTrajectory PublishedTrajectory { get; }
}

public sealed class TrajectoryExecutor
{
    public TrajectoryExecutionState State { get; }
    public TrajectoryControlCommand Update(DateTimeOffset now,
        VehicleMotionState measuredState, EmTrajectory trajectory);
}

public sealed class TrajectoryControlCommand
{
    public double SignedLongitudinalVelocity { get; }
    public double YawRate { get; }
    public TravelDirection Direction { get; }
    public bool RequestDirectionChange { get; }
    public bool HoldBrake { get; }
    public bool IsTrajectoryComplete { get; }
}
```

### Task 1: Cycle Identity, Scheduling Decision, and Stale-Result Suppression

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleIdentity.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleInput.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleResult.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IEmPlanningCycleSink.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`

**Interfaces:**
- Consumes: pure `IEmPlanningService`, captured request inputs, and configuration scheduling values.
- Produces: latest-wins asynchronous planning and a read-only published trajectory.

- [ ] **Step 1: Write failing coordinator concurrency checks**

Use a controllable fake planning service to start cycle A, then cycle B before A completes. Complete B first with success and A later with success. Assert B alone is published and A returns `Superseded`. Add checks for map ID, reference ID, state sequence, segment, and prior trajectory ID changes invalidating an otherwise successful result.

Also assert `ShouldStartCycle(now)` is false before `0.20 s` and true at exactly `0.20 s`; this decision uses caller-supplied time.

- [ ] **Step 2: Run and verify coordinator types are absent**

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

Expected: build failure naming `EmPlanningCoordinator`.

- [ ] **Step 3: Implement latest-wins coordination**

Use an incrementing `long` cycle version, a private lock only around publication state, and a per-cycle linked cancellation source. Starting a newer cycle cancels the prior source. After planning, compare the complete identity and version again under the publication lock; map stale success to `Superseded` without exposing its trajectory.

Sink exceptions are caught and reported in the cycle diagnostic. Never hold the publication lock while running the planner or invoking a sink.

- [ ] **Step 4: Run concurrency checks 100 times**

```powershell
1..100 | ForEach-Object {
  dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
  if ($LASTEXITCODE -ne 0) { throw "Coordinator verification failed on iteration $_" }
}
```

Expected: every iteration prints `PASS coordinator`.

- [ ] **Step 5: Commit coordinator identity logic**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost
git commit -m "feat: coordinate rolling EM replans"
```

### Task 2: Safe Previous-Trajectory Handoff

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs`

**Interfaces:**
- Consumes: current published trajectory, measured state, latest segment identity, and `0.30 s` handoff lookahead.
- Produces: either a future trajectory-derived start state/seed or a measured-state start with no seed.

- [ ] **Step 1: Write failing handoff acceptance/rejection checks**

Assert acceptance only when trajectory age is valid, position/yaw/speed tracking errors are inside configured tolerances, the future sample remains on the same segment/direction, and the interval contains no gear boundary. Assert rejection for stale trajectory, large error, terminal proximity, segment mismatch, direction mismatch, and a handoff time beyond the trajectory.

- [ ] **Step 2: Run and verify selector is absent**

Expected: build failure naming `TrajectoryHandoffSelector`.

- [ ] **Step 3: Implement time interpolation and selection**

Binary-search `TimeFromStart`, interpolate X/Y, unwrapped yaw, signed speed, curvature, and PathS, then derive redundant fields through the trajectory-point constructor. Never interpolate across different boundary types, segment indices, or directions. Return a result object that states `PreviousTrajectory` or `MeasuredState` and includes a deterministic rejection reason.

- [ ] **Step 4: Run handoff checks**

Expected: `PASS coordinator`, including a reverse same-segment handoff.

- [ ] **Step 5: Commit handoff logic**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs
git commit -m "feat: select safe EM trajectory handoffs"
```

### Task 3: Gear-Switch State Machine and Trajectory Executor

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`

**Interfaces:**
- Consumes: measured state, current time, sampled trajectory, and exact boundary types.
- Produces: zero-speed holding, one-shot direction-change request, confirmed departure, and completion states.

- [ ] **Step 1: Write failing state-transition checks**

Cover this exact sequence:

```text
Following -> ApproachingGearSwitch
ApproachingGearSwitch -> HoldingZero when command reaches boundary
HoldingZero remains while |measured speed| >= 0.01 m/s
HoldingZero timer resets if speed rises above tolerance
HoldingZero -> RequestingDirectionChange after continuous 0.20 s below tolerance
RequestingDirectionChange emits exactly one request
AwaitingDirectionConfirmation holds zero
confirmed direction -> Following next segment
goal/rolling terminal -> Completed while holding zero
```

Assert no transition can output nonzero signed speed during holding or direction confirmation.

- [ ] **Step 2: Run and verify executor types are absent**

Expected: build failure naming `GearSwitchStateMachine`.

- [ ] **Step 3: Implement explicit state and event inputs**

The state machine receives caller-supplied `now`, measured signed speed, desired/current directions, and a boolean direction-confirmation input. It does not call hardware. `TrajectoryExecutor` samples the trajectory, delegates boundary behavior to the state machine, and returns an immutable execution state containing the selected point and reason.

- [ ] **Step 4: Run executor state checks**

Expected: `PASS executor`, including forward-to-reverse and reverse-to-forward sequences.

- [ ] **Step 5: Commit executor state machine**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs
git commit -m "feat: execute EM gear-switch boundaries"
```

### Task 4: Generic Control Adapter

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlCommand.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlAdapter.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IVehicleStateProvider.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs`

**Interfaces:**
- Consumes: an `EmTrajectoryPoint` and gear-switch execution state.
- Produces: signed linear velocity, yaw rate, direction-change request, brake hold, and completion flags.

- [ ] **Step 1: Write failing command-mapping checks**

For forward and reverse points assert command signed velocity and yaw rate equal the point fields exactly. Assert `Speed`, world `vx/vy`, pose, and curvature remain available in execution telemetry but are not reinterpreted as body lateral velocity. Assert holding states always override both command velocities to zero.

- [ ] **Step 2: Run and verify adapter is absent**

Expected: build failure naming `TrajectoryControlAdapter`.

- [ ] **Step 3: Implement adapter without hardware coupling**

`TrajectoryControlAdapter.CreateCommand` copies signed longitudinal velocity and yaw rate in normal following. For hold, switch, invalid, or completed states it sets both to zero and sets the corresponding flags. Do not reference `MultiVehicleScriptVx`, `MultiVehicleScriptVy`, or `MultiVehicleScriptVth`; a later hardware-specific adapter may map this generic command after the existing controller field semantics are confirmed.

- [ ] **Step 4: Run adapter and executor checks**

Expected: `PASS executor`; add an invariant that ordinary following never requests nonzero body lateral velocity or `v=0, yawRate!=0`.

- [ ] **Step 5: Commit control adaptation**

```powershell
git add ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs
git commit -m "feat: adapt EM trajectories to control commands"
```

### Task 5: Plugin Output and License Packaging

**Files:**
- Modify: `ClumsyPilot/ClumsyPilot.csproj`
- Create: `ClumsyPilot/scripts/Publish-ClumsyPilotPlugin.ps1`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/PluginPackagingChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`

**Interfaces:**
- Consumes: a successfully built `ClumsyPilot.dll` plus the pinned OSQP package.
- Produces: the exact deployable `plugins` tree.

- [ ] **Step 1: Write failing packaging checks**

Create a temporary output directory, invoke the future script, and assert exactly:

```text
plugins/ClumsyPilot.dll
plugins/osqp.dll
plugins/licenses/OSQP-LICENSE.txt
plugins/licenses/OSQP-NOTICE.txt
plugins/licenses/OSQP-VERSION.txt
```

Assert the deployed native hash equals `SHA256SUMS`, `ClumsyPilot.dll` is a managed assembly, OSQP is x64, and rerunning packaging replaces files without leaving stale temporary files.

- [ ] **Step 2: Run and verify packaging script is absent**

Expected: `PluginPackagingChecks` fails because `Publish-ClumsyPilotPlugin.ps1` does not exist.

- [ ] **Step 3: Add build-output metadata and transactional publish script**

Add these items to `ClumsyPilot.csproj` without rewriting existing targets:

```xml
<None Include="ThirdParty\OSQP\win-x64\osqp.dll"
      Link="osqp.dll" CopyToOutputDirectory="PreserveNewest" />
<None Include="ThirdParty\OSQP\LICENSE"
      Link="licenses\OSQP-LICENSE.txt" CopyToOutputDirectory="PreserveNewest" />
<None Include="ThirdParty\OSQP\NOTICE"
      Link="licenses\OSQP-NOTICE.txt" CopyToOutputDirectory="PreserveNewest" />
<None Include="ThirdParty\OSQP\VERSION"
      Link="licenses\OSQP-VERSION.txt" CopyToOutputDirectory="PreserveNewest" />
```

The publish script takes mandatory `-ManagedDll` and `-OutputDirectory`, resolves both absolute paths, validates inputs and hash, stages the five files in a unique sibling temporary directory, then renames the completed `plugins` directory into place. It refuses to operate when the resolved output is a drive root or workspace root.

- [ ] **Step 4: Run packaging checks against a fresh directory**

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

Expected: `PASS plugin-package`; no files exist outside the temporary test root.

- [ ] **Step 5: Commit packaging**

```powershell
git add ClumsyPilot/ClumsyPilot.csproj ClumsyPilot/scripts/Publish-ClumsyPilotPlugin.ps1 ClumsyPilot/tests/EMPlannerVerificationHost
git commit -m "build: package ClumsyPilot with OSQP"
```

### Task 6: Rolling End-to-End and Final Gate

**Files:**
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`

**Interfaces:**
- Consumes: all core, coordinator, executor, and packaging components.
- Produces: a complete first-version EM Planner workflow with safe rolling fallback.

- [ ] **Step 1: Add deterministic rolling scenarios**

Simulate caller-supplied time and measured states for: normal repeated forward replans, reverse replans, a solver failure with old-trajectory continuation, a superseded slow cycle, tracking-error reset to measured state, forward/reverse gear switch, and final goal stop. Assert every executed command comes from a currently published validated trajectory or is a zero hold.

- [ ] **Step 2: Add failure-tail assertions**

Force all new plans to fail after a successful publication. Advance time through the old trajectory and assert it reaches its exact zero-speed terminal and stays zero; no extrapolated nonzero command is allowed after its final point.

- [ ] **Step 3: Run the full first-version gate**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-all
git diff --check
```

Expected groups include foundation, OSQP, lateral, longitudinal, trajectory, facade, coordinator, executor, and plugin packaging, all with PASS output.

- [ ] **Step 4: Update README with ownership and deployment**

Document the pure-planner/coordinator/executor boundary, caller responsibilities, update cadence, handoff rules, gear state sequence, all trajectory fields, generic controller command, OSQP files, packaging command, and explicitly deferred dynamic-obstacle scope.

- [ ] **Step 5: Commit rolling integration evidence**

```powershell
git add ClumsyPilot/tests/EMPlannerVerificationHost ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md
git commit -m "test: verify rolling EM execution"
```

## Completion Gate

- One slow cycle can never overwrite a newer published trajectory.
- All handoffs remain within one segment/direction and use measured state when tracking is unsafe.
- Failed replans leave a complete prior trajectory that terminates safely at zero.
- Gear changes require measured zero-speed dwell and emit one explicit request.
- Generic control output never invents crab or in-place-rotation behavior.
- Packaging produces the exact DLL/license tree and validates the pinned native hash.
