ParkingRobot/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs

using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class FoundationChecks
{
    public static void Run()
    {
        VerifyContracts();
        VerifyConfigurationAndRequestValidation();
    }

    private static void VerifyContracts()
    {
        var reverseState = new VehicleMotionState(
            new Pose2D(1.5d, -2d, 0.25d),
            -0.15d,
            -0.03d,
            new DateTimeOffset(2026, 8, 3, 0, 0, 0, TimeSpan.Zero),
            17L);

        EMPlannerVerificationHost.Verification.NearlyEqual(-0.15d, reverseState.SignedLongitudinalSpeedMetersPerSecond,
            "reverse signed speed");

        var point = new EmTrajectoryPoint(
            1.25d,
            -0.75d,
            0.5d,
            -0.12d,
            0.4d,
            -0.2d,
            3,
            0.6d,
            0.8d,
            TravelDirection.Reverse,
            EmBoundaryType.GearSwitchApproach,
            -0.04d,
            0.03d);

        EMPlannerVerificationHost.Verification.NearlyEqual(1.25d, point.X, "point x");
        EMPlannerVerificationHost.Verification.NearlyEqual(-0.75d, point.Y, "point y");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, point.Yaw, "point yaw");
        EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d, point.SignedLongitudinalVelocity, "point signed speed");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.4d, point.TimeFromStart, "point time");
        EMPlannerVerificationHost.Verification.NearlyEqual(-0.2d, point.VehicleCurvature, "point curvature");
        EMPlannerVerificationHost.Verification.Equal(3, point.SegmentIndex, "point segment index");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.6d, point.SegmentLocalS, "point segment local s");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.8d, point.PathS, "point path s");
        EMPlannerVerificationHost.Verification.Equal(TravelDirection.Reverse, point.Direction, "point direction");
        EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach, point.BoundaryType, "point boundary type");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.12d, point.Speed, "point derived speed");
        EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Cos(0.5d), point.VelocityX, "point derived velocity x");
        EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Sin(0.5d), point.VelocityY, "point derived velocity y");
        EMPlannerVerificationHost.Verification.NearlyEqual((-0.12d) * (-0.2d), point.YawRate, "point derived yaw rate");
    }

    private static void VerifyConfigurationAndRequestValidation()
    {
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Scheduling.ReplanPeriodSeconds, "replan period");
        EMPlannerVerificationHost.Verification.NearlyEqual(6d, configuration.Scheduling.TimeHorizonSeconds, "time horizon");
        EMPlannerVerificationHost.Verification.NearlyEqual(5d, configuration.Scheduling.DistanceHorizonMeters, "distance horizon");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.05d, configuration.Scheduling.OutputTimeStepSeconds, "output time step");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, configuration.Scheduling.SolverTimeoutSeconds, "solver timeout");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Scheduling.HandoffLookaheadSeconds, "handoff lookahead");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Scheduling.MaximumVehicleStateAgeSeconds, "vehicle state age");

        EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, configuration.Corridor.LongitudinalSampleSpacingMeters, "longitudinal sample spacing");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.025d, configuration.Corridor.LateralSampleSpacingMeters, "lateral sample spacing");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Corridor.MaximumLateralOffsetMeters, "maximum lateral offset");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.02d, configuration.Corridor.AdditionalClearanceReserveMeters, "clearance reserve");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.025d, configuration.Corridor.MaximumCollisionCheckStepMeters, "collision check step");

        EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Frenet.MaximumProjectionDistanceMeters, "maximum projection distance");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Frenet.MinimumFrenetDenominator, "minimum Frenet denominator");
        EMPlannerVerificationHost.Verification.NearlyEqual(1e-8d, configuration.Frenet.BoundaryAnchorToleranceMeters, "boundary anchor tolerance");

        EMPlannerVerificationHost.Verification.NearlyEqual(0.05d, configuration.Lateral.MaximumLateralStepPerIterationMeters, "lateral trust region");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Lateral.MaximumLateralSlope, "maximum lateral slope");
        EMPlannerVerificationHost.Verification.NearlyEqual(1d, configuration.Lateral.MaximumLateralSecondDerivativePerMeter, "maximum lateral second derivative");
        EMPlannerVerificationHost.Verification.NearlyEqual(2d, configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter, "maximum lateral third derivative");

        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond, "maximum forward speed");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond, "maximum reverse speed");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared, "maximum acceleration");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared, "maximum deceleration");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed, "maximum jerk");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared, "maximum lateral acceleration");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond, "maximum curvature rate");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.01d, configuration.Longitudinal.StopSpeedToleranceMetersPerSecond, "stop speed tolerance");
        EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.ZeroSpeedHoldSeconds, "zero speed hold");

        EMPlannerVerificationHost.Verification.Equal(5, configuration.Solver.MaximumOuterIterations, "maximum outer iterations");
        EMPlannerVerificationHost.Verification.Equal(4000, configuration.Solver.MaximumOsqpIterations, "maximum OSQP iterations");
        EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.AbsoluteTolerance, "absolute tolerance");
        EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.RelativeTolerance, "relative tolerance");
        EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.StrictResidualTolerance, "strict residual tolerance");
        EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.WarmStart, "warm start");
        EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.Polish, "polish");
        EMPlannerVerificationHost.Verification.Equal(false, configuration.Solver.NativeVerbose, "native verbose");

        VerifyLateralWeights(configuration.Lateral.Weights);
        VerifyLongitudinalWeights(configuration.Longitudinal.Weights);

        EmPlanningRequest valid = CreateValidRequest(configuration);
        AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
            CreateRequest(null!, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null reference path");
        AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, null!, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null map");
        AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, null!, 0, EmMotionModel.NonholonomicForwardReverse)), "null configuration");
        AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, CreateMap(false), valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "map not ready");
        AssertStatus(EmPlanningStatus.InvalidReferencePath, EmPlanningRequestValidator.Validate(
            CreateRequest(CreateFailedReferencePath(), valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "smoothing failure");
        AssertStatus(EmPlanningStatus.InvalidReferencePath, EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse)), "segment index");
        AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(double.NaN, 9L, valid.RequestedAtUtc), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "non-finite speed");
        AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(0d, -1L, valid.RequestedAtUtc), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "negative sequence ID");
        AssertStatus(EmPlanningStatus.StaleVehicleState, EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(0d, 9L, valid.RequestedAtUtc.AddSeconds(-1d)), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "stale state");
        AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, valid.Map, new VehicleParameters { LengthMeters = 1d, WidthMeters = 0.5d }, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "missing curvature limit");
        AssertStatus(EmPlanningStatus.UnsupportedMotionMode, EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.CrabTranslation)), "crab motion");
        AssertStatus(EmPlanningStatus.UnsupportedMotionMode, EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.InPlaceRotation)), "in-place rotation");

        EmPlanningRequestValidationResult first = EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse));
        EmPlanningRequestValidationResult second = EmPlanningRequestValidator.Validate(
            CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse));
        EMPlannerVerificationHost.Verification.Equal(first.FailureReason, second.FailureReason, "deterministic rejection message");
    }

    private static void VerifyLateralWeights(LateralWeights weights)
    {
        EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceOffset, "lateral reference weight");
        EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.HeadingDeviation, "lateral heading weight");
        EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.SecondDerivative, "lateral second derivative weight");
        EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ThirdDerivative, "lateral third derivative weight");
        EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.Curvature, "lateral curvature weight");
        EMPlannerVerificationHost.Verification.NearlyEqual(20d, weights.CurvatureVariation, "lateral curvature variation weight");
        EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.PreviousTrajectory, "lateral previous weight");
        EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.RollingTerminal, "lateral rolling terminal weight");
    }

    private static void VerifyLongitudinalWeights(LongitudinalWeights weights)
    {
        EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceSpeed, "longitudinal speed weight");
        EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.Acceleration, "longitudinal acceleration weight");
        EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.Jerk, "longitudinal jerk weight");
        EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.PreviousTrajectory, "longitudinal previous weight");
        EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.TerminalAcceleration, "longitudinal terminal acceleration weight");
    }

    private static void AssertStatus(EmPlanningStatus expected, EmPlanningRequestValidationResult actual, string name)
    {
        EMPlannerVerificationHost.Verification.Equal(expected, actual.Status, name);
    }

    private static EmPlanningRequest CreateValidRequest(EmPlannerConfiguration configuration)
    {
        DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero);
        return CreateRequest(
            CreateValidReferencePath(),
            CreateMap(true),
            new VehicleParameters { LengthMeters = 1d, WidthMeters = 0.5d, SafetyMarginMeters = 0.05d, MaximumCurvaturePerMeter = 0.5d },
            CreateState(0d, 9L, requestedAtUtc),
            configuration,
            0,
            EmMotionModel.NonholonomicForwardReverse);
    }

    private static EmPlanningRequest CreateRequest(
        PathSmoothingResult referencePath,
        PlanningGridMap map,
        VehicleParameters vehicle,
        VehicleMotionState vehicleState,
        EmPlannerConfiguration configuration,
        int segmentIndex,
        EmMotionModel motionModel)
    {
        DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero);
        return new EmPlanningRequest(
            referencePath,
            map,
            vehicle,
            vehicleState,
            configuration,
            segmentIndex,
            null!,
            requestedAtUtc,
            requestedAtUtc.AddSeconds(0.1d),
            "trajectory-1",
            "reference-1",
            string.Empty,
            motionModel);
    }

    private static VehicleMotionState CreateState(double speed, long sequenceId, DateTimeOffset capturedAtUtc)
    {
        return new VehicleMotionState(new Pose2D(0d, 0d, 0d), speed, null, capturedAtUtc, sequenceId);
    }

    private static PlanningGridMap CreateMap(bool planningReady)
    {
        var result = new PlanningMapFactory().Create(new PlanningMapRequest
        {
            Bounds = new MapBoundsMm(-2000f, 2000f, -2000f, 2000f),
            ResolutionMm = 100f,
            ObstacleSources = new List<IMapObstacleSource>(),
            AllowExplicitEmptyMap = planningReady,
        });
        if (!result.Succeeded || result.Map == null)
            throw new InvalidOperationException("Unable to create test planning map.");
        return result.Map;
    }

    private static PathSmoothingResult CreateValidReferencePath()
    {
        var points = new List<SmoothedPathPoint>
        {
            new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
            new SmoothedPathPoint(1d, 0d, 0d, 0d, 1d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
        };
        var segments = new List<SmoothedPathSegment>
        {
            new SmoothedPathSegment(0, TravelDirection.Forward, 0, 1, false, false),
        };
        var metrics = new PathQualityMetrics(true, 1d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
        return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
            new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
    }

    private static PathSmoothingResult CreateFailedReferencePath()
    {
        return PathSmoothingResult.Failure(PathSmoothingStatus.Failed, new PathSmoothingDiagnostics());
    }
}