using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>按硬方向边界生成并合并局部 G2 曲率事件的候选窗口。</summary>
internal sealed class LocalG2WindowPlanner
{
    private const double MergeToleranceMeters = 1e-9d;

    internal bool TryPlan(
        PreparedPath originalPath,
        IReadOnlyList<CurvatureTransition> transitions,
        LocalG2OptionsSnapshot options,
        out IReadOnlyList<LocalG2SmoothingRegion> regions,
        out string reason)
    {
        regions = Empty<LocalG2SmoothingRegion>();
        reason = string.Empty;
        if (originalPath == null || transitions == null || options == null)
        {
            reason = "局部 G2 窗口规划输入无效。";
            return false;
        }
        if (!TryGetSegmentLengths(originalPath, out Dictionary<int, double> segmentLengths, out reason)) return false;

        var ordered = new List<CurvatureTransition>(transitions.Count);
        for (int index = 0; index < transitions.Count; index++)
        {
            CurvatureTransition transition = transitions[index];
            if (transition == null || !segmentLengths.TryGetValue(transition.SegmentIndex, out double length) ||
                !NumericGuard.IsFinite(transition.LocalArcLengthMeters) || transition.LocalArcLengthMeters < 0d ||
                transition.LocalArcLengthMeters > length + MergeToleranceMeters)
            {
                reason = "局部 G2 曲率事件不属于有效方向分段。";
                return false;
            }
            ordered.Add(transition);
        }
        ordered.Sort(CompareTransitions);

        var planned = new List<LocalG2SmoothingRegion>();
        int cursor = 0;
        while (cursor < ordered.Count)
        {
            CurvatureTransition first = ordered[cursor];
            double segmentLength = segmentLengths[first.SegmentIndex];
            var group = new List<CurvatureTransition> { first };
            IReadOnlyList<LocalG2WindowVariant> variants =
                BuildVariants(group, segmentLength, options);
            if (variants.Count == 0)
            {
                reason = "局部 G2 单事件无法生成满足总长度约束的窗口。";
                return false;
            }
            cursor++;

            while (cursor < ordered.Count &&
                ordered[cursor].SegmentIndex == first.SegmentIndex)
            {
                var tentative = new List<CurvatureTransition>(group)
                {
                    ordered[cursor],
                };
                IReadOnlyList<LocalG2WindowVariant> tentativeVariants =
                    BuildVariants(tentative, segmentLength, options);
                if (tentativeVariants.Count == 0) break;
                group = tentative;
                variants = tentativeVariants;
                cursor++;
            }

            double minimumStart = double.PositiveInfinity;
            double maximumEnd = double.NegativeInfinity;
            for (int variantIndex = 0; variantIndex < variants.Count; variantIndex++)
            {
                minimumStart = Math.Min(
                    minimumStart,
                    variants[variantIndex].StartArcLengthMeters);
                maximumEnd = Math.Max(
                    maximumEnd,
                    variants[variantIndex].EndArcLengthMeters);
            }

            planned.Add(new LocalG2SmoothingRegion(
                first.SegmentIndex,
                group,
                minimumStart,
                maximumEnd,
                variants));
        }
        regions = new ReadOnlyCollection<LocalG2SmoothingRegion>(planned);
        return true;
    }

    private static IReadOnlyList<LocalG2WindowVariant> BuildVariants(
        IReadOnlyList<CurvatureTransition> transitions,
        double segmentLength,
        LocalG2OptionsSnapshot options)
    {
        var variants = new List<LocalG2WindowVariant>();
        double firstEvent = transitions[0].LocalArcLengthMeters;
        double lastEvent = transitions[transitions.Count - 1].LocalArcLengthMeters;
        double anchor = (firstEvent + lastEvent) / 2d;
        IReadOnlyList<double> targets = BuildTargets(options, segmentLength);
        double[] ratios = { 0.5d, 0.4d, 0.6d };
        for (int ratioIndex = 0; ratioIndex < ratios.Length; ratioIndex++)
        {
            for (int targetIndex = 0; targetIndex < targets.Count; targetIndex++)
            {
                if (variants.Count >= options.MaximumCandidatesPerRegion)
                    return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
                AddIfLegal(
                    variants,
                    targets[targetIndex],
                    ratios[ratioIndex],
                    anchor,
                    firstEvent,
                    lastEvent,
                    segmentLength,
                    ratioIndex == 0,
                    options);
            }
        }
        return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
    }

