using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>对一个局部 G2 替换候选执行区域质量门和完整路径安全复核。</summary>
internal sealed class LocalG2CandidateEvaluator
{
    private const double CurvatureRangeTolerance = 1e-6d;
    private const double WindowPointToleranceMeters = 1e-10d;
    private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
        new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
    private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
        new ReadOnlyCollection<SmoothedPathSegment>(new List<SmoothedPathSegment>());
    private readonly PathGeometryAnalyzer _analyzer;
    private readonly LocalG2PathSplicer _splicer;
    private readonly SmoothedPathValidator _validator;

    internal LocalG2CandidateEvaluator()
        : this(new PathGeometryAnalyzer(), new LocalG2PathSplicer(), new SmoothedPathValidator())
    {
    }

    internal LocalG2CandidateEvaluator(
        PathGeometryAnalyzer analyzer,
        LocalG2PathSplicer splicer,
        SmoothedPathValidator validator)
    {
        _analyzer = analyzer ?? throw new ArgumentNullException(nameof(analyzer));
        _splicer = splicer ?? throw new ArgumentNullException(nameof(splicer));
        _validator = validator ?? throw new ArgumentNullException(nameof(validator));
    }

    internal LocalG2CandidateEvaluation Evaluate(
        PreparedPath rawPath,
        PreparedPath currentPath,
        LocalG2SmoothingRegion region,
        LocalG2CandidateGeometry candidate,
        PathSmoothingRequest request,
        LocalG2OptionsSnapshot options,
        CancellationToken cancellationToken)
    {
        cancellationToken.ThrowIfCancellationRequested();
        int candidateIndex = candidate == null ? -1 : candidate.CandidateIndex;
        if (!HasUsableInput(rawPath, currentPath, region, candidate, request, options))
            return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "局部 G2 候选评价输入无效。");

