ParkingRobot/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSegmentChecks.cs

using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

namespace EMPlannerVerificationHost;

internal static class TrajectoryObservationSegmentChecks
{
    public static void Run()
    {
        AdvancesOnlyAfterContinuousStopAndStableNextDirection();
        RejectsNonTerminalOrMismatchedGearTrajectory();
        ResetsConfirmationForInvalidDirectionEvidence();
        CompletesOneSegmentWithoutIndexingPastTheEnd();
    }

    private static void AdvancesOnlyAfterContinuousStopAndStableNextDirection()
    {
        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 0, 0, 0, TimeSpan.Zero);
        var tracker = CreateTracker();
        EmTrajectory trajectory = GearTerminal(t0);

        tracker.Update(t0, StateAtSwitch(0d, t0, 1L), trajectory);
        Verification.Equal(TrajectoryObservationSegmentPhase.WaitingForStop, tracker.State.Phase,
            "first zero sample begins stop hold");
        tracker.Update(t0.AddSeconds(0.21d), StateAtSwitch(0d, t0.AddSeconds(0.21d), 2L), trajectory);
        Verification.Equal(TrajectoryObservationSegmentPhase.WaitingForDirection, tracker.State.Phase,
            "continuous stop arms next direction");
        tracker.Update(t0.AddSeconds(0.25d), StateAtSwitch(-0.03d, t0.AddSeconds(0.25d), 3L), trajectory);
        tracker.Update(t0.AddSeconds(0.30d), StateAtSwitch(-0.03d, t0.AddSeconds(0.30d), 4L), trajectory);
        TrajectoryObservationSegmentUpdate advanced = tracker.Update(
            t0.AddSeconds(0.35d), StateAtSwitch(-0.03d, t0.AddSeconds(0.35d), 5L), trajectory);