    private static void AddIfLegal(List<LocalG2WindowVariant> variants, double target, double leftRatio,
        double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift,
        LocalG2OptionsSnapshot options)
    {
        if (variants.Count >= options.MaximumCandidatesPerRegion) return;
        double left = target * leftRatio;
        double right = target - left;
        double availableLeft = anchor;
        double availableRight = segmentLength - anchor;
        if (permitBoundaryShift)
        {
            left = Math.Min(left, availableLeft);
            right = Math.Min(right, availableRight);
            double missing = target - left - right;
            double addRight = Math.Min(missing, availableRight - right);
            right += addRight;
            left += Math.Min(missing - addRight, availableLeft - left);
        }
        else if (left > availableLeft + MergeToleranceMeters || right > availableRight + MergeToleranceMeters)
        {
            return;
        }

        double start = anchor - left;
        double end = anchor + right;
        if (start > firstEvent + MergeToleranceMeters || end + MergeToleranceMeters < lastEvent || end - start + MergeToleranceMeters < target)
            return;
        double actualLength = end - start;
        if (actualLength + MergeToleranceMeters < options.MinimumWindowLengthMeters ||
            actualLength > options.MaximumWindowLengthMeters + MergeToleranceMeters)
        {
            return;
        }
        variants.Add(new LocalG2WindowVariant(variants.Count, start, end, left, right));
    }

    private static IReadOnlyList<double> BuildTargets(LocalG2OptionsSnapshot options, double segmentLength)
    {
        if (segmentLength + MergeToleranceMeters < options.MinimumWindowLengthMeters)
            return new ReadOnlyCollection<double>(new List<double>());

        double[] requested =
        {
            options.PreferredWindowLengthMeters,
            options.MinimumWindowLengthMeters,
            options.MaximumWindowLengthMeters,
            0.75d * options.PreferredWindowLengthMeters,
            1.25d * options.PreferredWindowLengthMeters,
        };
        var targets = new List<double>(requested.Length);
        for (int index = 0; index < requested.Length; index++)
        {
            double target = Math.Min(segmentLength,
                Math.Max(options.MinimumWindowLengthMeters, Math.Min(options.MaximumWindowLengthMeters, requested[index])));
            bool duplicate = false;
            for (int prior = 0; prior < targets.Count; prior++)
            {
                if (Math.Abs(targets[prior] - target) <= MergeToleranceMeters)
                {
                    duplicate = true;
                    break;
                }
            }
            if (!duplicate) targets.Add(target);
        }
        return targets;
    }

    private static bool TryGetSegmentLengths(PreparedPath originalPath, out Dictionary<int, double> lengths, out string reason)
    {
        lengths = new Dictionary<int, double>();
        reason = string.Empty;
        for (int position = 0; position < originalPath.Segments.Count; position++)
        {
            PreparedDirectionSegment segment = originalPath.Segments[position];
            if (segment == null || segment.SegmentIndex != position || segment.Points == null || segment.Points.Count == 0)
            {
                reason = "局部 G2 窗口规划的预处理方向分段无效。";
                return false;
            }
            double previousArc = -1d;
            for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
            {
                double arc = segment.Points[pointIndex].ArcLength;
                if (!NumericGuard.IsFinite(arc) || arc < 0d || arc < previousArc)
                {
                    reason = "局部 G2 窗口规划要求分段弧长有限且非递减。";
                    return false;
                }
                previousArc = arc;
            }
            lengths.Add(segment.SegmentIndex, previousArc);
        }
        return true;
    }

    private static int CompareTransitions(CurvatureTransition left, CurvatureTransition right)
    {
        int segment = left.SegmentIndex.CompareTo(right.SegmentIndex);
        if (segment != 0) return segment;
        int arc = left.LocalArcLengthMeters.CompareTo(right.LocalArcLengthMeters);
        return arc != 0 ? arc : left.LeftCoarsePathIndex.CompareTo(right.LeftCoarsePathIndex);
    }

