# EM Observation Planning Diagnostics Implementation Plan

> **For agentic workers:** REQUIRED SUB-SKILL: Use executing-plans to implement this plan task-by-task. Steps use checkbox syntax for tracking.

**Goal:** Print one effective EM planning configuration snapshot at observation-session startup and make trajectory validation failures and successes numerically diagnosable without changing planning behavior.

**Architecture:** EmTrajectoryValidator enriches its existing jerk rejection reason with the values used in the decision. TrajectoryObservationDiagnostics owns pure invariant-culture formatters for configuration and successful trajectory metrics. The MovementTest runner prints the configuration formatter once after it creates the session-local controller.

**Tech Stack:** C#/.NET, existing EMPlannerVerificationHost, OSQP-backed EM planner.

## Global Constraints

- Keep the MovementTest OBSERVE_ONLY; do not add actuator calls.
- Do not change solver configuration, motion limits, QP formulation, horizon selection, or acceptance rules.
- Print configuration once per session; do not put it in the recurring UI diagnostic or every planning cycle.
- Format values with CultureInfo.InvariantCulture.
- Failed publications keep returning no EmTrajectory; their numeric evidence belongs in the failure reason.

## Preflight: Preserve manual MovementTest parameter tuning

`TrajectoryObservationMovementTest` exposes fields precisely so an operator can tune a test session. The verification host must not require those source literals to equal the defaults of `TrajectoryObservationSettings`; it must continue to verify that the fields exist, are copied into settings, and that `TrajectoryObservationSettings` itself retains its independently tested defaults.

- [ ] Run `dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation` and observe the current source-literal failure.
- [ ] In `TrajectoryObservationChecks.VerifiesMovementTestVehicleInputsAndSettingsSnapshot`, replace only the four source-text checks that require `0.50d`, `12000`, `6d`, and `0.10d` with checks for `public` fields named `SolverTimeoutSeconds`, `MaximumOsqpIterations`, `TimeHorizonSeconds`, and `OutputTimeStepSeconds`. Preserve the separate `new TrajectoryObservationSettings()` default assertions and all field-to-settings propagation checks.
- [ ] Re-run the same command and require exit code `0` before adding the Task 1 regression test.

---

### Task 1: Detail a jerk-limit rejection with computed evidence

**Files:**

- Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs:132-199
- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs:58-110

**Interfaces:**

- Consumes: the existing EmTrajectoryValidator.Validate(...) method.
- Produces: an unchanged JerkLimitExceeded result whose Message contains time, dt, previousAcceleration, acceleration, jerk, limit, excess, storedPreviousJerk, and storedCurrentJerk.

- [ ] **Step 1: Write the failing test**