        Verification.True(advanced.Advanced, "three stable reverse samples advance exactly one segment");
        Verification.Equal(1, tracker.State.ActiveSegmentIndex, "tracker advances only from N to N+1");
        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, tracker.State.Phase,
            "advanced tracker resumes planning on next segment");
    }

    private static void RejectsNonTerminalOrMismatchedGearTrajectory()
    {
        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 1, 0, 0, TimeSpan.Zero);
        var tracker = CreateTracker();

        tracker.Update(t0, StateAtSwitch(0d, t0, 1L), GearTerminal(t0, EmTerminalType.Goal));
        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, tracker.State.Phase,
            "non-gear terminal cannot begin confirmation");

        tracker.Update(t0.AddSeconds(0.01d), StateAtSwitch(0d, t0.AddSeconds(0.01d), 2L),
            GearTerminal(t0, EmTerminalType.GearSwitch, TravelDirection.Reverse));
        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, tracker.State.Phase,
            "wrong trajectory direction cannot begin confirmation");

        tracker.Update(t0.AddSeconds(0.02d), StateAtSwitch(0d, t0.AddSeconds(0.02d), 3L),
            GearTerminal(t0, EmTerminalType.GearSwitch, TravelDirection.Forward, 0.10d));
        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, tracker.State.Phase,
            "trajectory before its absolute terminal cannot begin confirmation");
    }

    private static void ResetsConfirmationForInvalidDirectionEvidence()
    {
        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 2, 0, 0, TimeSpan.Zero);
        AssertConfirmationReset(t0, StateAtSwitch(0.03d, t0.AddSeconds(0.25d), 3L), GearTerminal(t0),
            "wrong signed direction resets confirmation");
        AssertConfirmationReset(t0, StateAtSwitch(0d, t0.AddSeconds(0.25d), 3L), GearTerminal(t0),
            "zero speed resets confirmation");
        AssertConfirmationReset(t0, StateAt(2d, 0d, -0.03d, t0.AddSeconds(0.25d), 3L), GearTerminal(t0),
            "excessive switch projection distance resets confirmation");
        AssertConfirmationReset(t0, StateAtSwitch(-0.03d, t0.AddSeconds(0.25d), 3L),
            GearTerminal(t0, EmTerminalType.Goal), "non-gear trajectory resets confirmation");

        var repeated = CreateTracker();
        ArmWaitingForDirection(repeated, t0, GearTerminal(t0));
        repeated.Update(t0.AddSeconds(0.25d), StateAtSwitch(-0.03d, t0.AddSeconds(0.25d), 2L), GearTerminal(t0));
        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, repeated.State.Phase,
            "repeated sequence resets confirmation");

        var discontinuous = CreateTracker();
        ArmWaitingForDirection(discontinuous, t0, GearTerminal(t0));
        discontinuous.Update(t0.AddSeconds(0.20d), StateAtSwitch(-0.03d, t0.AddSeconds(0.20d), 3L), GearTerminal(t0));
        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, discontinuous.State.Phase,
            "time discontinuity resets confirmation");
    }

    private static void CompletesOneSegmentWithoutIndexingPastTheEnd()
    {
        DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 3, 0, 0, TimeSpan.Zero);
        var tracker = new TrajectoryObservationSegmentTracker(new[] { CreateSegments()[0] },
            CreateSettings(), 0.01d);

        TrajectoryObservationSegmentUpdate completed = tracker.Update(
            t0, StateAtSwitch(0d, t0, 1L), GearTerminal(t0));

        Verification.True(completed.Completed, "single segment update reports completion");
        Verification.Equal(TrajectoryObservationSegmentPhase.Completed, tracker.State.Phase,
            "single segment reaches completed instead of indexing past end");
        Verification.Equal(0, tracker.State.ActiveSegmentIndex, "completed tracker retains final segment index");
    }

    private static void AssertConfirmationReset(DateTimeOffset t0, VehicleMotionState invalidState,
        EmTrajectory invalidTrajectory, string name)
    {
        var tracker = CreateTracker();
        ArmWaitingForDirection(tracker, t0, GearTerminal(t0));
        tracker.Update(invalidState.CapturedAtUtc, invalidState, invalidTrajectory);
        Verification.Equal(TrajectoryObservationSegmentPhase.Planning, tracker.State.Phase, name);
    }

    private static void ArmWaitingForDirection(TrajectoryObservationSegmentTracker tracker, DateTimeOffset t0,
        EmTrajectory trajectory)
    {
        tracker.Update(t0, StateAtSwitch(0d, t0, 1L), trajectory);
        tracker.Update(t0.AddSeconds(0.21d), StateAtSwitch(0d, t0.AddSeconds(0.21d), 2L), trajectory);
        Verification.Equal(TrajectoryObservationSegmentPhase.WaitingForDirection, tracker.State.Phase,
            "fixture arms direction confirmation");
    }

    private static TrajectoryObservationSegmentTracker CreateTracker()
    {
        return new TrajectoryObservationSegmentTracker(CreateSegments(), CreateSettings(), 0.01d);
    }

    private static TrajectoryObservationSettings CreateSettings()
    {
        return new TrajectoryObservationSettings
        {
            DirectionConfirmationSpeedMetersPerSecond = 0.02d,
            DirectionConfirmationSamples = 3,
            GearSwitchProjectionToleranceMeters = 0.50d,
            GearSwitchStopHoldSeconds = 0.20d,
        }.CreateValidatedSnapshot();
    }

    private static IReadOnlyList<DirectionSegmentView> CreateSegments()
    {
        return new[]
        {
            CreateSegment(0, TravelDirection.Forward, 0d, 1d, false, 0d,
                EmBoundaryType.None, EmBoundaryType.GearSwitchApproach),
            CreateSegment(1, TravelDirection.Reverse, 1d, 0d, true, 1d,
                EmBoundaryType.GearSwitchDeparture, EmBoundaryType.Goal),
        };
    }

    private static DirectionSegmentView CreateSegment(int index, TravelDirection direction, double startX,
        double endX, bool startIsGearSwitch, double sourceStartS, EmBoundaryType startBoundary,
        EmBoundaryType endBoundary)
    {
        var points = new List<SmoothedPathPoint>
        {
            new SmoothedPathPoint(startX, 0d, 0d, 0d, 0d, direction, 0d, 0d, 1d,
                startIsGearSwitch, SmoothedPathPointSource.Anchor),
            new SmoothedPathPoint(endX, 0d, 0d, 0d, 1d, direction, 0d, 0d, 1d,
                false, SmoothedPathPointSource.Anchor),
        };
        return new DirectionSegmentView(index, direction, points,
            new ReferenceBoundary(index, 0d, startBoundary, sourceStartS),
            new ReferenceBoundary(index, 1d, endBoundary, sourceStartS + 1d), sourceStartS);
    }

    private static VehicleMotionState StateAtSwitch(double speed, DateTimeOffset time, long sequenceId)
    {
        return StateAt(1d, 0d, speed, time, sequenceId);
    }

    private static VehicleMotionState StateAt(double x, double y, double speed, DateTimeOffset time, long sequenceId)
    {
        return new VehicleMotionState(new Pose2D(x, y, 0d), speed, null, time, sequenceId);
    }

    private static EmTrajectory GearTerminal(DateTimeOffset effectiveAtUtc,
        EmTerminalType terminalType = EmTerminalType.GearSwitch, TravelDirection direction = TravelDirection.Forward,
        double finalTimeFromStart = 0d)
    {
        var metadata = new EmTrajectoryMetadata("gear-terminal-" + effectiveAtUtc.Ticks, effectiveAtUtc,
            effectiveAtUtc, 1L, "segment-check", 1L, string.Empty, 0, direction, terminalType,
            EmLongitudinalMode.ExactStopAtBoundary);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(1d, 0d, 0d, 0d, finalTimeFromStart, 0d, 0, 1d, 1d,
                direction, EmBoundaryType.GearSwitchApproach, 0d, 0d),
        });
    }
}