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

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>从原始粗路径的同向相邻点中识别恒曲率原语边界。</summary>
internal sealed class CurvatureTransitionDetector
{
    internal bool TryDetect(
        PathSmoothingRequest request,
        double maximumVehicleCurvaturePerMeter,
        LocalG2OptionsSnapshot options,
        out IReadOnlyList<CurvatureTransition> transitions,
        out string reason)
    {
        transitions = Empty<CurvatureTransition>();
        reason = string.Empty;
        if (request == null || request.CoarsePath == null || request.Segments == null || options == null ||
            !NumericGuard.IsPositiveFinite(maximumVehicleCurvaturePerMeter))
        {
            reason = "局部 G2 曲率事件检测输入无效。";
            return false;
        }
        if (!ValidatePath(request.CoarsePath, out reason) || !ValidateSegments(request.CoarsePath, request.Segments, out reason))
            return false;

        double threshold = Math.Max(
            options.AbsoluteCurvatureJumpFloorPerMeter,
            options.CurvatureJumpRatioOfMaximum * maximumVehicleCurvaturePerMeter);
        if (!NumericGuard.IsPositiveFinite(threshold))
        {
            reason = "局部 G2 曲率事件阈值无效。";
            return false;
        }

        var detected = new List<CurvatureTransition>();
        for (int segmentPosition = 0; segmentPosition < request.Segments.Count; segmentPosition++)
        {
            PathSegment segment = request.Segments[segmentPosition];
            double segmentStartArc = request.CoarsePath[segment.StartIndex].ArcLength;
            for (int leftIndex = segment.StartIndex; leftIndex < segment.EndIndex; leftIndex++)
            {
                CoarsePathPoint left = request.CoarsePath[leftIndex];
                CoarsePathPoint right = request.CoarsePath[leftIndex + 1];
                double delta = right.VehicleCurvature - left.VehicleCurvature;
                if (Math.Abs(delta) >= threshold)
                {
                    detected.Add(new CurvatureTransition(
                        segment.SegmentIndex,
                        leftIndex,
                        leftIndex + 1,
                        left.ArcLength - segmentStartArc,
                        left.X,
                        left.Y,
                        left.Heading,
                        left.VehicleCurvature,
                        right.VehicleCurvature));
                }
            }
        }
        transitions = new ReadOnlyCollection<CurvatureTransition>(detected);
        return true;
    }

    /// <summary>反射脚本使用的确定性检测与窗口规划接缝。</summary>
    public static class TestHooks
    {
        public static DetectionTestSnapshot Execute(string scenario)
        {
            if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
            switch (scenario)
            {
                case "SingleTransition": return DetectSingleTransition();
                case "GearSwitch": return DetectGearSwitch();
                case "Noise": return DetectNoise();
                case "Overlap": return PlanOverlap();
                case "NearStart": return PlanNearStart();
                default: throw new ArgumentOutOfRangeException(nameof(scenario));
            }
        }

        public sealed class DetectionTestSnapshot
        {
            internal DetectionTestSnapshot(int transitionCount, double maximumJump, int regionCount,
                int transitionCountInFirstRegion, double startArcLength, double leftWindowLength, double rightWindowLength)
            {
                TransitionCount = transitionCount;
                MaximumJump = maximumJump;
                RegionCount = regionCount;
                TransitionCountInFirstRegion = transitionCountInFirstRegion;
                StartArcLength = startArcLength;
                LeftWindowLength = leftWindowLength;
                RightWindowLength = rightWindowLength;
            }

            public int TransitionCount { get; }
            public double MaximumJump { get; }
            public int RegionCount { get; }
            public int TransitionCountInFirstRegion { get; }
            public double StartArcLength { get; }
            public double LeftWindowLength { get; }
            public double RightWindowLength { get; }
        }

