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

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;

/// <summary>按方向段局部弧长构造 C2 连续的分段五次 Hermite 原始几何候选。</summary>
internal sealed class PiecewiseQuinticSmoother : IPathSmoother
{
    private const int SamplesPerInterval = 8;
    private const double DoubleMachineEpsilon = 2.2204460492503131e-16d;
    private const double EndpointNormalizationUlps = 32d;

    /// <inheritdoc />
    public SmoothingMethod Method => SmoothingMethod.PiecewiseQuintic;

    /// <inheritdoc />
    public SmoothingCandidate Smooth(
        SmoothingAlgorithmInput input,
        double effectiveStrength,
        CancellationToken cancellationToken)
    {
        if (input == null || input.OriginalPath == null || input.Options == null ||
            !NumericGuard.IsPositiveFinite(effectiveStrength) ||
            !NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
            input.MinimumClearanceReserveMeters < 0d ||
            !NumericGuard.IsPositiveFinite(input.Options.QuinticKnotSpacingMeters) ||
            !NumericGuard.IsPositiveFinite(input.Options.QuinticMinimumKnotSpacingMeters) ||
            input.Options.QuinticKnotSpacingMeters < input.Options.QuinticMinimumKnotSpacingMeters)
        {
            return SmoothingCandidate.Failed("五次 Hermite 输入、强度、净空预留或结点间距无效。");
        }

        var candidateSegments = new List<PreparedDirectionSegment>(input.OriginalPath.Segments.Count);
        for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++)
        {
            cancellationToken.ThrowIfCancellationRequested();
            PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
            if (!TrySmoothSegment(
                    sourceSegment,
                    effectiveStrength,
                    input.MinimumClearanceReserveMeters,
                    input.Options.QuinticKnotSpacingMeters,
                    input.Options.QuinticMinimumKnotSpacingMeters,
                    cancellationToken,
                    out IReadOnlyList<SmoothingPoint2D> points,
                    out string reason,
                    out SmoothingCandidateStatus status))
            {
                return status == SmoothingCandidateStatus.RetryableInfeasible
                    ? SmoothingCandidate.RetryableInfeasible(reason)
                    : SmoothingCandidate.Failed(reason);
            }

            candidateSegments.Add(new PreparedDirectionSegment(
                sourceSegment.SegmentIndex,
                sourceSegment.Direction,
                points,
                sourceSegment.StartsAtGearSwitch,
                sourceSegment.EndsAtGearSwitch));
        }