In TrajectoryChecks.VerifiesWorldSpacePublicationMutationsAreRejected, retain the current jerkMutated trajectory and add:

    EmTrajectoryValidationResult jerkResult = new EmTrajectoryValidator().Validate(
        jerkMutated, context.EmptyMap, context.Vehicle, context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
    Verification.Equal(EmTrajectoryValidationFailure.JerkLimitExceeded, jerkResult.Failure,
        "jerk diagnostic failure code");
    Verification.Equal(2, jerkResult.PointIndex, "jerk diagnostic point index");
    foreach (string field in new[]
    {
        "time=", "dt=", "previousAcceleration=", "acceleration=", "jerk=", "limit=", "excess=",
        "storedPreviousJerk=", "storedCurrentJerk=",
    })
        Verification.True(jerkResult.Message.Contains(field), "jerk diagnostic includes " + field);

- [ ] **Step 2: Run the test to confirm RED**

Run:

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

Expected: the new assertion fails because the current message contains only Trajectory finite-difference jerk exceeds its limit.

- [ ] **Step 3: Implement the message enrichment**

Add using System.Globalization; to EmTrajectoryValidator.cs. Calculate finiteDifferenceJerk before the condition and preserve the existing inequality:

    double finiteDifferenceJerk = (acceleration - previousAcceleration) / dt;
    if (hasPreviousAcceleration && Math.Abs(finiteDifferenceJerk) > limits.MaximumJerk + limits.KinematicTolerance)
    {
        double storedPreviousJerk = index >= 2 ? trajectory.Points[index - 2].LongitudinalJerk : 0d;
        double storedCurrentJerk = previous.LongitudinalJerk;
        return Reject(EmTrajectoryValidationFailure.JerkLimitExceeded, index,
            "Trajectory finite-difference jerk exceeds its limit" +
            " (time=" + Format(point.TimeFromStart) + "s, dt=" + Format(dt) + "s" +
            ", previousAcceleration=" + Format(previousAcceleration) + "m/s2" +
            ", acceleration=" + Format(acceleration) + "m/s2" +
            ", jerk=" + Format(finiteDifferenceJerk) + "m/s3" +
            ", limit=" + Format(limits.MaximumJerk) + "m/s3" +
            ", excess=" + Format(Math.Abs(finiteDifferenceJerk) - limits.MaximumJerk) + "m/s3" +
            ", storedPreviousJerk=" + Format(storedPreviousJerk) + "m/s3" +
            ", storedCurrentJerk=" + Format(storedCurrentJerk) + "m/s3).");
    }

Add this private method to the same class:

    private static string Format(double value)
    {
        return value.ToString("G17", CultureInfo.InvariantCulture);
    }

storedPreviousJerk and storedCurrentJerk are evidence only and do not influence the validation decision.

- [ ] **Step 4: Confirm GREEN and commit**

Run:

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

Expected: host exit code 0.

Run:

    git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
    git commit -m "feat: detail trajectory jerk validation failures"

### Task 2: Format and print the effective session configuration once

**Files:**

- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs:17-39
- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs:188-241
- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs:269-305
- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs:18-39,96-191

**Interfaces:**

- Consumes: the effective EmPlannerConfiguration held privately by TrajectoryObservationController.
- Produces: TrajectoryObservationDiagnostics.CreateConfiguration(EmPlannerConfiguration) and TrajectoryObservationController.CreateConfigurationDiagnostic().
- Produces: one terminal planning configuration block before the observation loop begins.

- [ ] **Step 1: Write failing configuration tests**

Add VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration() to TrajectoryObservationChecks.Run(). Bootstrap a controller with:

    var settings = new TrajectoryObservationSettings
    {
        ReplanPeriodSeconds = 0.25d,
        SolverTimeoutSeconds = 1.25d,
        MaximumOsqpIterations = 54321,
        TimeHorizonSeconds = 3.5d,
        OutputTimeStepSeconds = 0.10d,
    };
    var controller = new TrajectoryObservationController(bootstrap, settings,
        new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "config-diagnostic");
    string text = controller.CreateConfigurationDiagnostic().Text;
    foreach (string expected in new[]
    {
        "planning configuration:", "timeHorizon=3.50s", "distanceHorizon=5.00m", "outputTimeStep=0.10s",
        "outputFrequency=10.00Hz", "trajectoryKnots=36", "maximumOsqpIterations=54321",
        "solverTimeout=1.25s", "replanPeriod=0.25s",
    })
        Verification.True(text.Contains(expected), "configuration diagnostic includes " + expected);

Source-read MovementTest.TrajectoryObservationTest.cs and assert the runner call is singular:

    int firstCall = runnerSource.IndexOf("CreateConfigurationDiagnostic()", StringComparison.Ordinal);
    Verification.True(firstCall >= 0, "runner prints configuration diagnostic");
    Verification.Equal(-1, runnerSource.IndexOf("CreateConfigurationDiagnostic()", firstCall + 1,
        StringComparison.Ordinal), "runner prints configuration diagnostic once");

- [ ] **Step 2: Run the test to confirm RED**

Run:

    dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation

Expected: compilation fails because CreateConfigurationDiagnostic does not exist.

- [ ] **Step 3: Implement pure formatting and the one runner call**

Add to TrajectoryObservationDiagnostics:

    public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration)
    {
        if (configuration == null || configuration.Scheduling == null || configuration.Solver == null)
            throw new ArgumentNullException(nameof(configuration));
        double horizon = configuration.Scheduling.TimeHorizonSeconds;
        double step = configuration.Scheduling.OutputTimeStepSeconds;
        int knots = checked((int)Math.Ceiling(horizon / step) + 1);
        return new TrajectoryObservationDiagnostic("planning configuration:\n" +
            "timeHorizon=" + Format(horizon, "F2") + "s\n" +
            "distanceHorizon=" + Format(configuration.Scheduling.DistanceHorizonMeters, "F2") + "m\n" +
            "outputTimeStep=" + Format(step, "F2") + "s\n" +
            "outputFrequency=" + Format(1d / step, "F2") + "Hz\n" +
            "trajectoryKnots=" + knots.ToString(CultureInfo.InvariantCulture) + "\n" +
            "maximumOsqpIterations=" + configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture) + "\n" +
            "solverTimeout=" + Format(configuration.Scheduling.SolverTimeoutSeconds, "F2") + "s\n" +
            "replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s");
    }

    private static string Format(double value, string format)
    {
        return value.ToString(format, CultureInfo.InvariantCulture);
    }

