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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Builds only the seed-connected static free-space corridor of a direction segment.</summary>
public sealed class StaticCorridorBuilder
{
    private const double Epsilon = 1e-12d;
    private const double ReconstructionDenominator = 1e-12d;

    private readonly FootprintCollisionChecker _collisionChecker;

    public StaticCorridorBuilder()
        : this(new FootprintCollisionChecker())
    {
    }

    public StaticCorridorBuilder(FootprintCollisionChecker collisionChecker)
    {
        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
    }

    public bool TryBuild(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
        IReadOnlyList<FrenetProjection> seed, PlanningGridMap map, VehicleParameters vehicle,
        CorridorConfiguration configuration, out StaticCorridor corridor, out string failureReason)
    {
        corridor = null;
        failureReason = string.Empty;
        if (!TryValidateInput(segment, startReferenceS, endReferenceS, map, vehicle, configuration, out failureReason))
            return false;
        if (!TryReadSeeds(seed, segment, out List<SeedSample> seeds, out failureReason))
            return false;

        var stations = new List<LateralInterval>();
        LateralInterval previous = null;
        foreach (double referenceS in CreateStations(startReferenceS, endReferenceS,
                     configuration.LongitudinalSampleSpacingMeters))
        {
            FrenetReferencePoint reference;
            try
            {
                reference = ReferencePathInterpolator.Interpolate(segment, referenceS);
            }
            catch (ArgumentException)
            {
                failureReason = "Reference interpolation failed at S=" + referenceS + ".";
                return false;
            }

            double seedL = GetSeedL(seeds, referenceS);
            if (seedL < -configuration.MaximumLateralOffsetMeters - Epsilon ||
                seedL > configuration.MaximumLateralOffsetMeters + Epsilon)
            {
                failureReason = "Seed lateral offset is outside configured bounds at S=" + referenceS + ".";
                return false;
            }

            if (!TrySelectSeedConnectedInterval(reference, seedL, map, vehicle, configuration,
                    out double minimumL, out double maximumL))
            {
                failureReason = "Seed-connected free interval disappeared at S=" + referenceS + ".";
                return false;
            }

            var selected = new LateralInterval(referenceS, minimumL, maximumL, seedL);
            if (previous != null && Math.Max(previous.MinimumL, selected.MinimumL) >
                Math.Min(previous.MaximumL, selected.MaximumL) + Epsilon)
            {
                failureReason = "Seed-connected corridor loses overlap at S=" + referenceS + ".";
                return false;
            }

            stations.Add(selected);
            previous = selected;
        }

        corridor = new StaticCorridor(stations);
        return true;
    }