        return SmoothingCandidate.Success(candidateSegments);
    }

    private static bool TrySmoothSegment(
        PreparedDirectionSegment sourceSegment,
        double effectiveStrength,
        double reserveMeters,
        double knotSpacingMeters,
        double minimumKnotSpacingMeters,
        CancellationToken cancellationToken,
        out IReadOnlyList<SmoothingPoint2D> result,
        out string reason,
        out SmoothingCandidateStatus status)
    {
        result = null;
        reason = string.Empty;
        status = SmoothingCandidateStatus.Failed;
        if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count < 2)
        {
            reason = "五次 Hermite 方向段至少需要两个锚点。";
            return false;
        }

        IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
        if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;

        if (!TryCreateKnots(
                anchors,
                sourceSegment.Direction,
                effectiveStrength,
                knotSpacingMeters,
                minimumKnotSpacingMeters,
                cancellationToken,
                out List<Knot> knots,
                out reason))
        {
            return false;
        }

        // Each physical acceleration is blended once into its shared knot and then scaled by
        // the left/right local interval independently. Reusing this value is what makes the
        // curve C2 with respect to local arc length, even for nonuniform final intervals.
        if (!TryAssignSharedAccelerations(knots, out reason)) return false;
        if (!TryCreateIntervals(knots, out List<QuinticInterval> intervals, out reason)) return false;

        var sampled = new List<SmoothingPoint2D>(1 + intervals.Count * SamplesPerInterval);
        for (int intervalIndex = 0; intervalIndex < intervals.Count; intervalIndex++)
        {
            cancellationToken.ThrowIfCancellationRequested();
            QuinticInterval interval = intervals[intervalIndex];
            int firstSample = intervalIndex == 0 ? 0 : 1;
            for (int sampleIndex = firstSample; sampleIndex <= SamplesPerInterval; sampleIndex++)
            {
                cancellationToken.ThrowIfCancellationRequested();
                double parameter = (double)sampleIndex / SamplesPerInterval;
                double referenceArcLength = interval.Start.ArcLength + parameter * interval.Length;
                if (!NumericGuard.IsFinite(referenceArcLength))
                {
                    reason = "五次 Hermite 采样参考弧长无效。";
                    return false;
                }

                if (!PathReferenceInterpolator.TryInterpolateByArcLength(
                        anchors,
                        referenceArcLength,
                        out SmoothingPoint2D reference,
                        out reason))
                {
                    return false;
                }

                Point2D evaluated;
                if (sampleIndex == 0)
                    evaluated = interval.Start.Position;
                else if (sampleIndex == SamplesPerInterval)
                    evaluated = interval.End.Position;
                else if (!interval.TryEvaluate(parameter, out evaluated, out Point2D derivative, out Point2D secondDerivative))
                {
                    reason = "五次 Hermite 采样产生非有限位置或导数。";
                    return false;
                }

                double displacement = Distance(evaluated, reference);
                if (!NumericGuard.IsFinite(displacement))
                {
                    reason = "五次 Hermite 采样位移无效。";
                    return false;
                }
                double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters);
                if (displacement > allowedDisplacement)
                {
                    reason = "五次 Hermite 采样点超过对应局部弧长参考点的允许移动范围。";
                    status = SmoothingCandidateStatus.RetryableInfeasible;
                    return false;
                }

                bool firstEndpoint = intervalIndex == 0 && sampleIndex == 0;
                bool lastEndpoint = intervalIndex == intervals.Count - 1 && sampleIndex == SamplesPerInterval;
                if (firstEndpoint)
                {
                    sampled.Add(anchors[0]);
                }
                else if (lastEndpoint)
                {
                    sampled.Add(anchors[anchors.Count - 1]);
                }
                else
                {
                    sampled.Add(new SmoothingPoint2D(
                        evaluated.X,
                        evaluated.Y,
                        reference.ArcLength,
                        reference.Heading,
                        reference.UnwrappedHeading,
                        reference.BodyClearance,
                        false,
                        SmoothedPathPointSource.Interpolated));
                }
            }
        }

        result = sampled;
        return true;
    }

    private static bool ValidateAnchors(
        IReadOnlyList<SmoothingPoint2D> anchors,
        CancellationToken cancellationToken,
        out string reason)
    {
        reason = string.Empty;
        for (int index = 0; index < anchors.Count; index++)
        {
            cancellationToken.ThrowIfCancellationRequested();
            SmoothingPoint2D point = anchors[index];
            if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
                !NumericGuard.IsFinite(point.ArcLength) || !NumericGuard.IsFinite(point.Heading) ||
                !NumericGuard.IsFinite(point.UnwrappedHeading) || !NumericGuard.IsFinite(point.BodyClearance) ||
                point.ArcLength < 0d || point.BodyClearance < 0d ||
                (index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
            {
                reason = "五次 Hermite 方向段包含非有限、非递增弧长或无效净空的锚点。";
                return false;
            }
        }
        return true;
    }

    private static bool TryCreateKnots(
        IReadOnlyList<SmoothingPoint2D> anchors,
        TravelDirection direction,
        double effectiveStrength,
        double knotSpacingMeters,
        double minimumKnotSpacingMeters,
        CancellationToken cancellationToken,
        out List<Knot> knots,
        out string reason)
    {
        knots = new List<Knot>();
        reason = string.Empty;
        double startArcLength = anchors[0].ArcLength;
        double endArcLength = anchors[anchors.Count - 1].ArcLength;
        double totalLength = endArcLength - startArcLength;
        if (!NumericGuard.IsPositiveFinite(totalLength) || totalLength < minimumKnotSpacingMeters)
        {
            reason = "五次 Hermite 方向段短于配置的最小结点间距。";
            return false;
        }

        if (!TryCreateKnot(anchors[0], direction, effectiveStrength, out Knot first))
        {
            reason = "五次 Hermite 起点结点或行进切向无效。";
            return false;
        }
        knots.Add(first);

        double endpointTolerance = GetEndpointNormalizationTolerance(
            startArcLength,
            endArcLength,
            knotSpacingMeters);
        double previousArcLength = startArcLength;
        for (long knotOrdinal = 1L; ; knotOrdinal++)
        {
            cancellationToken.ThrowIfCancellationRequested();
            double targetArcLength = startArcLength + knotOrdinal * knotSpacingMeters;
            if (!NumericGuard.IsFinite(targetArcLength))
            {
                reason = "五次 Hermite 内部结点弧长无效。";
                return false;
            }
            if (targetArcLength >= endArcLength - endpointTolerance) break;
            if (targetArcLength <= previousArcLength)
            {
                reason = "五次 Hermite 内部结点无法在浮点弧长尺度上保持递增。";
                return false;
            }
            if (!PathReferenceInterpolator.TryInterpolateByArcLength(
                    anchors,
                    targetArcLength,
                    out SmoothingPoint2D reference,
                    out reason) ||
                !TryCreateKnot(reference, direction, effectiveStrength, out Knot knot))
            {
                if (string.IsNullOrEmpty(reason)) reason = "五次 Hermite 内部结点或行进切向无效。";
                return false;
            }
            knots.Add(knot);
            previousArcLength = targetArcLength;
        }

        if (!TryCreateKnot(anchors[anchors.Count - 1], direction, effectiveStrength, out Knot last))
        {
            reason = "五次 Hermite 终点结点或行进切向无效。";
            return false;
        }
        knots.Add(last);

        for (int index = 1; index < knots.Count; index++)
        {
            double intervalLength = knots[index].ArcLength - knots[index - 1].ArcLength;
            if (!NumericGuard.IsPositiveFinite(intervalLength) || intervalLength < minimumKnotSpacingMeters)
            {
                reason = "五次 Hermite 结点间隔无效或短于配置的最小间距。";
                return false;
            }
        }
        return true;
    }

    private static double GetEndpointNormalizationTolerance(
        double startArcLength,
        double endArcLength,
        double knotSpacingMeters)
    {
        double magnitude = Math.Max(
            Math.Abs(startArcLength),
            Math.Max(Math.Abs(endArcLength), Math.Abs(knotSpacingMeters)));
        return EndpointNormalizationUlps * DoubleMachineEpsilon * magnitude;
    }

    private static bool TryCreateKnot(
        SmoothingPoint2D reference,
        TravelDirection direction,
        double effectiveStrength,
        out Knot knot)
    {
        knot = default;
        if (reference == null || !NumericGuard.IsFinite(reference.X) || !NumericGuard.IsFinite(reference.Y) ||
            !NumericGuard.IsFinite(reference.ArcLength) || !NumericGuard.IsFinite(reference.Heading))
        {
            return false;
        }

        double travelHeading = direction == TravelDirection.Forward
            ? reference.Heading
            : reference.Heading - Math.PI;
        double tangentX = Math.Cos(travelHeading);
        double tangentY = Math.Sin(travelHeading);
        if (!NumericGuard.IsFinite(tangentX) || !NumericGuard.IsFinite(tangentY)) return false;

        var velocity = new Point2D(tangentX * effectiveStrength, tangentY * effectiveStrength);
        if (!velocity.IsFinite) return false;
        knot = new Knot(reference.ArcLength, new Point2D(reference.X, reference.Y), velocity);
        return true;
    }

    private static bool TryAssignSharedAccelerations(List<Knot> knots, out string reason)
    {
        reason = string.Empty;
        for (int index = 0; index < knots.Count; index++)
        {
            Point2D acceleration;
            if (index == 0)
            {
                if (!TryAcceleration(knots[0], knots[1], out acceleration))
                {
                    reason = "五次 Hermite 起点加速度无效。";
                    return false;
                }
            }
            else if (index == knots.Count - 1)
            {
                if (!TryAcceleration(knots[index - 1], knots[index], out acceleration))
                {
                    reason = "五次 Hermite 终点加速度无效。";
                    return false;
                }
            }
            else
            {
                if (!TryAcceleration(knots[index - 1], knots[index], out Point2D left) ||
                    !TryAcceleration(knots[index], knots[index + 1], out Point2D right))
                {
                    reason = "五次 Hermite 共享结点加速度无效。";
                    return false;
                }
                acceleration = new Point2D((left.X + right.X) / 2d, (left.Y + right.Y) / 2d);
                if (!acceleration.IsFinite)
                {
                    reason = "五次 Hermite 共享结点加速度混合产生非有限数值。";
                    return false;
                }
            }

            knots[index] = knots[index].WithAcceleration(acceleration);
        }
        return true;
    }

    private static bool TryAcceleration(Knot start, Knot end, out Point2D acceleration)
    {
        acceleration = default;
        double intervalLength = end.ArcLength - start.ArcLength;
        if (!NumericGuard.IsPositiveFinite(intervalLength)) return false;
        acceleration = new Point2D(
            (end.Velocity.X - start.Velocity.X) / intervalLength,
            (end.Velocity.Y - start.Velocity.Y) / intervalLength);
        return acceleration.IsFinite;
    }

    private static bool TryCreateIntervals(
        IReadOnlyList<Knot> knots,
        out List<QuinticInterval> intervals,
        out string reason)
    {
        intervals = new List<QuinticInterval>(knots.Count - 1);
        reason = string.Empty;
        for (int index = 1; index < knots.Count; index++)
        {
            if (!QuinticInterval.TryCreate(knots[index - 1], knots[index], out QuinticInterval interval))
            {
                reason = "五次 Hermite 系数、端点导数或结点区间无效。";
                return false;
            }
            intervals.Add(interval);
        }
        return true;
    }

    private static double Distance(Point2D point, SmoothingPoint2D reference)
    {
        double deltaX = point.X - reference.X;
        double deltaY = point.Y - reference.Y;
        return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
    }

    private readonly struct Knot
    {
        internal Knot(double arcLength, Point2D position, Point2D velocity)
        {
            ArcLength = arcLength;
            Position = position;
            Velocity = velocity;
            Acceleration = default;
        }

        internal double ArcLength { get; }

        internal Point2D Position { get; }

        internal Point2D Velocity { get; }

        internal Point2D Acceleration { get; }

        internal Knot WithAcceleration(Point2D acceleration)
        {
            return new Knot(ArcLength, Position, Velocity, acceleration);
        }

        private Knot(double arcLength, Point2D position, Point2D velocity, Point2D acceleration)
        {
            ArcLength = arcLength;
            Position = position;
            Velocity = velocity;
            Acceleration = acceleration;
        }
    }

    private readonly struct QuinticInterval
    {
        private QuinticInterval(Knot start, Knot end, Point2D c0, Point2D c1, Point2D c2, Point2D c3, Point2D c4, Point2D c5)
        {
            Start = start;
            End = end;
            Length = end.ArcLength - start.ArcLength;
            _c0 = c0;
            _c1 = c1;
            _c2 = c2;
            _c3 = c3;
            _c4 = c4;
            _c5 = c5;
        }

        private readonly Point2D _c0;
        private readonly Point2D _c1;
        private readonly Point2D _c2;
        private readonly Point2D _c3;
        private readonly Point2D _c4;
        private readonly Point2D _c5;

        internal Knot Start { get; }

        internal Knot End { get; }

        internal double Length { get; }

        internal static bool TryCreate(Knot start, Knot end, out QuinticInterval interval)
        {
            interval = default;
            double length = end.ArcLength - start.ArcLength;
            if (!NumericGuard.IsPositiveFinite(length) || !start.Position.IsFinite || !end.Position.IsFinite ||
                !start.Velocity.IsFinite || !end.Velocity.IsFinite ||
                !start.Acceleration.IsFinite || !end.Acceleration.IsFinite)
            {
                return false;
            }

            Point2D c0 = start.Position;
            Point2D c1 = Scale(start.Velocity, length);
            Point2D c2 = Scale(start.Acceleration, length * length / 2d);
            Point2D difference = Subtract(end.Position, start.Position);
            Point2D endVelocity = Scale(end.Velocity, length);
            Point2D startAcceleration = Scale(start.Acceleration, length * length);
            Point2D endAcceleration = Scale(end.Acceleration, length * length);
            Point2D c3 = Add(
                Add(Scale(difference, 10d), Scale(c1, -6d)),
                Add(Scale(endVelocity, -4d), Add(Scale(startAcceleration, -1.5d), Scale(endAcceleration, 0.5d))));
            Point2D c4 = Add(
                Add(Scale(difference, -15d), Scale(c1, 8d)),
                Add(Scale(endVelocity, 7d), Add(Scale(startAcceleration, 1.5d), Scale(endAcceleration, -1d))));
            Point2D c5 = Add(
                Add(Scale(difference, 6d), Add(Scale(c1, -3d), Scale(endVelocity, -3d))),
                Add(Scale(startAcceleration, -0.5d), Scale(endAcceleration, 0.5d)));
            if (!c0.IsFinite || !c1.IsFinite || !c2.IsFinite || !c3.IsFinite || !c4.IsFinite || !c5.IsFinite)
                return false;

            interval = new QuinticInterval(start, end, c0, c1, c2, c3, c4, c5);
            return interval.TryEvaluate(0d, out _, out _, out _) && interval.TryEvaluate(1d, out _, out _, out _);
        }

        internal bool TryEvaluate(double parameter, out Point2D position, out Point2D derivative, out Point2D secondDerivative)
        {
            position = default;
            derivative = default;
            secondDerivative = default;
            if (!NumericGuard.IsFinite(parameter) || parameter < 0d || parameter > 1d) return false;

            double t2 = parameter * parameter;
            double t3 = t2 * parameter;
            double t4 = t3 * parameter;
            double t5 = t4 * parameter;
            position = Add(Add(Add(_c0, Scale(_c1, parameter)), Add(Scale(_c2, t2), Scale(_c3, t3))),
                Add(Scale(_c4, t4), Scale(_c5, t5)));
            derivative = Add(Add(_c1, Scale(_c2, 2d * parameter)),
                Add(Scale(_c3, 3d * t2), Add(Scale(_c4, 4d * t3), Scale(_c5, 5d * t4))));
            secondDerivative = Add(Scale(_c2, 2d),
                Add(Scale(_c3, 6d * parameter), Add(Scale(_c4, 12d * t2), Scale(_c5, 20d * t3))));
            return position.IsFinite && derivative.IsFinite && secondDerivative.IsFinite;
        }
    }

    private readonly struct Point2D
    {
        internal Point2D(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);
    }

    private static Point2D Add(Point2D left, Point2D right)
    {
        return new Point2D(left.X + right.X, left.Y + right.Y);
    }

    private static Point2D Subtract(Point2D left, Point2D right)
    {
        return new Point2D(left.X - right.X, left.Y - right.Y);
    }

    private static Point2D Scale(Point2D point, double scale)
    {
        return new Point2D(point.X * scale, point.Y * scale);
    }
}