using System;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

namespace EMPlannerVerificationHost;

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

    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 bool Throws(Action action)
    {
        try { action(); return false; }
        catch (ArgumentOutOfRangeException) { return true; }
    }
}