        private static DetectionTestSnapshot DetectSingleTransition()
        {
            IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
                new[] { Point(0d, 0d), Point(0.1d, 0.4167d), Point(0.2d, 0.4167d) },
                new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) }));
            return Snapshot(transitions);
        }

        private static DetectionTestSnapshot DetectGearSwitch()
        {
            IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
                new[]
                {
                    Point(0d, 0d, TravelDirection.Forward), Point(0.2d, 0d, TravelDirection.Forward),
                    Point(0.2d, 0.4167d, TravelDirection.Reverse, true), Point(0.4d, 0.4167d, TravelDirection.Reverse),
                },
                new[]
                {
                    new PathSegment(0, TravelDirection.Forward, 0, 1, false, true),
                    new PathSegment(1, TravelDirection.Reverse, 2, 3, true, false),
                }));
            return Snapshot(transitions);
        }

        private static DetectionTestSnapshot DetectNoise()
        {
            IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
                new[] { Point(0d, 0d), Point(0.1d, 0.01d), Point(0.2d, 0.01d) },
                new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) }));
            return Snapshot(transitions);
        }

        private static DetectionTestSnapshot PlanOverlap()
        {
            LocalG2OptionsSnapshot options = CreateOptions();
            var transitions = new[]
            {
                new CurvatureTransition(0, 1, 2, 0.4d, 0.4d, 0d, 0d, 0d, 0.5d),
                new CurvatureTransition(0, 2, 3, 0.7d, 0.7d, 0d, 0d, 0.5d, 0d),
            };
            var planner = new LocalG2WindowPlanner();
            if (!planner.TryPlan(CreatePreparedPath(1.4d), transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
                throw new InvalidOperationException(reason);
            return new DetectionTestSnapshot(2, 0.5d, regions.Count, regions[0].Transitions.Count, 0d, 0d, 0d);
        }

        private static DetectionTestSnapshot PlanNearStart()
        {
            LocalG2OptionsSnapshot options = CreateOptions();
            var transitions = new[] { new CurvatureTransition(0, 0, 1, 0.1d, 0.1d, 0d, 0d, 0d, 0.5d) };
            var planner = new LocalG2WindowPlanner();
            if (!planner.TryPlan(CreatePreparedPath(1d), transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
                throw new InvalidOperationException(reason);
            LocalG2WindowVariant first = regions[0].WindowVariants[0];
            return new DetectionTestSnapshot(1, 0.5d, regions.Count, 1,
                first.StartArcLengthMeters, first.LeftWindowLengthMeters, first.RightWindowLengthMeters);
        }

        private static IReadOnlyList<CurvatureTransition> Detect(PathSmoothingRequest request)
        {
            var detector = new CurvatureTransitionDetector();
            if (!detector.TryDetect(request, 0.8333d, CreateOptions(), out IReadOnlyList<CurvatureTransition> transitions, out string reason))
                throw new InvalidOperationException(reason);
            return transitions;
        }

        private static DetectionTestSnapshot Snapshot(IReadOnlyList<CurvatureTransition> transitions)
        {
            double maximum = 0d;
            for (int index = 0; index < transitions.Count; index++) maximum = Math.Max(maximum, transitions[index].CurvatureJumpPerMeter);
            return new DetectionTestSnapshot(transitions.Count, maximum, 0, 0, 0d, 0d, 0d);
        }

        private static PathSmoothingRequest CreateRequest(IReadOnlyList<CoarsePathPoint> points, IReadOnlyList<PathSegment> segments)
        {
            return new PathSmoothingRequest(points, segments, null, null, new PathSmoothingConfiguration());
        }

        private static CoarsePathPoint Point(double arcLength, double curvature, TravelDirection direction = TravelDirection.Forward, bool gearSwitch = false)
        {
            return new CoarsePathPoint(arcLength, 0d, 0d, 0d, arcLength, direction, curvature, 1d, gearSwitch,
                CoarsePathPointSource.MotionPrimitive);
        }

        private static LocalG2OptionsSnapshot CreateOptions() => new LocalG2OptionsSnapshot(new PathSmoothingConfiguration());

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

    private static bool ValidatePath(IReadOnlyList<CoarsePathPoint> path, out string reason)
    {
        reason = string.Empty;
        if (path.Count == 0)
        {
            reason = "局部 G2 曲率事件检测需要粗路径点。";
            return false;
        }
        double previousArc = -1d;
        for (int index = 0; index < path.Count; index++)
        {
            CoarsePathPoint point = path[index];
            if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
                !NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
                !NumericGuard.IsFinite(point.VehicleCurvature) || point.ArcLength < previousArc)
            {
                reason = "局部 G2 曲率事件检测要求有限且非递减的粗路径。";
                return false;
            }
            previousArc = point.ArcLength;
        }
        return true;
    }

    private static bool ValidateSegments(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, out string reason)
    {
        reason = string.Empty;
        for (int position = 0; position < segments.Count; position++)
        {
            PathSegment segment = segments[position];
            if (segment == null || segment.SegmentIndex != position || segment.StartIndex < 0 ||
                segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count)
            {
                reason = "局部 G2 曲率事件检测的方向分段无效。";
                return false;
            }
            for (int index = segment.StartIndex; index <= segment.EndIndex; index++)
            {
                if (path[index].Direction != segment.Direction)
                {
                    reason = "局部 G2 曲率事件检测的方向分段包含换向点。";
                    return false;
                }
            }
        }
        return true;
    }

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