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

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>以固定窗口和尺度顺序构造有限的局部五次 Hermite G2 候选。</summary>
internal sealed class LocalG2CandidateBuilder
{
    private const double MinimumDerivativeNorm = 1e-10d;
    private const int MaximumDerivativeCertificationDepth = 40;
    private const int MaximumDerivativeCertificationIntervals = 8192;
    private const double DerivativeCertificationMargin = 1e-12d;
    private const int MaximumSubdivisionDepth = 32;
    private static readonly double[] DerivativeScaleMultipliers = { 1d, 0.85d, 1.15d };

    internal IReadOnlyList<LocalG2CandidateGeometry> Build(
        PreparedDirectionSegment originalSegment,
        LocalG2SmoothingRegion region,
        double outputSpacingMeters,
        LocalG2OptionsSnapshot options,
        CancellationToken cancellationToken)
    {
        if (!IsValidInput(originalSegment, region, outputSpacingMeters, options)) return Empty();

        var candidates = new List<LocalG2CandidateGeometry>();
        for (int windowIndex = 0; windowIndex < region.WindowVariants.Count; windowIndex++)
        {
            LocalG2WindowVariant window = region.WindowVariants[windowIndex];
            for (int scaleIndex = 0; scaleIndex < DerivativeScaleMultipliers.Length; scaleIndex++)
            {
                cancellationToken.ThrowIfCancellationRequested();
                if (candidates.Count >= Math.Min(options.MaximumCandidatesPerRegion, 12)) return ReadOnly(candidates);
                if (TryBuildCandidate(
                        candidates.Count,
                        originalSegment,
                        region,
                        window,
                        DerivativeScaleMultipliers[scaleIndex],
                        outputSpacingMeters,
                        cancellationToken,
                        out LocalG2CandidateGeometry candidate))
                {
                    candidates.Add(candidate);
                }
            }
        }
        return ReadOnly(candidates);
    }