Add to TrajectoryObservationController:

    public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic()
    {
        return TrajectoryObservationDiagnostics.CreateConfiguration(configuration);
    }

Immediately after constructing controller in RunSessionAsync, before new TrajectoryObservationLoop(controller), add exactly:

    LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text);

Do not call this method from the while loop or the UI drawing methods.

- [ ] **Step 4: Confirm GREEN and commit**

Run:

    dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation

Expected: host exit code 0.

Run:

    git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
    git commit -m "feat: log EM observation session planning configuration"

### Task 3: Summarize each successfully published trajectory

**Files:**

- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs:17-39
- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs:538-561

**Interfaces:**

- Consumes: latestCycle.Result.Trajectory only when latestCycle.Published is true.
- Produces: a trajectory summary with ID, point count, duration, PathS length, max speed, max finite-difference acceleration, and max finite-difference jerk.

- [ ] **Step 1: Write the failing summary test**

Add VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary() to TrajectoryObservationChecks.Run():

    EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);
    var succeeded = new PlanningCycleResult(9L,
        new PlanningCycleIdentity(1L, "summary-reference", 2L, string.Empty, 0),
        new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty), true, string.Empty);
    string text = TrajectoryObservationDiagnostics.Create(
        succeeded, TimeSpan.FromMilliseconds(12d), false, trajectory).Text;
    foreach (string expected in new[]
    {
        "trajectory summary:", "trajectoryId=observer-published", "points=2", "duration=1.000s",
        "pathLength=1.000m", "maxSpeed=0.400m/s", "maxAcceleration=0.200m/s2", "maxJerk=0.000m/s3",
    })
        Verification.True(text.Contains(expected), "published diagnostic includes " + expected);

Also add this assertion to the existing failed-cycle test:

    Verification.True(!diagnostic.Text.Contains("trajectory summary:"), "failed diagnostic has no stale trajectory summary");

- [ ] **Step 2: Run the test to confirm RED**

Run:

    dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation

Expected: summary assertions fail because the formatter only writes trajectory=<id>.

- [ ] **Step 3: Implement summary formatting**

Replace the publishedTrajectory != null branch in TrajectoryObservationDiagnostics.Create(...) with a branch requiring latestCycle.Published && latestCycle.Result.Trajectory != null. Add CreateTrajectorySummary(EmTrajectory trajectory), which:

- reads immutable trajectory points;
- calculates maximum absolute signed speed;
- calculates acceleration and jerk by the same direction-aware finite-difference convention as EmTrajectoryValidator;
- ignores a nonpositive or non-finite dt only in logging;
- reports first and last PathS difference as actual pathLength;
- uses Format(value, "F3") and CultureInfo.InvariantCulture;
- returns exactly these two lines:

    trajectory summary:
    trajectoryId=<id>, points=<count>, duration=<last-time>s, pathLength=<last-pathS-first-pathS>m
    maxSpeed=<absolute-signed-speed>m/s, maxAcceleration=<absolute-finite-difference>m/s2, maxJerk=<absolute-finite-difference>m/s3

Do not call this helper for an unsuccessful cycle, even when an older published trajectory is still observed.

- [ ] **Step 4: Confirm GREEN, inspect, and commit**

Run:

    dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
    git diff --check

Expected: host exit code 0 and no whitespace errors.

Run:

    git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
    git commit -m "feat: summarize published EM observation trajectories"

## Final verification

- [ ] Run:

    dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
    dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
    git diff --check
    git log -3 --oneline

Expected: verification exits 0, no whitespace errors exist, and three focused diagnostics commits follow the already committed design document.