ParkingRobot/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs

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

namespace EMPlannerVerificationHost;

internal static class TrajectoryObservationChecks
{
    public static void Run()
    {
        VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
        RejectsObstacleOutsideConfiguredBounds();
        VerifiesLsAndStUsePublishedTrajectoryData();
    }

    private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding()
    {
        var settings = new TrajectoryObservationSettings
        {
            MapPaddingMeters = 2d,
            MapResolutionMillimeters = 50f,
        };
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(10d, -5d, 0d), new Pose2D(13d, -1d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 17L);

        Verification.NearlyEqual(8000d, job.MapRequest.Bounds.XMin, "observer map x min");
        Verification.NearlyEqual(15000d, job.MapRequest.Bounds.XMax, "observer map x max");
        Verification.NearlyEqual(-7000d, job.MapRequest.Bounds.YMin, "observer map y min");
        Verification.NearlyEqual(1000d, job.MapRequest.Bounds.YMax, "observer map y max");
        Verification.NearlyEqual(50d, job.MapRequest.ResolutionMm, "observer map resolution");
    }

    private static void RejectsObstacleOutsideConfiguredBounds()
    {
        Verification.True(Throws(() => TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 1d, 0d), new TrajectoryObservationSettings(),
            new[] { TrajectoryObservationObstacle.Rectangle(-2100d, -2000d, 0d, 100d) }, 1L)),
            "observer obstacle outside configured bounds");
    }

    private static void VerifiesLsAndStUsePublishedTrajectoryData()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
        DirectionSegmentView segment = CreateStraightSegment();
        EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);

        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(trajectory, segment, 0.5d);
        Verification.Equal(2, charts.StSamples.Count, "observer ST sample count");
        Verification.NearlyEqual(0d, charts.StSamples[0].TimeFromStart, "observer first ST time");
        Verification.NearlyEqual(4d, charts.StSamples[0].PathS, "observer first ST path S");
        Verification.NearlyEqual(1d, charts.StSamples[1].TimeFromStart, "observer second ST time");
        Verification.NearlyEqual(5d, charts.StSamples[1].PathS, "observer second ST path S");
        Verification.Equal(2, charts.SpeedSamples.Count, "observer speed sample count");
        Verification.NearlyEqual(0.20d, charts.SpeedSamples[0].SignedLongitudinalVelocity,
            "observer first signed speed");
        Verification.NearlyEqual(0.40d, charts.SpeedSamples[1].SignedLongitudinalVelocity,
            "observer second signed speed");
        Verification.Equal(2, charts.LsSamples.Count, "observer LS projection count");
        Verification.Equal(0, charts.FailedProjectionCount, "observer LS projection failure count");
        Verification.NearlyEqual(10.25d, charts.LsSamples[0].PathS, "observer first LS path S");
        Verification.NearlyEqual(0.10d, charts.LsSamples[0].LateralOffset, "observer first LS offset");

        var settings = new TrajectoryObservationSettings();
        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
            Array.Empty<TrajectoryObservationObstacle>(), 0L);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper(
            new CoarsePathPlanningService(), new PathSmoothingService())
            .Bootstrap(job, CancellationToken.None);
        Verification.True(bootstrap.Succeeded, "observer bootstrap succeeds");

        var planningService = new FixedTrajectoryPlanningService(trajectory);
        var controller = new TrajectoryObservationController(bootstrap, settings, planningService, "observer-check");
        var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 1L);
        PlanningCycleResult firstCycle = controller.StartCycle(effectiveAt, state, CancellationToken.None)
            .GetAwaiter().GetResult();
        Verification.True(firstCycle.Published, "observer first cycle publishes");
        Verification.Equal(0, planningService.Requests[0].SegmentIndex, "observer starts at segment zero");
        Verification.Equal("observer-check-trajectory-1", planningService.Requests[0].OutputTrajectoryId,
            "observer first trajectory identity");
        Verification.NearlyEqual(settings.ReplanPeriodSeconds,
            planningService.Requests[0].Configuration.Scheduling.ReplanPeriodSeconds,
            "observer configured replan period");

        TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
        Verification.NearlyEqual(0.5d, observation.SelectedPoint.TimeFromStart,
            "observer executor interpolates from published effective time");

        controller.StartCycle(effectiveAt.AddSeconds(settings.ReplanPeriodSeconds), state, CancellationToken.None)
            .GetAwaiter().GetResult();
        Verification.Equal(trajectory, planningService.Requests[1].PreviousTrajectory,
            "observer rolls published trajectory into next request");
        Verification.Equal(trajectory.Metadata.TrajectoryId, planningService.Requests[1].PreviousTrajectoryId,
            "observer rolls published trajectory identity into next request");
    }

    private static DirectionSegmentView CreateStraightSegment()
    {
        var points = new List<SmoothedPathPoint>
        {
            new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward,
                0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
            new SmoothedPathPoint(2d, 0d, 0d, 0d, 2d, TravelDirection.Forward,
                0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
        };
        return new DirectionSegmentView(0, TravelDirection.Forward, points,
            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 10d),
            new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 12d), 10d);
    }

    private static EmTrajectory CreatePublishedTrajectory(DateTimeOffset effectiveAt)
    {
        var metadata = new EmTrajectoryMetadata("observer-published", effectiveAt, effectiveAt, 1L,
            "observer-reference", 1L, string.Empty, 0, TravelDirection.Forward, EmTerminalType.Goal);
        return new EmTrajectory(metadata, new[]
        {
            new EmTrajectoryPoint(0.25d, 0.10d, 0d, 0.20d, 0d, 0d, 0, 0.25d, 4d,
                TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
            new EmTrajectoryPoint(1.25d, -0.20d, 0d, 0.40d, 1d, 0d, 0, 1.25d, 5d,
                TravelDirection.Forward, EmBoundaryType.None, 0d, 0d),
        });
    }

    private sealed class FixedTrajectoryPlanningService : IEmPlanningService
    {
        private readonly EmTrajectory trajectory;

        public FixedTrajectoryPlanningService(EmTrajectory trajectory)
        {
            this.trajectory = trajectory;
        }

        public List<EmPlanningRequest> Requests { get; } = new List<EmPlanningRequest>();

        public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
        {
            Requests.Add(request);
            return new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty);
        }
    }

    private static bool Throws(Action action)
    {
        try { action(); return false; }
        catch (ArgumentOutOfRangeException) { return true; }
    }
}