    private static bool TryBuildCandidate(
        int candidateIndex,
        PreparedDirectionSegment segment,
        LocalG2SmoothingRegion region,
        LocalG2WindowVariant window,
        double multiplier,
        double outputSpacingMeters,
        CancellationToken cancellationToken,
        out LocalG2CandidateGeometry candidate)
    {
        candidate = null;
        if (!PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, window.StartArcLengthMeters, out SmoothingPoint2D startReference, out _) ||
            !PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, window.EndArcLengthMeters, out SmoothingPoint2D endReference, out _))
        {
            return false;
        }

        double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
        CurvatureTransition first = region.Transitions[0];
        CurvatureTransition last = region.Transitions[region.Transitions.Count - 1];
        bool firstAtStart = SameArc(first.LocalArcLengthMeters, window.StartArcLengthMeters);
        bool lastAtEnd = SameArc(last.LocalArcLengthMeters, window.EndArcLengthMeters);
        double startVehicleCurvature = firstAtStart && segment.StartVehicleCurvaturePerMeter.HasValue
            ? segment.StartVehicleCurvaturePerMeter.Value
            : first.LeftVehicleCurvaturePerMeter;
        double endVehicleCurvature = last.RightVehicleCurvaturePerMeter;
        if (!NumericGuard.IsFinite(startVehicleCurvature) || !NumericGuard.IsFinite(endVehicleCurvature)) return false;

        var nodes = new List<BoundaryNode>();
        nodes.Add(new BoundaryNode(window.StartArcLengthMeters, startReference.X, startReference.Y,
            startReference.Heading, startVehicleCurvature));
        for (int transitionIndex = 0; transitionIndex < region.Transitions.Count; transitionIndex++)
        {
            CurvatureTransition transition = region.Transitions[transitionIndex];
            bool atStart = SameArc(transition.LocalArcLengthMeters, window.StartArcLengthMeters);
            bool atEnd = SameArc(transition.LocalArcLengthMeters, window.EndArcLengthMeters);
            // A one-sided transition at a hard segment boundary has no left/right zero-length piece.
            if (atStart || atEnd) continue;
            double leftAvailable = transition.LocalArcLengthMeters - window.StartArcLengthMeters;
            double rightAvailable = window.EndArcLengthMeters - transition.LocalArcLengthMeters;
            double denominator = leftAvailable + rightAvailable;
            if (!NumericGuard.IsPositiveFinite(denominator)) return false;
            double sharedCurvature = transition.LeftVehicleCurvaturePerMeter +
                (transition.RightVehicleCurvaturePerMeter - transition.LeftVehicleCurvaturePerMeter) * leftAvailable / denominator;
            if (!NumericGuard.IsFinite(sharedCurvature)) return false;
            nodes.Add(new BoundaryNode(transition.LocalArcLengthMeters, transition.X, transition.Y,
                transition.VehicleHeadingRadians, sharedCurvature));
        }
        nodes.Add(new BoundaryNode(window.EndArcLengthMeters, endReference.X, endReference.Y,
            endReference.Heading, endVehicleCurvature));

        if (!TryAssignDerivativeScales(nodes, multiplier) || nodes.Count < 2) return false;
        var sampled = new List<SmoothingPoint2D>();
        for (int nodeIndex = 1; nodeIndex < nodes.Count; nodeIndex++)
        {
            cancellationToken.ThrowIfCancellationRequested();
            BoundaryNode left = nodes[nodeIndex - 1];
            BoundaryNode right = nodes[nodeIndex];
            if (!TryCreateCurve(left, right, directionSign, out QuinticHermiteCurve2D curve)) return false;
            if (!TryAppendCurveSamples(
                    curve,
                    left,
                    right,
                    segment,
                    outputSpacingMeters,
                    sampled,
                    cancellationToken))
            {
                return false;
            }
        }

        if (sampled.Count < 2) return false;
        candidate = new LocalG2CandidateGeometry(
            candidateIndex,
            segment.SegmentIndex,
            window.StartArcLengthMeters,
            window.EndArcLengthMeters,
            window.LeftWindowLengthMeters,
            window.RightWindowLengthMeters,
            sampled,
            startVehicleCurvature,
            endVehicleCurvature,
            directionSign * startVehicleCurvature,
            directionSign * endVehicleCurvature,
            true);
        return true;
    }

    private static bool TryAssignDerivativeScales(List<BoundaryNode> nodes, double multiplier)
    {
        if (!NumericGuard.IsPositiveFinite(multiplier)) return false;
        for (int index = 0; index < nodes.Count; index++)
        {
            double scale;
            if (index == 0) scale = nodes[1].ArcLengthMeters - nodes[0].ArcLengthMeters;
            else if (index == nodes.Count - 1) scale = nodes[index].ArcLengthMeters - nodes[index - 1].ArcLengthMeters;
            else scale = ((nodes[index].ArcLengthMeters - nodes[index - 1].ArcLengthMeters) +
                (nodes[index + 1].ArcLengthMeters - nodes[index].ArcLengthMeters)) / 2d;
            scale *= multiplier;
            if (!NumericGuard.IsPositiveFinite(scale)) return false;
            nodes[index] = nodes[index].WithDerivativeScale(scale);
        }
        return true;
    }

    private static bool TryCreateCurve(BoundaryNode left, BoundaryNode right, double directionSign, out QuinticHermiteCurve2D curve)
    {
        curve = null;
        if (!TryGetDerivatives(left, directionSign, out double ldx, out double ldy, out double lddx, out double lddy) ||
            !TryGetDerivatives(right, directionSign, out double rdx, out double rdy, out double rddx, out double rddy))
        {
            return false;
        }
        return QuinticHermiteCurve2D.TryCreate(left.X, left.Y, ldx, ldy, lddx, lddy,
            right.X, right.Y, rdx, rdy, rddx, rddy, out curve, out _);
    }

    private static bool TryGetDerivatives(BoundaryNode node, double directionSign,
        out double dx, out double dy, out double ddx, out double ddy)
    {
        dx = dy = ddx = ddy = 0d;
        double travelHeading = directionSign > 0d ? node.VehicleHeadingRadians : node.VehicleHeadingRadians - Math.PI;
        double geometricCurvature = directionSign * node.VehicleCurvaturePerMeter;
        double tx = Math.Cos(travelHeading);
        double ty = Math.Sin(travelHeading);
        double nx = -ty;
        double ny = tx;
        dx = node.DerivativeScale * tx;
        dy = node.DerivativeScale * ty;
        ddx = node.DerivativeScale * node.DerivativeScale * geometricCurvature * nx;
        ddy = node.DerivativeScale * node.DerivativeScale * geometricCurvature * ny;
        return NumericGuard.IsFinite(dx) && NumericGuard.IsFinite(dy) && NumericGuard.IsFinite(ddx) && NumericGuard.IsFinite(ddy) &&
            Math.Sqrt(dx * dx + dy * dy) >= MinimumDerivativeNorm;
    }

    private static bool TryAppendCurveSamples(
        QuinticHermiteCurve2D curve,
        BoundaryNode left,
        BoundaryNode right,
        PreparedDirectionSegment segment,
        double outputSpacingMeters,
        List<SmoothingPoint2D> output,
        CancellationToken cancellationToken)
    {
        // A short endpoint chord and a finite sample grid cannot rule out an interior cusp.
        // Certify a derivative-norm lower bound over the whole parameter interval first.
        if (!TryCertifyDerivativeLowerBound(curve)) return false;
        if (!TryEvaluate(curve, 0d, out CurveSample start) || !TryEvaluate(curve, 1d, out CurveSample end)) return false;
        if (output.Count == 0 && !TryAppendPoint(start, left, right, segment, output)) return false;
        return TrySubdivide(curve, left, right, segment, outputSpacingMeters, 0d, start, 1d, end, 0, output, cancellationToken);
    }

    private static bool TrySubdivide(
        QuinticHermiteCurve2D curve,
        BoundaryNode left,
        BoundaryNode right,
        PreparedDirectionSegment segment,
        double outputSpacingMeters,
        double u0,
        CurveSample sample0,
        double u1,
        CurveSample sample1,
        int depth,
        List<SmoothingPoint2D> output,
        CancellationToken cancellationToken)
    {
        cancellationToken.ThrowIfCancellationRequested();
        if (Distance(sample0, sample1) <= outputSpacingMeters)
        {
            return TryAppendPoint(sample1, left, right, segment, output);
        }
        if (depth >= MaximumSubdivisionDepth) return false;
        double middle = (u0 + u1) / 2d;
        if (!TryEvaluate(curve, middle, out CurveSample middleSample)) return false;
        return TrySubdivide(curve, left, right, segment, outputSpacingMeters, u0, sample0, middle, middleSample,
                depth + 1, output, cancellationToken) &&
            TrySubdivide(curve, left, right, segment, outputSpacingMeters, middle, middleSample, u1, sample1,
                depth + 1, output, cancellationToken);
    }

    private static bool TryAppendPoint(
        CurveSample sample,
        BoundaryNode left,
        BoundaryNode right,
        PreparedDirectionSegment segment,
        List<SmoothingPoint2D> output)
    {
        double referenceArcLength = left.ArcLengthMeters + (right.ArcLengthMeters - left.ArcLengthMeters) * sample.Parameter;
        if (!PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, referenceArcLength, out SmoothingPoint2D reference, out _))
            return false;
        double x = SameArc(sample.Parameter, 0d) ? left.X : SameArc(sample.Parameter, 1d) ? right.X : sample.X;
        double y = SameArc(sample.Parameter, 0d) ? left.Y : SameArc(sample.Parameter, 1d) ? right.Y : sample.Y;
        double travelHeading = Math.Atan2(sample.Dy, sample.Dx);
        double vehicleHeading = segment.Direction == TravelDirection.Forward ? travelHeading : travelHeading + Math.PI;
        if (!NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(travelHeading) || !NumericGuard.IsFinite(vehicleHeading)) return false;
        if (output.Count > 0 && Distance(output[output.Count - 1], x, y) <= 1e-12d) return false;
        output.Add(new SmoothingPoint2D(
            x,
            y,
            referenceArcLength,
            AngleMath.NormalizeRadians(vehicleHeading),
            vehicleHeading,
            reference.BodyClearance,
            false,
            SmoothedPathPointSource.LocalG2Transition));
        return true;
    }

    private static bool TryEvaluate(QuinticHermiteCurve2D curve, double parameter, out CurveSample sample)
    {
        sample = default;
        curve.Evaluate(parameter, out double x, out double y, out double dx, out double dy, out double ddx, out double ddy);
        double derivativeNorm = Math.Sqrt(dx * dx + dy * dy);
        if (!NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(dx) || !NumericGuard.IsFinite(dy) ||
            !NumericGuard.IsFinite(ddx) || !NumericGuard.IsFinite(ddy) || !NumericGuard.IsFinite(derivativeNorm) ||
            derivativeNorm < MinimumDerivativeNorm)
        {
            return false;
        }
        sample = new CurveSample(parameter, x, y, dx, dy);
        return true;
    }

    private static bool TryCertifyDerivativeLowerBound(QuinticHermiteCurve2D curve)
    {
        if (!TryCreateDerivativeBezierControls(curve, out DerivativeControlPoint d0, out DerivativeControlPoint d1,
                out DerivativeControlPoint d2, out DerivativeControlPoint d3, out DerivativeControlPoint d4))
        {
            return false;
        }
        int visitedIntervals = 0;
        return TryCertifyDerivativeLowerBound(d0, d1, d2, d3, d4, 0, ref visitedIntervals);
    }

    private static bool TryCreateDerivativeBezierControls(
        QuinticHermiteCurve2D curve,
        out DerivativeControlPoint d0,
        out DerivativeControlPoint d1,
        out DerivativeControlPoint d2,
        out DerivativeControlPoint d3,
        out DerivativeControlPoint d4)
    {
        d0 = d1 = d2 = d3 = d4 = default;
        curve.Evaluate(0d, out _, out _, out double dx0, out double dy0, out double ddx0, out double ddy0);
        curve.Evaluate(0.5d, out _, out _, out double dxMiddle, out double dyMiddle, out _, out _);
        curve.Evaluate(1d, out _, out _, out double dxEnd, out double dyEnd, out double ddxEnd, out double ddyEnd);
        d0 = new DerivativeControlPoint(dx0, dy0);
        d1 = new DerivativeControlPoint(dx0 + ddx0 / 4d, dy0 + ddy0 / 4d);
        d4 = new DerivativeControlPoint(dxEnd, dyEnd);
        d3 = new DerivativeControlPoint(dxEnd - ddxEnd / 4d, dyEnd - ddyEnd / 4d);
        d2 = new DerivativeControlPoint(
            (16d * dxMiddle - d0.X - 4d * d1.X - 4d * d3.X - d4.X) / 6d,
            (16d * dyMiddle - d0.Y - 4d * d1.Y - 4d * d3.Y - d4.Y) / 6d);
        return d0.IsFinite && d1.IsFinite && d2.IsFinite && d3.IsFinite && d4.IsFinite;
    }

    private static bool TryCertifyDerivativeLowerBound(
        DerivativeControlPoint d0,
        DerivativeControlPoint d1,
        DerivativeControlPoint d2,
        DerivativeControlPoint d3,
        DerivativeControlPoint d4,
        int depth,
        ref int visitedIntervals)
    {
        if (++visitedIntervals > MaximumDerivativeCertificationIntervals) return false;
        if (!TryGetOriginToConvexHullDistance(d0, d1, d2, d3, d4, out double lowerBound, out double margin))
            return false;
        if (lowerBound > MinimumDerivativeNorm + margin) return true;
        if (depth >= MaximumDerivativeCertificationDepth) return false;

        DerivativeControlPoint d01 = Midpoint(d0, d1);
        DerivativeControlPoint d12 = Midpoint(d1, d2);
        DerivativeControlPoint d23 = Midpoint(d2, d3);
        DerivativeControlPoint d34 = Midpoint(d3, d4);
        DerivativeControlPoint d012 = Midpoint(d01, d12);
        DerivativeControlPoint d123 = Midpoint(d12, d23);
        DerivativeControlPoint d234 = Midpoint(d23, d34);
        DerivativeControlPoint d0123 = Midpoint(d012, d123);
        DerivativeControlPoint d1234 = Midpoint(d123, d234);
        DerivativeControlPoint middle = Midpoint(d0123, d1234);
        if (!d01.IsFinite || !d12.IsFinite || !d23.IsFinite || !d34.IsFinite || !d012.IsFinite || !d123.IsFinite ||
            !d234.IsFinite || !d0123.IsFinite || !d1234.IsFinite || !middle.IsFinite)
        {
            return false;
        }

        return TryCertifyDerivativeLowerBound(d0, d01, d012, d0123, middle, depth + 1, ref visitedIntervals) &&
            TryCertifyDerivativeLowerBound(middle, d1234, d234, d34, d4, depth + 1, ref visitedIntervals);
    }

    private static bool TryGetOriginToConvexHullDistance(
        DerivativeControlPoint d0,
        DerivativeControlPoint d1,
        DerivativeControlPoint d2,
        DerivativeControlPoint d3,
        DerivativeControlPoint d4,
        out double distance,
        out double margin)
    {
        distance = 0d;
        margin = 0d;
        var controls = new[] { d0, d1, d2, d3, d4 };
        double maximumCoordinate = 0d;
        for (int index = 0; index < controls.Length; index++)
        {
            if (!controls[index].IsFinite) return false;
            maximumCoordinate = Math.Max(maximumCoordinate, Math.Max(Math.Abs(controls[index].X), Math.Abs(controls[index].Y)));
        }
        if (!NumericGuard.IsFinite(maximumCoordinate)) return false;
        margin = DerivativeCertificationMargin * Math.Max(1d, maximumCoordinate);

        for (int first = 0; first < controls.Length - 2; first++)
        {
            for (int second = first + 1; second < controls.Length - 1; second++)
            {
                for (int third = second + 1; third < controls.Length; third++)
                {
                    if (ContainsOrigin(controls[first], controls[second], controls[third], margin)) return true;
                }
            }
        }

        distance = double.PositiveInfinity;
        for (int first = 0; first < controls.Length; first++)
        {
            distance = Math.Min(distance, controls[first].Norm);
            for (int second = first + 1; second < controls.Length; second++)
                distance = Math.Min(distance, DistanceToSegment(controls[first], controls[second]));
        }
        return NumericGuard.IsFinite(distance);
    }

    private static bool ContainsOrigin(DerivativeControlPoint first, DerivativeControlPoint second,
        DerivativeControlPoint third, double margin)
    {
        double sideX = second.X - first.X;
        double sideY = second.Y - first.Y;
        double thirdOffsetX = third.X - first.X;
        double thirdOffsetY = third.Y - first.Y;
        double triangleArea = sideX * thirdOffsetY - sideY * thirdOffsetX;
        double crossFirstSecond = Cross(first, second);
        double crossSecondThird = Cross(second, third);
        double crossThirdFirst = Cross(third, first);
        if (!NumericGuard.IsFinite(triangleArea) || !NumericGuard.IsFinite(crossFirstSecond) ||
            !NumericGuard.IsFinite(crossSecondThird) || !NumericGuard.IsFinite(crossThirdFirst))
        {
            return true;
        }
        double areaMargin = margin * Math.Max(1d, Math.Max(first.Norm, Math.Max(second.Norm, third.Norm)));
        if (!NumericGuard.IsFinite(areaMargin)) return true;
        if (Math.Abs(triangleArea) <= areaMargin)
        {
            // A degenerate triangle is only a closed line segment (or a point), not a
            // two-dimensional region.  Treat it as origin-containing only when the origin
            // lies on one of its actual closed segments; otherwise its distance is positive.
            return IsOriginOnSegment(first, second, margin) || IsOriginOnSegment(second, third, margin) ||
                IsOriginOnSegment(third, first, margin);
        }
        return (crossFirstSecond >= -areaMargin && crossSecondThird >= -areaMargin && crossThirdFirst >= -areaMargin) ||
            (crossFirstSecond <= areaMargin && crossSecondThird <= areaMargin && crossThirdFirst <= areaMargin);
    }

    private static bool IsOriginOnSegment(DerivativeControlPoint start, DerivativeControlPoint end, double margin)
    {
        double dx = end.X - start.X;
        double dy = end.Y - start.Y;
        double lengthSquared = dx * dx + dy * dy;
        if (!NumericGuard.IsFinite(lengthSquared)) return true;
        if (lengthSquared == 0d) return start.Norm <= margin;
        double length = Math.Sqrt(lengthSquared);
        double cross = Cross(start, end);
        double projection = -(start.X * dx + start.Y * dy);
        if (!NumericGuard.IsFinite(length) || !NumericGuard.IsFinite(cross) || !NumericGuard.IsFinite(projection)) return true;
        double lineMargin = margin * Math.Max(1d, length);
        double projectionMargin = margin * Math.Max(1d, length);
        if (!NumericGuard.IsFinite(lineMargin) || !NumericGuard.IsFinite(projectionMargin)) return true;
        return Math.Abs(cross) <= lineMargin && projection >= -projectionMargin &&
            projection <= lengthSquared + projectionMargin;
    }

    private static double DistanceToSegment(DerivativeControlPoint start, DerivativeControlPoint end)
    {
        double dx = end.X - start.X;
        double dy = end.Y - start.Y;
        double denominator = dx * dx + dy * dy;
        if (!NumericGuard.IsFinite(denominator)) return double.NaN;
        if (denominator == 0d) return start.Norm;
        double parameter = -(start.X * dx + start.Y * dy) / denominator;
        if (!NumericGuard.IsFinite(parameter)) return double.NaN;
        parameter = Math.Max(0d, Math.Min(1d, parameter));
        double x = start.X + parameter * dx;
        double y = start.Y + parameter * dy;
        return Math.Sqrt(x * x + y * y);
    }

    private static DerivativeControlPoint Midpoint(DerivativeControlPoint left, DerivativeControlPoint right) =>
        new DerivativeControlPoint((left.X + right.X) / 2d, (left.Y + right.Y) / 2d);
    private static double Cross(DerivativeControlPoint left, DerivativeControlPoint right) => left.X * right.Y - left.Y * right.X;

    private static bool IsValidInput(PreparedDirectionSegment segment, LocalG2SmoothingRegion region,
        double outputSpacingMeters, LocalG2OptionsSnapshot options)
    {
        if (segment == null || region == null || options == null || segment.SegmentIndex != region.SegmentIndex ||
            !NumericGuard.IsPositiveFinite(outputSpacingMeters) || segment.Points == null || segment.Points.Count < 2 ||
            region.Transitions == null || region.Transitions.Count == 0 || region.WindowVariants == null)
        {
            return false;
        }
        for (int index = 0; index < region.Transitions.Count; index++)
        {
            CurvatureTransition transition = region.Transitions[index];
            if (transition == null || transition.SegmentIndex != segment.SegmentIndex) return false;
        }
        return true;
    }

    private static double Distance(CurveSample left, CurveSample right) => Distance(left.X, left.Y, right.X, right.Y);
    private static double Distance(SmoothingPoint2D point, double x, double y) => Distance(point.X, point.Y, x, y);
    private static double Distance(double x0, double y0, double x1, double y1)
    {
        double dx = x1 - x0;
        double dy = y1 - y0;
        return Math.Sqrt(dx * dx + dy * dy);
    }
    private static bool SameArc(double left, double right) => Math.Abs(left - right) <= 1e-10d;
    private static IReadOnlyList<LocalG2CandidateGeometry> Empty() => new ReadOnlyCollection<LocalG2CandidateGeometry>(new List<LocalG2CandidateGeometry>());
    private static IReadOnlyList<LocalG2CandidateGeometry> ReadOnly(List<LocalG2CandidateGeometry> values) =>
        new ReadOnlyCollection<LocalG2CandidateGeometry>(values);

    private readonly struct BoundaryNode
    {
        internal BoundaryNode(double arcLengthMeters, double x, double y, double vehicleHeadingRadians, double vehicleCurvaturePerMeter)
        {
            ArcLengthMeters = arcLengthMeters;
            X = x;
            Y = y;
            VehicleHeadingRadians = vehicleHeadingRadians;
            VehicleCurvaturePerMeter = vehicleCurvaturePerMeter;
            DerivativeScale = 0d;
        }
        private BoundaryNode(double arcLengthMeters, double x, double y, double vehicleHeadingRadians,
            double vehicleCurvaturePerMeter, double derivativeScale)
        {
            ArcLengthMeters = arcLengthMeters;
            X = x;
            Y = y;
            VehicleHeadingRadians = vehicleHeadingRadians;
            VehicleCurvaturePerMeter = vehicleCurvaturePerMeter;
            DerivativeScale = derivativeScale;
        }
        internal double ArcLengthMeters { get; }
        internal double X { get; }
        internal double Y { get; }
        internal double VehicleHeadingRadians { get; }
        internal double VehicleCurvaturePerMeter { get; }
        internal double DerivativeScale { get; }
        internal BoundaryNode WithDerivativeScale(double derivativeScale) => new BoundaryNode(
            ArcLengthMeters, X, Y, VehicleHeadingRadians, VehicleCurvaturePerMeter, derivativeScale);
    }

    private readonly struct CurveSample
    {
        internal CurveSample(double parameter, double x, double y, double dx, double dy)
        {
            Parameter = parameter;
            X = x;
            Y = y;
            Dx = dx;
            Dy = dy;
        }
        internal double Parameter { get; }
        internal double X { get; }
        internal double Y { get; }
        internal double Dx { get; }
        internal double Dy { get; }
    }

    private readonly struct DerivativeControlPoint
    {
        internal DerivativeControlPoint(double x, double y)
        {
            X = x;
            Y = y;
        }
        internal double X { get; }
        internal double Y { get; }
        internal bool IsFinite => NumericGuard.IsFinite(X) && NumericGuard.IsFinite(Y);
        internal double Norm => Math.Sqrt(X * X + Y * Y);
    }

    /// <summary>反射脚本使用的窄范围候选与拼接场景入口。</summary>
    public static class TestHooks
    {
        public static CandidateTestSnapshot Execute(string scenario)
        {
            if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
            switch (scenario)
            {
                case "Isolated": return BuildIsolated();
                case "Cluster": return BuildCluster();
                case "Reverse": return BuildReverse();
                case "Spliced": return BuildSpliced();
                case "StartBoundary": return BuildStartBoundary();
                case "InteriorStationaryCurve": return BuildInteriorStationaryCurve();
                case "ConstantVelocityCurve": return BuildConstantVelocityCurve();
                case "ExactSpliceEndpoints": return BuildExactSpliceEndpoints();
                case "GearBoundary": return BuildGearBoundary();
                case "TwoRegionWorkOrder": return BuildTwoRegionWorkOrder();
                case "MultiSegmentWorkOrder": return BuildMultiSegmentWorkOrder();
                default: throw new ArgumentOutOfRangeException(nameof(scenario));
            }
        }

        public sealed class CandidateTestSnapshot
        {
            internal CandidateTestSnapshot(int candidateCount, double startPositionError, double endPositionError,
                double startCurvatureError, double endCurvatureError, bool containsLocalG2Source,
                int outputRegionCount, bool internalConnectionsAreG2, string direction,
                bool vehicleAndGeometricCurvatureSignsAreOpposite, bool noDuplicateNonGearPoints,
                bool endpointsUnchanged, bool rejected = false, bool endpointsAreExact = false,
                bool gearBoundaryMarkerPreserved = false, bool accepted = false,
                bool workOrderDescending = false, bool frontArcPreservedAfterBackReplacement = false,
                bool bothReplacementsRetained = false, bool forwardOrderRejected = false,
                bool deterministicWorkOrder = false, bool invalidWorkOrderRejected = false,
                bool deterministicReplacementGeometry = false, bool segmentOrderAscending = false,
                bool equalArcUsesReportOrder = false)
            {
                CandidateCount = candidateCount;
                StartPositionError = startPositionError;
                EndPositionError = endPositionError;
                StartCurvatureError = startCurvatureError;
                EndCurvatureError = endCurvatureError;
                ContainsLocalG2Source = containsLocalG2Source;
                OutputRegionCount = outputRegionCount;
                InternalConnectionsAreG2 = internalConnectionsAreG2;
                Direction = direction;
                VehicleAndGeometricCurvatureSignsAreOpposite = vehicleAndGeometricCurvatureSignsAreOpposite;
                NoDuplicateNonGearPoints = noDuplicateNonGearPoints;
                EndpointsUnchanged = endpointsUnchanged;
                Rejected = rejected;
                EndpointsAreExact = endpointsAreExact;
                GearBoundaryMarkerPreserved = gearBoundaryMarkerPreserved;
                Accepted = accepted;
                WorkOrderDescending = workOrderDescending;
                FrontArcPreservedAfterBackReplacement = frontArcPreservedAfterBackReplacement;
                BothReplacementsRetained = bothReplacementsRetained;
                ForwardOrderRejected = forwardOrderRejected;
                DeterministicWorkOrder = deterministicWorkOrder;
                InvalidWorkOrderRejected = invalidWorkOrderRejected;
                DeterministicReplacementGeometry = deterministicReplacementGeometry;
                SegmentOrderAscending = segmentOrderAscending;
                EqualArcUsesReportOrder = equalArcUsesReportOrder;
            }
            public int CandidateCount { get; }
            public double StartPositionError { get; }
            public double EndPositionError { get; }
            public double StartCurvatureError { get; }
            public double EndCurvatureError { get; }
            public bool ContainsLocalG2Source { get; }
            public int OutputRegionCount { get; }
            public bool InternalConnectionsAreG2 { get; }
            public string Direction { get; }
            public bool VehicleAndGeometricCurvatureSignsAreOpposite { get; }
            public bool NoDuplicateNonGearPoints { get; }
            public bool EndpointsUnchanged { get; }
            public bool Rejected { get; }
            public bool EndpointsAreExact { get; }
            public bool GearBoundaryMarkerPreserved { get; }
            public bool Accepted { get; }
            public bool WorkOrderDescending { get; }
            public bool FrontArcPreservedAfterBackReplacement { get; }
            public bool BothReplacementsRetained { get; }
            public bool ForwardOrderRejected { get; }
            public bool DeterministicWorkOrder { get; }
            public bool InvalidWorkOrderRejected { get; }
            public bool DeterministicReplacementGeometry { get; }
            public bool SegmentOrderAscending { get; }
            public bool EqualArcUsesReportOrder { get; }
        }

        private static CandidateTestSnapshot BuildIsolated()
        {
            PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null);
            LocalG2SmoothingRegion region = CreateRegion(new[]
            {
                new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, 0d, 0d, 0.4d),
            }, 0.25d, 0.75d);
            IReadOnlyList<LocalG2CandidateGeometry> candidates = new LocalG2CandidateBuilder().Build(
                segment, region, 0.025d, CreateOptions(), CancellationToken.None);
            if (candidates.Count == 0) throw new InvalidOperationException("The isolated candidate scenario produced no candidate.");
            LocalG2CandidateGeometry candidate = candidates[0];
            PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.25d, out SmoothingPoint2D start, out _);
            PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.75d, out SmoothingPoint2D end, out _);
            return new CandidateTestSnapshot(candidates.Count,
                Distance(start, candidate.RegionPoints[0]), Distance(end, candidate.RegionPoints[candidate.RegionPoints.Count - 1]),
                Math.Abs(candidate.StartVehicleCurvaturePerMeter - 0d),
                Math.Abs(candidate.EndVehicleCurvaturePerMeter - 0.4d),
                ContainsLocalG2(candidate.RegionPoints), 0, false, string.Empty, false, false, false);
        }

        private static CandidateTestSnapshot BuildCluster()
        {
            PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null);
            var transitions = new[]
            {
                new CurvatureTransition(0, 1, 2, 0.4d, 0.4d, 0d, 0d, 0d, 0.4d),
                new CurvatureTransition(0, 2, 3, 0.6d, 0.6d, 0d, 0d, 0.4d, 0d),
            };
            var planner = new LocalG2WindowPlanner();
            if (!planner.TryPlan(new PreparedPath(new[] { segment }), transitions, CreateOptions(),
                    out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
            {
                throw new InvalidOperationException(reason);
            }
            IReadOnlyList<LocalG2CandidateGeometry> candidates = new LocalG2CandidateBuilder().Build(
                segment, regions[0], 0.025d, CreateOptions(), CancellationToken.None);
            if (candidates.Count == 0) throw new InvalidOperationException("The clustered candidate scenario produced no candidate.");
            return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, regions.Count,
                candidates[0].InternalConnectionsAreG2, string.Empty, false, false, false);
        }

        private static CandidateTestSnapshot BuildReverse()
        {
            PreparedDirectionSegment segment = CreateSegment(TravelDirection.Reverse, null);
            LocalG2SmoothingRegion region = CreateRegion(new[]
            {
                new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, Math.PI, 0.2d, 0.4d),
            }, 0.25d, 0.75d);
            IReadOnlyList<LocalG2CandidateGeometry> candidates = new LocalG2CandidateBuilder().Build(
                segment, region, 0.025d, CreateOptions(), CancellationToken.None);
            if (candidates.Count == 0) throw new InvalidOperationException("The reverse candidate scenario produced no candidate.");
            LocalG2CandidateGeometry candidate = candidates[0];
            bool opposite = candidate.StartVehicleCurvaturePerMeter * candidate.StartGeometricCurvaturePerMeter < 0d &&
                candidate.EndVehicleCurvaturePerMeter * candidate.EndGeometricCurvaturePerMeter < 0d;
            return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false,
                segment.Direction.ToString(), opposite, false, false);
        }

        private static CandidateTestSnapshot BuildSpliced()
        {
            PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, 0.17d);
            LocalG2SmoothingRegion region = CreateRegion(new[]
            {
                new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, 0d, 0.17d, 0.4d),
            }, 0.25d, 0.75d);
            IReadOnlyList<LocalG2CandidateGeometry> candidates = new LocalG2CandidateBuilder().Build(
                segment, region, 0.025d, CreateOptions(), CancellationToken.None);
            if (candidates.Count == 0) throw new InvalidOperationException("The splice candidate scenario produced no candidate.");
            PreparedDirectionSegment untouched = CreateSegment(TravelDirection.Reverse, null);
            var path = new PreparedPath(new[] { segment, untouched });
            if (!new LocalG2PathSplicer().TryReplace(path, candidates[0], out PreparedPath replaced, out string reason))
                throw new InvalidOperationException(reason);
            PreparedDirectionSegment result = replaced.Segments[0];
            bool noDuplicates = true;
            for (int index = 1; index < result.Points.Count; index++)
            {
                if (!result.Points[index - 1].IsGearSwitchPoint && !result.Points[index].IsGearSwitchPoint &&
                    Distance(result.Points[index - 1], result.Points[index]) <= 1e-12d)
                {
                    noDuplicates = false;
                    break;
                }
            }
            bool monotonicallyIncreasing = true;
            for (int index = 1; index < result.Points.Count; index++)
            {
                if (result.Points[index].ArcLength <= result.Points[index - 1].ArcLength)
                {
                    monotonicallyIncreasing = false;
                    break;
                }
            }
            bool endpoints = Distance(segment.Points[0], result.Points[0]) <= 1e-12d &&
                Distance(segment.Points[segment.Points.Count - 1], result.Points[result.Points.Count - 1]) <= 1e-12d &&
                result.StartVehicleCurvaturePerMeter == segment.StartVehicleCurvaturePerMeter && monotonicallyIncreasing &&
                ReferenceEquals(untouched, replaced.Segments[1]);
            return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
                false, noDuplicates, endpoints);
        }

        private static CandidateTestSnapshot BuildStartBoundary()
        {
            PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, 0.17d);
            LocalG2SmoothingRegion region = CreateRegion(new[]
            {
                new CurvatureTransition(0, 0, 1, 0d, 0d, 0d, 0d, 0.17d, 0.4d),
            }, 0d, 0.5d);
            IReadOnlyList<LocalG2CandidateGeometry> candidates = new LocalG2CandidateBuilder().Build(
                segment, region, 0.025d, CreateOptions(), CancellationToken.None);
            if (candidates.Count == 0) throw new InvalidOperationException("The start-boundary transition must not create a zero-length left curve.");
            LocalG2CandidateGeometry candidate = candidates[0];
            return new CandidateTestSnapshot(candidates.Count,
                Math.Abs(candidate.StartVehicleCurvaturePerMeter - 0.17d), 0d,
                Math.Abs(candidate.StartVehicleCurvaturePerMeter - 0.17d), 0d,
                ContainsLocalG2(candidate.RegionPoints), 0, false, string.Empty, false, false, false);
        }

        private static CandidateTestSnapshot BuildInteriorStationaryCurve()
        {
            const double endpointX = 0.00325d;
            if (!QuinticHermiteCurve2D.TryCreate(0d, 0d, 0.01d, 0d, 0d, 0d,
                    endpointX, 0d, 0.01d, 0d, 0d, 0d, out QuinticHermiteCurve2D curve, out string reason))
            {
                throw new InvalidOperationException(reason);
            }
            var points = new[]
            {
                new SmoothingPoint2D(0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
                new SmoothingPoint2D(endpointX, 0d, endpointX, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
            };
            var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false);
            var sampled = new List<SmoothingPoint2D>();
            bool accepted = TryAppendCurveSamples(curve,
                new BoundaryNode(0d, 0d, 0d, 0d, 0d),
                new BoundaryNode(endpointX, endpointX, 0d, 0d, 0d),
                segment, 0.025d, sampled, CancellationToken.None);
            return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
                false, false, false, !accepted);
        }

        private static CandidateTestSnapshot BuildConstantVelocityCurve()
        {
            if (!QuinticHermiteCurve2D.TryCreate(0d, 0d, 1d, 0d, 0d, 0d,
                    1d, 0d, 1d, 0d, 0d, 0d, out QuinticHermiteCurve2D curve, out string reason))
            {
                throw new InvalidOperationException(reason);
            }
            var points = new[]
            {
                new SmoothingPoint2D(0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
                new SmoothingPoint2D(1d, 0d, 1d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
            };
            var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false);
            var sampled = new List<SmoothingPoint2D>();
            bool accepted = TryAppendCurveSamples(curve,
                new BoundaryNode(0d, 0d, 0d, 0d, 0d),
                new BoundaryNode(1d, 1d, 0d, 0d, 0d),
                segment, 0.025d, sampled, CancellationToken.None);
            return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
                false, false, false, false, false, false, accepted);
        }

        private static CandidateTestSnapshot BuildExactSpliceEndpoints()
        {
            PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null);
            var candidate = new LocalG2CandidateGeometry(0, 0, 0.25d, 0.75d, 0.25d, 0.25d,
                new[]
                {
                    Point(0.2500000005d, 0d, 0.25d, false),
                    Point(0.5d, 0d, 0.5d, false),
                    Point(0.7499999995d, 0d, 0.75d, false),
                }, 0d, 0d, 0d, 0d, true);
            if (!new LocalG2PathSplicer().TryReplace(new PreparedPath(new[] { segment }), candidate,
                    out PreparedPath replaced, out string reason))
            {
                throw new InvalidOperationException(reason);
            }
            PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.25d, out SmoothingPoint2D start, out _);
            PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.75d, out SmoothingPoint2D end, out _);
            IReadOnlyList<SmoothingPoint2D> result = replaced.Segments[0].Points;
            bool exact = result[1].X == start.X && result[1].Y == start.Y &&
                result[result.Count - 2].X == end.X && result[result.Count - 2].Y == end.Y;
            return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
                false, false, false, false, exact);
        }

        private static CandidateTestSnapshot BuildGearBoundary()
        {
            var sourcePoints = new[]
            {
                Point(0d, 0d, 0d, true), Point(0.25d, 0d, 0.25d, false), Point(0.5d, 0d, 0.5d, false),
            };
            var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, sourcePoints, true, false);
            var candidate = new LocalG2CandidateGeometry(0, 0, 0d, 0.5d, 0d, 0.5d,
                new[] { Point(0d, 0d, 0d, false), Point(0.25d, 0d, 0.25d, false), Point(0.5d, 0d, 0.5d, false) },
                0d, 0d, 0d, 0d, true);
            if (!new LocalG2PathSplicer().TryReplace(new PreparedPath(new[] { segment }), candidate,
                    out PreparedPath replaced, out string reason))
            {
                throw new InvalidOperationException(reason);
            }
            bool preserved = replaced.Segments[0].StartsAtGearSwitch && replaced.Segments[0].Points[0].IsGearSwitchPoint;
            return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
                false, false, false, false, false, preserved);
        }

        private static CandidateTestSnapshot BuildTwoRegionWorkOrder()
        {
            var points = new List<SmoothingPoint2D>();
            for (int index = 0; index <= 12; index++)
            {
                double arc = index * 0.25d;
                points.Add(new SmoothingPoint2D(
                    arc,
                    0d,
                    arc,
                    0d,
                    0d,
                    1d,
                    false,
                    SmoothedPathPointSource.Anchor));
            }

            var segment = new PreparedDirectionSegment(
                0,
                TravelDirection.Forward,
                points,
                false,
                false);
            var original = new PreparedPath(new[] { segment });
            LocalG2SmoothingRegion frontRegion = CreateOrderedRegion(0.75d, 0.5d, 1.0d, 0);
            LocalG2SmoothingRegion backRegion = CreateOrderedRegion(2.25d, 2.0d, 2.5d, 1);
            var reportOrder = new[] { frontRegion, backRegion };

            var orderer = new LocalG2RegionWorkOrder();
            if (!orderer.TryCreate(
                    reportOrder,
                    out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
                    out string orderReason))
            {
                throw new InvalidOperationException(orderReason);
            }
            if (!orderer.TryCreate(
                    reportOrder,
                    out IReadOnlyList<LocalG2SmoothingRegion> repeatedOrder,
                    out string repeatedReason))
            {
                throw new InvalidOperationException(repeatedReason);
            }

            bool descending =
                ReferenceEquals(backRegion, workOrder[0]) &&
                ReferenceEquals(frontRegion, workOrder[1]);
            bool deterministic =
                ReferenceEquals(workOrder[0], repeatedOrder[0]) &&
                ReferenceEquals(workOrder[1], repeatedOrder[1]) &&
                ReferenceEquals(frontRegion, reportOrder[0]) &&
                ReferenceEquals(backRegion, reportOrder[1]);

            var wrongSegmentTransition = new CurvatureTransition(
                1,
                0,
                1,
                0.75d,
                0.75d,
                0d,
                0d,
                0d,
                0.4d);
            var invalidRegion = new LocalG2SmoothingRegion(
                0,
                new[] { wrongSegmentTransition },
                0.5d,
                1.0d,
                new[]
                {
                    new LocalG2WindowVariant(0, 0.5d, 1.0d, 0.25d, 0.25d),
                });
            bool invalidWorkOrderRejected =
                !orderer.TryCreate(
                    new[] { invalidRegion },
                    out _,
                    out _) &&
                !orderer.TryCreate(
                    new LocalG2SmoothingRegion[] { null },
                    out _,
                    out _);

            LocalG2CandidateGeometry frontCandidate =
                CreateLengthChangingCandidate(0, 0.5d, 1.0d);
            LocalG2CandidateGeometry backCandidate =
                CreateLengthChangingCandidate(1, 2.0d, 2.5d);
            var splicer = new LocalG2PathSplicer();

            if (!splicer.TryReplace(
                    original,
                    backCandidate,
                    out PreparedPath afterBack,
                    out string backReason))
            {
                throw new InvalidOperationException(backReason);
            }

            bool frontArcPreserved =
                PathReferenceInterpolator.TryInterpolateByArcLength(
                    afterBack.Segments[0].Points,
                    0.5d,
                    out SmoothingPoint2D frontStart,
                    out _) &&
                PathReferenceInterpolator.TryInterpolateByArcLength(
                    afterBack.Segments[0].Points,
                    1.0d,
                    out SmoothingPoint2D frontEnd,
                    out _) &&
                Math.Abs(frontStart.X - 0.5d) <= 1e-12d &&
                Math.Abs(frontEnd.X - 1.0d) <= 1e-12d;

            if (!splicer.TryReplace(
                    afterBack,
                    frontCandidate,
                    out PreparedPath afterBoth,
                    out string frontReason))
            {
                throw new InvalidOperationException(frontReason);
            }

            if (!splicer.TryReplace(
                    original,
                    backCandidate,
                    out PreparedPath repeatedAfterBack,
                    out string repeatedBackReason))
            {
                throw new InvalidOperationException(repeatedBackReason);
            }
            if (!splicer.TryReplace(
                    repeatedAfterBack,
                    frontCandidate,
                    out PreparedPath repeatedAfterBoth,
                    out string repeatedFrontReason))
            {
                throw new InvalidOperationException(repeatedFrontReason);
            }

            bool frontInteriorRetained = false;
            bool backInteriorRetained = false;
            int frontInteriorCount = 0;
            int backInteriorCount = 0;
            for (int index = 0; index < afterBoth.Segments[0].Points.Count; index++)
            {
                SmoothingPoint2D point = afterBoth.Segments[0].Points[index];
                if (point.Source != SmoothedPathPointSource.LocalG2Transition)
                    continue;
                if (point.X == 0.75d && point.Y == 0.20d)
                {
                    frontInteriorCount++;
                    frontInteriorRetained = true;
                }
                if (point.X == 2.25d && point.Y == 0.20d)
                {
                    backInteriorCount++;
                    backInteriorRetained = true;
                }
            }
            bool exactInteriorsRetained =
                frontInteriorRetained &&
                backInteriorRetained &&
                frontInteriorCount == 1 &&
                backInteriorCount == 1;
            bool deterministicReplacementGeometry =
                HasSameReplacementGeometry(afterBoth, repeatedAfterBoth);

            if (!splicer.TryReplace(
                    original,
                    frontCandidate,
                    out PreparedPath afterFront,
                    out string firstReason))
            {
                throw new InvalidOperationException(firstReason);
            }
            bool forwardOrderRejected = !splicer.TryReplace(
                afterFront,
                backCandidate,
                out _,
                out _);

            return new CandidateTestSnapshot(
                0,
                0d,
                0d,
                0d,
                0d,
                false,
                0,
                false,
                string.Empty,
                false,
                false,
                false,
                false,
                false,
                false,
                false,
                descending,
                frontArcPreserved,
                exactInteriorsRetained,
                forwardOrderRejected,
                deterministic,
                invalidWorkOrderRejected,
                deterministicReplacementGeometry);
        }

        private static CandidateTestSnapshot BuildMultiSegmentWorkOrder()
        {
            LocalG2SmoothingRegion laterSegment =
                CreateOrderedRegion(0.25d, 0d, 0.5d, 0, 1);
            LocalG2SmoothingRegion firstTie =
                CreateOrderedRegion(0.75d, 0.5d, 1.0d, 0);
            LocalG2SmoothingRegion secondTie =
                CreateOrderedRegion(0.75d, 0.5d, 1.0d, 1);
            var reportOrder = new[] { laterSegment, firstTie, secondTie };
            if (!new LocalG2RegionWorkOrder().TryCreate(
                    reportOrder,
                    out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
                    out string reason))
            {
                throw new InvalidOperationException(reason);
            }

            bool segmentOrderAscending =
                workOrder.Count == 3 &&
                workOrder[0].SegmentIndex == 0 &&
                workOrder[1].SegmentIndex == 0 &&
                workOrder[2].SegmentIndex == 1;
            bool equalArcUsesReportOrder =
                ReferenceEquals(firstTie, workOrder[0]) &&
                ReferenceEquals(secondTie, workOrder[1]);
            return new CandidateTestSnapshot(
                0,
                0d,
                0d,
                0d,
                0d,
                false,
                0,
                false,
                string.Empty,
                false,
                false,
                false,
                segmentOrderAscending: segmentOrderAscending,
                equalArcUsesReportOrder: equalArcUsesReportOrder);
        }

        private static bool HasSameReplacementGeometry(
            PreparedPath first,
            PreparedPath second)
        {
            if (first == null || second == null ||
                first.Points.Count != second.Points.Count ||
                first.Segments.Count != second.Segments.Count)
            {
                return false;
            }

            for (int segmentIndex = 0; segmentIndex < first.Segments.Count; segmentIndex++)
            {
                PreparedDirectionSegment firstSegment = first.Segments[segmentIndex];
                PreparedDirectionSegment secondSegment = second.Segments[segmentIndex];
                if (firstSegment.Direction != secondSegment.Direction ||
                    firstSegment.Points.Count != secondSegment.Points.Count)
                {
                    return false;
                }

                for (int pointIndex = 0; pointIndex < firstSegment.Points.Count; pointIndex++)
                {
                    SmoothingPoint2D firstPoint = firstSegment.Points[pointIndex];
                    SmoothingPoint2D secondPoint = secondSegment.Points[pointIndex];
                    if (firstPoint.X != secondPoint.X ||
                        firstPoint.Y != secondPoint.Y ||
                        firstPoint.Source != secondPoint.Source ||
                        firstSegment.Direction != secondSegment.Direction)
                    {
                        return false;
                    }
                }
            }
            return true;
        }

        private static LocalG2SmoothingRegion CreateOrderedRegion(
            double eventArc,
            double startArc,
            double endArc,
            int index,
            int segmentIndex = 0)
        {
            var transition = new CurvatureTransition(
                segmentIndex,
                index,
                index + 1,
                eventArc,
                eventArc,
                0d,
                0d,
                0d,
                0.4d);
            return new LocalG2SmoothingRegion(
                segmentIndex,
                new[] { transition },
                startArc,
                endArc,
                new[]
                {
                    new LocalG2WindowVariant(
                        0,
                        startArc,
                        endArc,
                        eventArc - startArc,
                        endArc - eventArc),
                });
        }

        private static LocalG2CandidateGeometry CreateLengthChangingCandidate(
            int candidateIndex,
            double startArc,
            double endArc)
        {
            double middleArc = 0.5d * (startArc + endArc);
            var points = new[]
            {
                Point(startArc, 0d, startArc, false),
                Point(middleArc, 0.20d, middleArc, false),
                Point(endArc, 0d, endArc, false),
            };
            return new LocalG2CandidateGeometry(
                candidateIndex,
                0,
                startArc,
                endArc,
                middleArc - startArc,
                endArc - middleArc,
                points,
                0d,
                0d,
                0d,
                0d,
                true);
        }

        private static PreparedDirectionSegment CreateSegment(TravelDirection direction, double? startCurvature)
        {
            double heading = direction == TravelDirection.Forward ? 0d : Math.PI;
            var points = new List<SmoothingPoint2D>();
            for (int index = 0; index <= 4; index++)
            {
                double arc = index * 0.25d;
                points.Add(new SmoothingPoint2D(arc, 0d, arc, heading, heading, 1d, false, SmoothedPathPointSource.Anchor));
            }
            return new PreparedDirectionSegment(0, direction, points, false, false, startCurvature);
        }

        private static LocalG2SmoothingRegion CreateRegion(
            IReadOnlyList<CurvatureTransition> transitions,
            double startArcLengthMeters,
            double endArcLengthMeters)
        {
            return new LocalG2SmoothingRegion(0, transitions, startArcLengthMeters, endArcLengthMeters,
                new[] { new LocalG2WindowVariant(0, startArcLengthMeters, endArcLengthMeters,
                    transitions[0].LocalArcLengthMeters - startArcLengthMeters,
                    endArcLengthMeters - transitions[transitions.Count - 1].LocalArcLengthMeters) });
        }

        private static LocalG2OptionsSnapshot CreateOptions() => new LocalG2OptionsSnapshot(new PathSmoothingConfiguration());
        private static bool ContainsLocalG2(IReadOnlyList<SmoothingPoint2D> points)
        {
            for (int index = 0; index < points.Count; index++)
                if (points[index].Source == SmoothedPathPointSource.LocalG2Transition) return true;
            return false;
        }
        private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right) =>
            Math.Sqrt((right.X - left.X) * (right.X - left.X) + (right.Y - left.Y) * (right.Y - left.Y));
        private static SmoothingPoint2D Point(double x, double y, double arcLength, bool isGearSwitch) =>
            new SmoothingPoint2D(x, y, arcLength, 0d, 0d, 1d, isGearSwitch,
                isGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.LocalG2Transition);
    }
}