    private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());

    public static class TestHooks
    {
        public static WindowPlanningTestSnapshot Execute(string scenario)
        {
            if (string.IsNullOrWhiteSpace(scenario))
                throw new ArgumentException("A scenario is required.", nameof(scenario));

            IReadOnlyList<CurvatureTransition> transitions;
            double segmentLength;
            switch (scenario)
            {
                case "SeparatedByOneMeter":
                    transitions = new[]
                    {
                        Transition(0.2d, 0),
                        Transition(1.2d, 1),
                    };
                    segmentLength = 1.4d;
                    break;
                case "Mergeable":
                    transitions = new[]
                    {
                        Transition(0.4d, 0),
                        Transition(0.7d, 1),
                    };
                    segmentLength = 1.4d;
                    break;
                case "ThreeEventPartition":
                    transitions = new[]
                    {
                        Transition(0.2d, 0),
                        Transition(0.6d, 1),
                        Transition(1.2d, 2),
                    };
                    segmentLength = 1.4d;
                    break;
                case "NearBoundary":
                    transitions = new[] { Transition(0.1d, 0) };
                    segmentLength = 1d;
                    break;
                case "InteriorCoverage":
                    transitions = new[] { Transition(1d, 0) };
                    segmentLength = 2d;
                    break;
                default:
                    throw new ArgumentOutOfRangeException(nameof(scenario));
            }

            var planner = new LocalG2WindowPlanner();
            if (!planner.TryPlan(
                    CreatePreparedPath(segmentLength),
                    transitions,
                    new LocalG2OptionsSnapshot(new PathSmoothingConfiguration()),
                    out IReadOnlyList<LocalG2SmoothingRegion> regions,
                    out string reason))
            {
                throw new InvalidOperationException(reason);
            }

            double maximumLength = 0d;
            bool exactEnvelope = true;
            var counts = new List<string>(regions.Count);
            var signature = new List<string>();
            for (int regionIndex = 0; regionIndex < regions.Count; regionIndex++)
            {
                LocalG2SmoothingRegion region = regions[regionIndex];
                counts.Add(region.Transitions.Count.ToString());
                double minimumStart = double.PositiveInfinity;
                double maximumEnd = double.NegativeInfinity;
                for (int variantIndex = 0; variantIndex < region.WindowVariants.Count; variantIndex++)
                {
                    LocalG2WindowVariant variant = region.WindowVariants[variantIndex];
                    maximumLength = Math.Max(
                        maximumLength,
                        variant.EndArcLengthMeters - variant.StartArcLengthMeters);
                    minimumStart = Math.Min(minimumStart, variant.StartArcLengthMeters);
                    maximumEnd = Math.Max(maximumEnd, variant.EndArcLengthMeters);
                    signature.Add(
                        region.SegmentIndex + ":" +
                        variant.CandidateIndex + ":" +
                        variant.StartArcLengthMeters.ToString("R") + ":" +
                        variant.EndArcLengthMeters.ToString("R"));
                }
                exactEnvelope &= Math.Abs(region.MaximumStartArcLengthMeters - minimumStart) <= 1e-9d;
                exactEnvelope &= Math.Abs(region.MaximumEndArcLengthMeters - maximumEnd) <= 1e-9d;
            }

            IReadOnlyList<LocalG2WindowVariant> firstVariants = regions[0].WindowVariants;
            bool representativeTargetsFirst =
                firstVariants.Count >= 3 &&
                Math.Abs(WindowLength(firstVariants[0]) - 0.50d) <= 1e-9d &&
                Math.Abs(WindowLength(firstVariants[1]) - 0.20d) <= 1e-9d &&
                Math.Abs(WindowLength(firstVariants[2]) - 0.80d) <= 1e-9d;
            bool hasAsymmetricVariant = false;
            for (int index = 0; index < firstVariants.Count; index++)
            {
                if (Math.Abs(
                        firstVariants[index].LeftWindowLengthMeters -
                        firstVariants[index].RightWindowLengthMeters) > 1e-9d)
                {
                    hasAsymmetricVariant = true;
                    break;
                }
            }

            LocalG2WindowVariant first = firstVariants[0];
            return new WindowPlanningTestSnapshot(
                regions.Count,
                string.Join(",", counts),
                maximumLength,
                exactEnvelope,
                first.LeftWindowLengthMeters,
                first.RightWindowLengthMeters,
                string.Join("|", signature),
                firstVariants.Count,
                representativeTargetsFirst,
                hasAsymmetricVariant);
        }

        public sealed class WindowPlanningTestSnapshot
        {
            internal WindowPlanningTestSnapshot(
                int regionCount,
                string transitionCounts,
                double maximumWindowLength,
                bool exactEnvelope,
                double firstLeftLength,
                double firstRightLength,
                string signature,
                int firstRegionVariantCount,
                bool representativeTargetsFirst,
                bool hasAsymmetricVariant)
            {
                RegionCount = regionCount;
                TransitionCounts = transitionCounts;
                MaximumWindowLength = maximumWindowLength;
                ExactEnvelope = exactEnvelope;
                FirstLeftLength = firstLeftLength;
                FirstRightLength = firstRightLength;
                Signature = signature;
                FirstRegionVariantCount = firstRegionVariantCount;
                RepresentativeTargetsFirst = representativeTargetsFirst;
                HasAsymmetricVariant = hasAsymmetricVariant;
            }

            public int RegionCount { get; }
            public string TransitionCounts { get; }
            public double MaximumWindowLength { get; }
            public bool ExactEnvelope { get; }
            public double FirstLeftLength { get; }
            public double FirstRightLength { get; }
            public string Signature { get; }
            public int FirstRegionVariantCount { get; }
            public bool RepresentativeTargetsFirst { get; }
            public bool HasAsymmetricVariant { get; }
        }

        private static double WindowLength(LocalG2WindowVariant variant) =>
            variant.EndArcLengthMeters - variant.StartArcLengthMeters;

        private static CurvatureTransition Transition(double arcLength, int index)
        {
            return new CurvatureTransition(
                0,
                index,
                index + 1,
                arcLength,
                arcLength,
                0d,
                0d,
                index % 2 == 0 ? 0d : 0.5d,
                index % 2 == 0 ? 0.5d : 0d);
        }

        private static PreparedPath CreatePreparedPath(double length)
        {
            var points = new[]
            {
                new SmoothingPoint2D(
                    0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
                new SmoothingPoint2D(
                    length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
            };
            return new PreparedPath(new[]
            {
                new PreparedDirectionSegment(
                    0, TravelDirection.Forward, points, false, false),
            });
        }
    }
}