        PreparedDirectionSegment currentSegment = currentPath.Segments[candidate.SegmentIndex];
        if (!TryExtractWindow(currentSegment, candidate.StartArcLengthMeters, candidate.EndArcLengthMeters,
                out IReadOnlyList<SmoothingPoint2D> rawWindow, out string reason) ||
            !TryAnalyzeWindow(currentSegment, rawWindow, request.Configuration.OutputSpacingMeters,
                out PathGeometryAnalysis rawAnalysis, out reason) ||
            !TryAnalyzeWindow(currentSegment, candidate.RegionPoints, request.Configuration.OutputSpacingMeters,
                out PathGeometryAnalysis candidateAnalysis, out reason) ||
            !HasFiniteMetrics(rawAnalysis) || !HasFiniteMetrics(candidateAnalysis))
        {
            return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
                string.IsNullOrEmpty(reason) ? "局部 G2 区域几何分析失败。" : reason);
        }

        if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double vehicleMaximumCurvature))
            return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "车辆曲率约束无效。");
        if (candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter > vehicleMaximumCurvature + CurvatureRangeTolerance)
            return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureExceeded, "局部 G2 候选超过车辆曲率上限。");

        GetCurvatureRange(rawAnalysis.Path, out double rawMinimumCurvature, out double rawMaximumCurvature);
        if (ExceedsRawCurvatureRange(candidateAnalysis.Path, rawMinimumCurvature, rawMaximumCurvature))
            return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureOvershoot, "局部 G2 候选超出原始区域曲率范围。");

        double maximumDeviation = MaximumDistanceToPolyline(candidate.RegionPoints, rawWindow);
        if (!NumericGuard.IsFinite(maximumDeviation))
            return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "局部 G2 候选偏差计算失败。");
        if (maximumDeviation > options.MaximumDeviationMeters)
            return Rejected(candidateIndex, PathSmoothingRegionFailureReason.DeviationExceeded, "局部 G2 候选偏离原始窗口过远。");

        if (!_splicer.TryReplace(currentPath, candidate, out PreparedPath spliced, out reason) ||
            !_analyzer.TryAnalyze(spliced.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis fullAnalysis, out reason) ||
            !HasFiniteMetrics(fullAnalysis))
        {
            return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
                string.IsNullOrEmpty(reason) ? "局部 G2 完整路径几何分析失败。" : reason);
        }

        if (!_validator.TryValidate(fullAnalysis.Path, fullAnalysis.Segments, rawPath, request.Map, request.Vehicle,
                request.Configuration.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
                out double minimumClearance, out reason))
        {
            return Rejected(candidateIndex, PathSmoothingRegionFailureReason.Collision,
                string.IsNullOrEmpty(reason) ? "局部 G2 完整路径安全复核失败。" : reason);
        }
        if (!NumericGuard.IsFinite(minimumClearance) || minimumClearance < request.Configuration.MinimumClearanceReserveMeters)
            return new LocalG2CandidateEvaluation(false, PathSmoothingRegionFailureReason.InsufficientClearance, candidateIndex,
                null, EmptyPath, EmptySegments, rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
                candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter, rawAnalysis.CurvatureVariationCost,
                candidateAnalysis.CurvatureVariationCost, maximumDeviation, minimumClearance,
                candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter, 0d, "局部 G2 候选净空不足。");

        if (candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter >
            rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter *
            (1d - options.MinimumPeakGradientImprovementRatio))
        {
            return new LocalG2CandidateEvaluation(false, PathSmoothingRegionFailureReason.InsufficientImprovement, candidateIndex,
                null, EmptyPath, EmptySegments, rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
                candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter, rawAnalysis.CurvatureVariationCost,
                candidateAnalysis.CurvatureVariationCost, maximumDeviation, minimumClearance,
                candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter, 0d, "局部 G2 候选曲率导数峰值改善不足。");
        }
        if (candidateAnalysis.CurvatureVariationCost > rawAnalysis.CurvatureVariationCost *
            (1d + options.MaximumVariationCostRegressionRatio))
        {
            return Rejected(candidateIndex, PathSmoothingRegionFailureReason.VariationCostRegression, "局部 G2 候选曲率变化代价回退。");
        }

        if (!_analyzer.TryAnalyze(rawPath.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis rawFullAnalysis, out reason) ||
            !HasFiniteMetrics(rawFullAnalysis))
        {
            return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
                string.IsNullOrEmpty(reason) ? "原始完整路径几何分析失败。" : reason);
        }

        return new LocalG2CandidateEvaluation(
            true,
            PathSmoothingRegionFailureReason.None,
            candidateIndex,
            spliced,
            safePath,
            fullAnalysis.Segments,
            rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
            candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
            rawAnalysis.CurvatureVariationCost,
            candidateAnalysis.CurvatureVariationCost,
            maximumDeviation,
            minimumClearance,
            candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter,
            Math.Abs(fullAnalysis.PathLengthMeters - rawFullAnalysis.PathLengthMeters),
            "Accepted");
    }

    internal static LocalG2CandidateEvaluation SelectBest(IReadOnlyList<LocalG2CandidateEvaluation> evaluations)
    {
        if (evaluations == null) throw new ArgumentNullException(nameof(evaluations));
        LocalG2CandidateEvaluation best = null;
        for (int index = 0; index < evaluations.Count; index++)
        {
            LocalG2CandidateEvaluation evaluation = evaluations[index];
            if (evaluation == null || !evaluation.Accepted) continue;
            if (best == null || Compare(evaluation, best) < 0) best = evaluation;
        }
        return best ?? Rejected(-1, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "没有通过质量门的局部 G2 候选。");
    }

    private static int Compare(LocalG2CandidateEvaluation left, LocalG2CandidateEvaluation right)
    {
        int result = left.MaximumDeviationMeters.CompareTo(right.MaximumDeviationMeters);
        if (result != 0) return result;
        result = left.ResultPeakCurvatureDerivativePerSquareMeter.CompareTo(right.ResultPeakCurvatureDerivativePerSquareMeter);
        if (result != 0) return result;
        result = left.ResultCurvatureVariationCost.CompareTo(right.ResultCurvatureVariationCost);
        if (result != 0) return result;
        result = left.AbsolutePathLengthChangeMeters.CompareTo(right.AbsolutePathLengthChangeMeters);
        return result != 0 ? result : left.CandidateIndex.CompareTo(right.CandidateIndex);
    }

    private static bool HasUsableInput(PreparedPath rawPath, PreparedPath currentPath, LocalG2SmoothingRegion region,
        LocalG2CandidateGeometry candidate, PathSmoothingRequest request, LocalG2OptionsSnapshot options)
    {
        return rawPath != null && currentPath != null && region != null && candidate != null && request != null && options != null &&
            request.Configuration != null && request.Map != null && request.Vehicle != null && candidate.SegmentIndex == region.SegmentIndex &&
            candidate.SegmentIndex >= 0 && candidate.SegmentIndex < currentPath.Segments.Count &&
            NumericGuard.IsPositiveFinite(request.Configuration.OutputSpacingMeters) &&
            NumericGuard.IsPositiveFinite(request.Configuration.MaximumCollisionCheckStepMeters) &&
            NumericGuard.IsFinite(request.Configuration.MinimumClearanceReserveMeters) &&
            request.Configuration.MinimumClearanceReserveMeters >= 0d;
    }

    private static bool TryExtractWindow(PreparedDirectionSegment segment, double startArcLength, double endArcLength,
        out IReadOnlyList<SmoothingPoint2D> window, out string reason)
    {
        window = null;
        reason = string.Empty;
        if (segment == null || !PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, startArcLength, out SmoothingPoint2D start, out reason) ||
            !PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, endArcLength, out SmoothingPoint2D end, out reason)) return false;
        var points = new List<SmoothingPoint2D> { start };
        for (int index = 0; index < segment.Points.Count; index++)
        {
            SmoothingPoint2D point = segment.Points[index];
            if (point.ArcLength > startArcLength && point.ArcLength < endArcLength &&
                !SamePosition(points[points.Count - 1], point))
            {
                points.Add(point);
            }
        }
        if (SamePosition(points[points.Count - 1], end))
            points[points.Count - 1] = end;
        else
            points.Add(end);
        window = new ReadOnlyCollection<SmoothingPoint2D>(points);
        return true;
    }

    private static bool SamePosition(SmoothingPoint2D left, SmoothingPoint2D right)
    {
        if (left == null || right == null) return false;
        double x = right.X - left.X;
        double y = right.Y - left.Y;
        return x * x + y * y <= WindowPointToleranceMeters * WindowPointToleranceMeters;
    }

    private bool TryAnalyzeWindow(PreparedDirectionSegment source, IReadOnlyList<SmoothingPoint2D> points, double spacing,
        out PathGeometryAnalysis analysis, out string reason)
    {
        var segment = new PreparedDirectionSegment(0, source.Direction, points, false, false,
            source.Points[0].ArcLength == points[0].ArcLength ? source.StartVehicleCurvaturePerMeter : null);
        return _analyzer.TryAnalyze(new[] { segment }, spacing, out analysis, out reason);
    }

    private static bool HasFiniteMetrics(PathGeometryAnalysis analysis)
    {
        return analysis != null && NumericGuard.IsFinite(analysis.PathLengthMeters) &&
            NumericGuard.IsFinite(analysis.MaximumAbsoluteVehicleCurvaturePerMeter) &&
            NumericGuard.IsFinite(analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter) &&
            NumericGuard.IsFinite(analysis.CurvatureVariationCost) && analysis.Path != null && analysis.Path.Count >= 2;
    }

    private static void GetCurvatureRange(IReadOnlyList<SmoothedPathPoint> path, out double minimum, out double maximum)
    {
        minimum = double.PositiveInfinity;
        maximum = double.NegativeInfinity;
        for (int index = 0; index < path.Count; index++)
        {
            minimum = Math.Min(minimum, path[index].VehicleCurvature);
            maximum = Math.Max(maximum, path[index].VehicleCurvature);
        }
    }

    private static bool ExceedsRawCurvatureRange(IReadOnlyList<SmoothedPathPoint> candidate, double minimum, double maximum)
    {
        for (int index = 0; index < candidate.Count; index++)
        {
            double curvature = candidate[index].VehicleCurvature;
            if (curvature < minimum - CurvatureRangeTolerance || curvature > maximum + CurvatureRangeTolerance) return true;
        }
        return false;
    }

    private static double MaximumDistanceToPolyline(IReadOnlyList<SmoothingPoint2D> candidate, IReadOnlyList<SmoothingPoint2D> raw)
    {
        if (candidate == null || raw == null || candidate.Count == 0 || raw.Count < 2) return double.NaN;
        double maximum = 0d;
        for (int index = 0; index < candidate.Count; index++)
        {
            double nearest = double.PositiveInfinity;
            for (int segment = 1; segment < raw.Count; segment++)
                nearest = Math.Min(nearest, PointToSegmentDistance(candidate[index], raw[segment - 1], raw[segment]));
            maximum = Math.Max(maximum, nearest);
        }
        return maximum;
    }

    private static double PointToSegmentDistance(SmoothingPoint2D point, SmoothingPoint2D start, SmoothingPoint2D end)
    {
        double dx = end.X - start.X;
        double dy = end.Y - start.Y;
        double lengthSquared = dx * dx + dy * dy;
        if (!NumericGuard.IsPositiveFinite(lengthSquared)) return double.NaN;
        double projection = ((point.X - start.X) * dx + (point.Y - start.Y) * dy) / lengthSquared;
        projection = Math.Max(0d, Math.Min(1d, projection));
        double nearestX = start.X + projection * dx;
        double nearestY = start.Y + projection * dy;
        double distanceX = point.X - nearestX;
        double distanceY = point.Y - nearestY;
        return Math.Sqrt(distanceX * distanceX + distanceY * distanceY);
    }

    private static LocalG2CandidateEvaluation Rejected(int candidateIndex, PathSmoothingRegionFailureReason failureReason, string reason)
    {
        return new LocalG2CandidateEvaluation(false, failureReason, candidateIndex, null, EmptyPath, EmptySegments,
            0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, reason);
    }

    /// <summary>反射脚本使用的窄范围确定性质量门覆盖入口。</summary>
    public static class TestHooks
    {
        public static EvaluationTestSnapshot Execute(string scenario)
        {
            LocalG2CandidateEvaluation evaluation;
            switch (scenario)
            {
                case "TooFar":
                    PreparedPath tooFarPath = CreateWavyPath(1d);
                    evaluation = Evaluate(tooFarPath, CreateCandidate(tooFarPath, 0, 1.12d), 0d);
                    break;
                case "Overshoot":
                    PreparedPath overshootPath = CreateStraightPath(1d);
                    evaluation = Evaluate(overshootPath, CreateCandidate(overshootPath, 0, 1.15d), 0d);
                    break;
                case "NoOp":
                    PreparedPath noOpPath = CreateWavyPath(1d);
                    evaluation = Evaluate(noOpPath, CreateCandidateFromPath(noOpPath, 0), 0d);
                    break;
                case "Oscillating":
                    PreparedPath oscillatingPath = CreateOscillationBaseline(1d);
                    evaluation = Evaluate(oscillatingPath, CreateOscillatingCandidate(oscillatingPath), 0d);
                    break;
                case "LowClearance":
                    PreparedPath lowClearancePath = CreateWavyPath(0.27d);
                    evaluation = Evaluate(lowClearancePath, CreateCandidateFromPath(lowClearancePath, 0), 0.02d, false, true);
                    break;
                case "Improved":
                    PreparedPath improvedPath = CreateWavyPath(1d);
                    evaluation = Evaluate(improvedPath, CreateCandidate(improvedPath, 0, 1d), 0d);
                    break;
                case "SmallestDeviation":
                    PreparedPath smallestDeviationPath = CreateWavyPath(1d);
                    evaluation = Evaluate(smallestDeviationPath, CreateCandidate(smallestDeviationPath, 4, 1d), 100d);
                    break;
                case "Best": evaluation = SelectBest(CreateWavyPath(1d)); break;
                default: throw new ArgumentOutOfRangeException(nameof(scenario));
            }
            return new EvaluationTestSnapshot(evaluation.Accepted ? "Accepted" : "Rejected", evaluation.FailureReason.ToString(), evaluation.CandidateIndex,
                evaluation.MinimumBodyClearanceMeters, evaluation.Reason);
        }

        private static LocalG2CandidateEvaluation Evaluate(PreparedPath path, LocalG2CandidateGeometry candidate, double minimumClearance,
            bool useNearObstacle = false, bool useEmptyMap = false)
        {
            PathSmoothingConfiguration configuration = CreateConfiguration(minimumClearance);
            PathSmoothingRequest request = new PathSmoothingRequest(null, null,
                useEmptyMap ? CreateEmptyMap() : CreateMap(useNearObstacle), CreateVehicle(), configuration);
            return new LocalG2CandidateEvaluator().Evaluate(path, path, CreateRegion(), candidate, request,
                new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
        }

        private static LocalG2CandidateEvaluation SelectBest(PreparedPath path)
        {
            PathSmoothingConfiguration configuration = CreateConfiguration(0d);
            PathSmoothingRequest request = new PathSmoothingRequest(null, null, CreateMap(false), CreateVehicle(), configuration);
            var evaluator = new LocalG2CandidateEvaluator();
            LocalG2CandidateEvaluation smallestDeviation = evaluator.Evaluate(path, path, CreateRegion(),
                CreateCandidate(path, 4, 1d), request, new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
            LocalG2CandidateEvaluation smootherButFarther = evaluator.Evaluate(path, path, CreateRegion(),
                CreateCandidate(path, 5, 1.01d), request, new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
            return LocalG2CandidateEvaluator.SelectBest(new[] { smootherButFarther, smallestDeviation });
        }

        private static PreparedPath CreateWavyPath(double startX)
        {
            return CreatePath(new[]
            {
                Point(startX, 1d), Point(startX + 0.10d, 1.04d), Point(startX + 0.20d, 1.08d),
                Point(startX + 0.30d, 1.05d), Point(startX + 0.40d, 0.98d), Point(startX + 0.50d, 0.92d),
                Point(startX + 0.60d, 0.98d), Point(startX + 0.70d, 1.05d), Point(startX + 0.80d, 1.08d),
                Point(startX + 0.90d, 1.04d), Point(startX + 1d, 1d),
            });
        }

        private static PreparedPath CreateStraightPath(double startX) => CreatePath(new[] { Point(startX, 1d), Point(startX + 0.5d, 1d), Point(startX + 1d, 1d) });

        private static PreparedPath CreateOscillationBaseline(double startX)
        {
            return CreatePath(new[]
            {
                Point(startX, 1d), Point(startX + 0.05d, 1.022d), Point(startX + 0.10d, 1d),
                Point(startX + 2.03d, 1d),
            });
        }

        private static PreparedPath CreatePath(IReadOnlyList<SmoothingPoint2D> points)
        {
            var normalized = new List<SmoothingPoint2D>(points.Count);
            double arcLength = 0d;
            for (int index = 0; index < points.Count; index++)
            {
                if (index > 0)
                {
                    double dx = points[index].X - points[index - 1].X;
                    double dy = points[index].Y - points[index - 1].Y;
                    arcLength += Math.Sqrt(dx * dx + dy * dy);
                }
                normalized.Add(new SmoothingPoint2D(points[index].X, points[index].Y, arcLength, 0d, 0d, 1d,
                    false, SmoothedPathPointSource.LocalG2Transition));
            }
            return new PreparedPath(new[] { new PreparedDirectionSegment(0, TravelDirection.Forward, normalized, false, false) });
        }

        private static LocalG2CandidateGeometry CreateCandidate(PreparedPath path, int index, double middleY)
        {
            IReadOnlyList<SmoothingPoint2D> source = path.Segments[0].Points;
            SmoothingPoint2D start = source[0];
            SmoothingPoint2D end = source[source.Count - 1];
            return new LocalG2CandidateGeometry(index, 0, start.ArcLength, end.ArcLength, 0d, 0d,
                new[] { Point(start.X, start.Y), Point((start.X + end.X) / 2d, middleY), Point(end.X, end.Y) },
                0d, 0d, 0d, 0d, true);
        }

        private static LocalG2CandidateGeometry CreateCandidateFromPath(PreparedPath path, int index)
        {
            IReadOnlyList<SmoothingPoint2D> points = path.Segments[0].Points;
            return new LocalG2CandidateGeometry(index, 0, points[0].ArcLength, points[points.Count - 1].ArcLength, 0d, 0d, points,
                0d, 0d, 0d, 0d, true);
        }

        private static LocalG2CandidateGeometry CreateOscillatingCandidate(PreparedPath path)
        {
            IReadOnlyList<SmoothingPoint2D> source = path.Segments[0].Points;
            SmoothingPoint2D start = source[0];
            SmoothingPoint2D end = source[source.Count - 1];
            var points = new List<SmoothingPoint2D> { Point(start.X, start.Y) };
            SmoothingPoint2D straightStart = source[source.Count - 2];
            points.Add(Point(straightStart.X, straightStart.Y));
            for (int index = 1; index < 20; index++)
            {
                double x = straightStart.X + index * (end.X - straightStart.X) / 20d;
                points.Add(Point(x, straightStart.Y + (index % 2 == 0 ? 0.004d : -0.004d)));
            }
            points.Add(Point(end.X, end.Y));
            return new LocalG2CandidateGeometry(0, 0, start.ArcLength, end.ArcLength, 0d, 0d, points, 0d, 0d, 0d, 0d, true);
        }

        private static LocalG2SmoothingRegion CreateRegion()
        {
            return new LocalG2SmoothingRegion(0,
                new[] { new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, 0d, 0d, 0d) },
                0d, 1d, new[] { new LocalG2WindowVariant(0, 0d, 1d, 0d, 0d) });
        }

        private static PathSmoothingConfiguration CreateConfiguration(double minimumClearance)
        {
            var configuration = new PathSmoothingConfiguration
            {
                OutputSpacingMeters = 0.05d,
                MaximumCollisionCheckStepMeters = 0.05d,
                MinimumClearanceReserveMeters = minimumClearance,
            };
            configuration.LocalG2Quintic.MaximumDeviationMeters = 0.10d;
            configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.20d;
            configuration.LocalG2Quintic.MaximumVariationCostRegressionRatio = 0.02d;
            return configuration;
        }

        private static VehicleParameters CreateVehicle() => new VehicleParameters
        {
            LengthMeters = 0.20d, WidthMeters = 0.20d, SafetyMarginMeters = 0d,
            MaximumCurvaturePerMeter = 1000000d, MinimumTurningRadiusMeters = 0.000001d,
        };

        private static PlanningGridMap CreateMap(bool useNearObstacle)
        {
            var request = new PlanningMapRequest
            {
                Bounds = new MapBoundsMm(0f, 4000f, 0f, 4000f), ResolutionMm = 20f,
                ObstacleSources = new IMapObstacleSource[]
                {
                    new ManualObstacleSource("local-g2-evaluator", 1, true, new IMapObstacle[]
                    {
                        useNearObstacle
                            ? new AxisAlignedRectangleObstacle(100f, 150f, 900f, 1100f)
                            : new AxisAlignedRectangleObstacle(3000f, 3100f, 3000f, 3100f),
                    }),
                },
            };
            PlanningMapBuildResult result = new PlanningMapFactory().Create(request);
            if (!result.Succeeded || result.Map == null) throw new InvalidOperationException("测试地图创建失败。");
            return result.Map;
        }

        private static PlanningGridMap CreateEmptyMap()
        {
            var request = new PlanningMapRequest
            {
                Bounds = new MapBoundsMm(0f, 4000f, 0f, 4000f), ResolutionMm = 20f,
                ObstacleSources = Array.Empty<IMapObstacleSource>(), AllowExplicitEmptyMap = true,
            };
            PlanningMapBuildResult result = new PlanningMapFactory().Create(request);
            if (!result.Succeeded || result.Map == null) throw new InvalidOperationException("测试空地图创建失败。");
            return result.Map;
        }

        private static SmoothingPoint2D Point(double x, double y) =>
            new SmoothingPoint2D(x, y, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.LocalG2Transition);

        public sealed class EvaluationTestSnapshot
        {
            internal EvaluationTestSnapshot(string status, string failureReason, int candidateIndex, double minimumClearanceMeters, string reason)
            {
                Status = status;
                FailureReason = failureReason;
                CandidateIndex = candidateIndex;
                MinimumClearanceMeters = minimumClearanceMeters;
                Reason = reason;
            }
            public string Status { get; }
            public string FailureReason { get; }
            public int CandidateIndex { get; }
            public double MinimumClearanceMeters { get; }
            public string Reason { get; }
        }
    }
}

/// <summary>不可变的局部 G2 候选质量与安全评价结果。</summary>
internal sealed class LocalG2CandidateEvaluation
{
    internal LocalG2CandidateEvaluation(bool accepted, PathSmoothingRegionFailureReason failureReason, int candidateIndex,
        PreparedPath splicedPreparedPath, IReadOnlyList<SmoothedPathPoint> safePath, IReadOnlyList<SmoothedPathSegment> safeSegments,
        double rawPeakCurvatureDerivativePerSquareMeter, double resultPeakCurvatureDerivativePerSquareMeter,
        double rawCurvatureVariationCost, double resultCurvatureVariationCost, double maximumDeviationMeters,
        double minimumBodyClearanceMeters, double maximumAbsoluteVehicleCurvaturePerMeter,
        double absolutePathLengthChangeMeters, string reason)
    {
        Accepted = accepted;
        FailureReason = failureReason;
        CandidateIndex = candidateIndex;
        SplicedPreparedPath = splicedPreparedPath;
        SafePath = Copy(safePath);
        SafeSegments = Copy(safeSegments);
        RawPeakCurvatureDerivativePerSquareMeter = rawPeakCurvatureDerivativePerSquareMeter;
        ResultPeakCurvatureDerivativePerSquareMeter = resultPeakCurvatureDerivativePerSquareMeter;
        RawCurvatureVariationCost = rawCurvatureVariationCost;
        ResultCurvatureVariationCost = resultCurvatureVariationCost;
        MaximumDeviationMeters = maximumDeviationMeters;
        MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
        MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
        AbsolutePathLengthChangeMeters = absolutePathLengthChangeMeters;
        Reason = reason ?? string.Empty;
    }

    internal bool Accepted { get; }
    internal PathSmoothingRegionFailureReason FailureReason { get; }
    internal int CandidateIndex { get; }
    internal PreparedPath SplicedPreparedPath { get; }
    internal IReadOnlyList<SmoothedPathPoint> SafePath { get; }
    internal IReadOnlyList<SmoothedPathSegment> SafeSegments { get; }
    internal double RawPeakCurvatureDerivativePerSquareMeter { get; }
    internal double ResultPeakCurvatureDerivativePerSquareMeter { get; }
    internal double RawCurvatureVariationCost { get; }
    internal double ResultCurvatureVariationCost { get; }
    internal double MaximumDeviationMeters { get; }
    internal double MinimumBodyClearanceMeters { get; }
    internal double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
    internal double AbsolutePathLengthChangeMeters { get; }
    internal string Reason { get; }

    private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
    {
        var copy = new List<T>(source == null ? 0 : source.Count);
        if (source != null) for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
        return new ReadOnlyCollection<T>(copy);
    }
}