    private static bool TryValidateInput(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
        PlanningGridMap map, VehicleParameters vehicle, CorridorConfiguration configuration, out string failureReason)
    {
        failureReason = string.Empty;
        if (segment == null || map == null || vehicle == null || configuration == null || !map.PlanningReady)
        {
            failureReason = "A planning-ready map, vehicle, segment, and corridor configuration are required.";
            return false;
        }
        if (!IsFinite(startReferenceS) || !IsFinite(endReferenceS) || startReferenceS < 0d ||
            endReferenceS < startReferenceS || endReferenceS > segment.LengthMeters + Epsilon)
        {
            failureReason = "Requested corridor S anchors are invalid.";
            return false;
        }
        if (!IsPositiveFinite(configuration.LongitudinalSampleSpacingMeters) ||
            !IsPositiveFinite(configuration.LateralSampleSpacingMeters) ||
            !IsFinite(configuration.MaximumLateralOffsetMeters) || configuration.MaximumLateralOffsetMeters < 0d ||
            !IsFinite(configuration.AdditionalClearanceReserveMeters) || configuration.AdditionalClearanceReserveMeters < 0d ||
            !IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters))
        {
            failureReason = "Corridor configuration is invalid.";
            return false;
        }
        if (!IsPositiveFinite(vehicle.LengthMeters) || !IsPositiveFinite(vehicle.WidthMeters) ||
            !IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d)
        {
            failureReason = "Vehicle footprint dimensions are invalid.";
            return false;
        }
        return true;
    }

    private static bool TryReadSeeds(IReadOnlyList<FrenetProjection> input, DirectionSegmentView segment,
        out List<SeedSample> seeds, out string failureReason)
    {
        seeds = new List<SeedSample>();
        failureReason = string.Empty;
        if (input == null)
            return true;

        for (int index = 0; index < input.Count; index++)
        {
            FrenetProjection projection = input[index];
            if (projection == null || projection.ReferencePoint == null ||
                projection.ReferenceS < -Epsilon || projection.ReferenceS > segment.LengthMeters + Epsilon ||
                !IsFinite(projection.LateralOffset))
            {
                failureReason = "Corridor seeds must belong to the selected direction segment.";
                return false;
            }
            seeds.Add(new SeedSample(projection.ReferenceS, projection.LateralOffset));
        }
        seeds.Sort((left, right) => left.ReferenceS.CompareTo(right.ReferenceS));
        return true;
    }

    private static IEnumerable<double> CreateStations(double startReferenceS, double endReferenceS, double spacing)
    {
        yield return startReferenceS;
        for (double candidate = startReferenceS + spacing; candidate < endReferenceS - Epsilon; candidate += spacing)
            yield return candidate;
        if (endReferenceS > startReferenceS + Epsilon)
            yield return endReferenceS;
    }

    private bool TrySelectSeedConnectedInterval(FrenetReferencePoint reference, double seedL, PlanningGridMap map,
        VehicleParameters vehicle, CorridorConfiguration configuration, out double minimumL, out double maximumL)
    {
        minimumL = 0d;
        maximumL = 0d;
        List<double> samples = CreateLateralSamples(seedL, configuration.MaximumLateralOffsetMeters,
            configuration.LateralSampleSpacingMeters);
        int index = 0;
        while (index < samples.Count)
        {
            if (!IsCollisionFree(reference, samples[index], map, vehicle, configuration))
            {
                index++;
                continue;
            }

            double groupMinimum = samples[index];
            double groupMaximum = samples[index];
            bool containsSeed = Math.Abs(samples[index] - seedL) <= Epsilon;
            index++;
            while (index < samples.Count && samples[index] - groupMaximum <= configuration.LateralSampleSpacingMeters + Epsilon)
            {
                if (!IsCollisionFree(reference, samples[index], map, vehicle, configuration))
                {
                    index++;
                    break;
                }
                groupMaximum = samples[index];
                containsSeed |= Math.Abs(samples[index] - seedL) <= Epsilon;
                index++;
            }

            if (containsSeed)
            {
                minimumL = groupMinimum;
                maximumL = groupMaximum;
                return true;
            }
        }
        return false;
    }

    private static List<double> CreateLateralSamples(double seedL, double maximumOffset, double spacing)
    {
        var samples = new List<double>();
        for (double candidate = -maximumOffset; candidate < maximumOffset - Epsilon; candidate += spacing)
            AddSortedUnique(samples, candidate);
        AddSortedUnique(samples, maximumOffset);
        AddSortedUnique(samples, -maximumOffset);
        AddSortedUnique(samples, seedL);
        samples.Sort();
        return samples;
    }

    private bool IsCollisionFree(FrenetReferencePoint reference, double lateralOffset, PlanningGridMap map,
        VehicleParameters vehicle, CorridorConfiguration configuration)
    {
        if (!FrenetTransform.TryReconstruct(reference, lateralOffset, 0d, ReconstructionDenominator, out Pose2D pose))
            return false;

        if (IsObviouslyClear(pose, map, vehicle, configuration.AdditionalClearanceReserveMeters))
            return true;
        return _collisionChecker.IsPoseCollisionFree(pose, map, vehicle,
            configuration.AdditionalClearanceReserveMeters, out _);
    }

    private static bool IsObviouslyClear(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
        double additionalClearanceReserveMeters)
    {
        double totalMargin = vehicle.SafetyMarginMeters + additionalClearanceReserveMeters;
        double halfLength = vehicle.LengthMeters / 2d + totalMargin;
        double halfWidth = vehicle.WidthMeters / 2d + totalMargin;
        double radius = Math.Sqrt(halfLength * halfLength + halfWidth * halfWidth);
        if (!IsFinite(radius) || map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y) <= radius)
            return false;

        double cosine = Math.Cos(pose.Heading);
        double sine = Math.Sin(pose.Heading);
        for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
        for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
        {
            double x = pose.X + longitudinalSign * halfLength * cosine - lateralSign * halfWidth * sine;
            double y = pose.Y + longitudinalSign * halfLength * sine + lateralSign * halfWidth * cosine;
            if (!map.TryWorldToGrid(x, y, out _, out _))
                return false;
        }
        return true;
    }

    private static double GetSeedL(IReadOnlyList<SeedSample> seeds, double referenceS)
    {
        if (seeds.Count == 0)
            return 0d;
        if (referenceS <= seeds[0].ReferenceS)
            return seeds[0].LateralOffset;
        for (int index = 1; index < seeds.Count; index++)
        {
            SeedSample upper = seeds[index];
            if (referenceS <= upper.ReferenceS)
            {
                SeedSample lower = seeds[index - 1];
                double span = upper.ReferenceS - lower.ReferenceS;
                return span <= Epsilon ? upper.LateralOffset : lower.LateralOffset +
                    (upper.LateralOffset - lower.LateralOffset) * (referenceS - lower.ReferenceS) / span;
            }
        }
        return seeds[seeds.Count - 1].LateralOffset;
    }

    private static void AddSortedUnique(List<double> samples, double value)
    {
        for (int index = 0; index < samples.Count; index++)
            if (Math.Abs(samples[index] - value) <= Epsilon)
                return;
        samples.Add(value);
    }

    private static bool IsPositiveFinite(double value)
    {
        return IsFinite(value) && value > 0d;
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }

    private sealed class SeedSample
    {
        public SeedSample(double referenceS, double lateralOffset)
        {
            ReferenceS = referenceS;
            LateralOffset = lateralOffset;
        }

        public double ReferenceS { get; }
        public double LateralOffset { get; }
    }
}
