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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class CorridorChecks
{
    public static void Run()
    {
        VerifiesEmptyAndNarrowedCorridors();
        VerifiesSeedConnectedIntervalIsPreserved();
        VerifiesDisappearingSeedIntervalFails();
    }

    private static void VerifiesEmptyAndNarrowedCorridors()
    {
        DirectionSegmentView segment = CreateStraightSegment();
        CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor;
        VehicleParameters vehicle = CreateVehicle();
        var builder = new StaticCorridorBuilder();

        Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, Array.Empty<FrenetProjection>(),
            CreateMap(Array.Empty<IMapObstacle>()), vehicle, configuration, out StaticCorridor empty, out string emptyReason),
            "empty map corridor succeeds: " + emptyReason);
        VerifyAnchors(empty, 0.2d, 1.8d);
        for (int index = 1; index + 1 < empty.Stations.Count; index++)
        {
            Verification.NearlyEqual(-0.3d, empty.Stations[index].MinimumL, "empty map minimum l");
            Verification.NearlyEqual(0.3d, empty.Stations[index].MaximumL, "empty map maximum l");
        }

        IMapObstacle[] leftNarrowing =
        {
            new AxisAlignedRectangleObstacle(800f, 1200f, 150f, 400f),
        };
        PlanningGridMap narrowedMap = CreateMap(leftNarrowing);
        IReadOnlyList<FrenetProjection> seed = CreateSeed(segment, -0.2d);
        Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, seed, narrowedMap, vehicle, configuration,
            out StaticCorridor narrowed, out string narrowedReason), "narrowed corridor succeeds: " + narrowedReason);
        LateralInterval narrowedStation = FindStation(narrowed, 1d);
        Verification.True(narrowedStation.MaximumL < 0.3d, "left obstacle narrows positive-l side");
        VerifyAcceptedSamplesUseExactFootprints(segment, narrowed, narrowedMap, vehicle, configuration);
    }

    private static void VerifiesSeedConnectedIntervalIsPreserved()
    {
        DirectionSegmentView segment = CreateStraightSegment();
        CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor;
        VehicleParameters vehicle = CreateVehicle();
        IMapObstacle[] split =
        {
            new AxisAlignedRectangleObstacle(800f, 1200f, -50f, 50f),
        };
        IReadOnlyList<FrenetProjection> seed = CreateSeed(segment, -0.2d);
        var builder = new StaticCorridorBuilder();

        Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, seed, CreateMap(split), vehicle, configuration,
            out StaticCorridor corridor, out string reason), "split corridor succeeds for left seed: " + reason);
        for (int index = 0; index < corridor.Stations.Count; index++)
        {
            LateralInterval station = corridor.Stations[index];
            Verification.True(station.MinimumL <= station.SeedL && station.SeedL <= station.MaximumL,
                "chosen interval contains seed at station " + index);
        }
        Verification.True(FindStation(corridor, 1d).MaximumL < 0d,
            "split station retains seed-connected left interval instead of right interval");
    }

    private static void VerifiesDisappearingSeedIntervalFails()
    {
        DirectionSegmentView segment = CreateStraightSegment();
        CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor;
        VehicleParameters vehicle = CreateVehicle();
        IMapObstacle[] removesLeft =
        {
            new AxisAlignedRectangleObstacle(800f, 1200f, -400f, -50f),
        };
        var builder = new StaticCorridorBuilder();

        Verification.True(!builder.TryBuild(segment, 0.2d, 1.8d, CreateSeed(segment, -0.2d), CreateMap(removesLeft),
            vehicle, configuration, out _, out string failureReason),
            "disappearing seed-connected interval does not switch sides");
        Verification.True(failureReason.IndexOf("S=", StringComparison.Ordinal) >= 0,
            "failure identifies the first failed reference-s station: " + failureReason);
    }

    private static void VerifyAnchors(StaticCorridor corridor, double startS, double endS)
    {
        Verification.NearlyEqual(startS, corridor.Stations[0].ReferenceS, "first station is exact requested start");
        Verification.NearlyEqual(endS, corridor.Stations[corridor.Stations.Count - 1].ReferenceS,
            "last station is exact requested end");
    }

    private static void VerifyAcceptedSamplesUseExactFootprints(DirectionSegmentView segment, StaticCorridor corridor,
        PlanningGridMap map, VehicleParameters vehicle, CorridorConfiguration configuration)
    {
        var checker = new FootprintCollisionChecker();
        for (int stationIndex = 0; stationIndex < corridor.Stations.Count; stationIndex++)
        {
            LateralInterval station = corridor.Stations[stationIndex];
            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, station.ReferenceS);
            for (double l = station.MinimumL; l <= station.MaximumL + 1e-12d; l += configuration.LateralSampleSpacingMeters)
            {
                Verification.True(FrenetTransform.TryReconstruct(reference, l, 0d, 0.2d, out Pose2D pose),
                    "accepted sample reconstructs");
                Verification.True(checker.IsPoseCollisionFree(pose, map, vehicle, configuration.AdditionalClearanceReserveMeters,
                    out _), "accepted sample passes exact rotated footprint");
            }
        }
    }

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

    private static IReadOnlyList<FrenetProjection> CreateSeed(DirectionSegmentView segment, double l)
    {
        return new FrenetProjection[]
        {
            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0.2d), l, 0d, 0d),
            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 1.8d), l, 0d, 0d),
        };
    }

    private static SmoothedPathPoint Point(double x, double s)
    {
        return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d,
            false, SmoothedPathPointSource.Anchor);
    }

    private static VehicleParameters CreateVehicle()
    {
        return new VehicleParameters
        {
            LengthMeters = 0.10d,
            WidthMeters = 0.10d,
            SafetyMarginMeters = 0d,
            MaximumCurvaturePerMeter = 1d,
        };
    }

    private static PlanningGridMap CreateMap(IReadOnlyList<IMapObstacle> obstacles)
    {
        IMapObstacleSource[] sources = obstacles.Count == 0
            ? Array.Empty<IMapObstacleSource>()
            : new IMapObstacleSource[] { new ManualObstacleSource("corridor-test", 1L, true, obstacles) };
        var result = new PlanningMapFactory().Create(new PlanningMapRequest
        {
            Bounds = new MapBoundsMm(-1000f, 3000f, -1000f, 1000f),
            ResolutionMm = 20f,
            ObstacleSources = sources,
            AllowExplicitEmptyMap = obstacles.Count == 0,
        });
        Verification.True(result.Succeeded && result.Map != null && result.Map.PlanningReady,
            "corridor test map builds: " + result.FailureReason);
        return result.Map!;
    }

    private static LateralInterval FindStation(StaticCorridor corridor, double referenceS)
    {
        for (int index = 0; index < corridor.Stations.Count; index++)
        {
            if (Math.Abs(corridor.Stations[index].ReferenceS - referenceS) <= 1e-12d)
                return corridor.Stations[index];
        }
        throw new InvalidOperationException("Requested corridor station was not sampled.");
    }
}
