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

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;

/// <summary>在方向段内以局部三次 Bézier 连接替换明显转角。</summary>
internal sealed class LocalCubicBezierSmoother : IPathSmoother
{
    private const double WindowToleranceMeters = 1e-9d;

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

    /// <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)
        {
            return SmoothingCandidate.Failed("Bézier 输入、强度或净空预留无效。");
        }

        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,
                    input.Options.BezierCornerHeadingThresholdRadians,
                    input.Options.BezierMaximumWindowLengthMeters,
                    input.Options.BezierHandleLengthRatio,
                    effectiveStrength,
                    input.MinimumClearanceReserveMeters,
                    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,
                sourceSegment.StartVehicleCurvaturePerMeter));
        }

        return SmoothingCandidate.Success(candidateSegments);
    }

    private static bool TrySmoothSegment(
        PreparedDirectionSegment sourceSegment,
        double cornerThresholdRadians,
        double maximumWindowLengthMeters,
        double handleLengthRatio,
        double strength,
        double reserveMeters,
        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 == 0)
        {
            reason = "Bézier 方向段为空。";
            return false;
        }

        IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
        if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;
        if (anchors.Count < 3)
        {
            result = anchors;
            return true;
        }

        if (!TryCreateMergedWindows(
                anchors,
                cornerThresholdRadians,
                maximumWindowLengthMeters,
                cancellationToken,
                out List<Window> windows,
                out reason))
        {
            return false;
        }
        if (windows.Count == 0)
        {
            result = anchors;
            return true;
        }

        var output = new List<SmoothingPoint2D>(anchors.Count);
        int anchorIndex = 0;
        for (int windowIndex = 0; windowIndex < windows.Count; windowIndex++)
        {
            cancellationToken.ThrowIfCancellationRequested();
            Window window = windows[windowIndex];
            while (anchorIndex <= window.StartIndex)
            {
                output.Add(anchors[anchorIndex]);
                anchorIndex++;
            }

            if (!TryAppendWindowInterior(
                    anchors,
                    window,
                    handleLengthRatio,
                    strength,
                    reserveMeters,
                    output,
                    cancellationToken,
                    out reason,
                    out status))
            {
                return false;
            }

            output.Add(anchors[window.EndIndex]);
            anchorIndex = window.EndIndex + 1;
        }

        while (anchorIndex < anchors.Count)
        {
            output.Add(anchors[anchorIndex]);
            anchorIndex++;
        }

        result = output;
        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) || point.ArcLength < 0d ||
                !NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) ||
                !NumericGuard.IsFinite(point.BodyClearance) || point.BodyClearance < 0d ||
                (index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
            {
                reason = "Bézier 方向段包含非有限、非递增弧长或无效净空的锚点。";
                return false;
            }
        }
        return true;
    }

    private static bool TryCreateMergedWindows(
        IReadOnlyList<SmoothingPoint2D> anchors,
        double cornerThresholdRadians,
        double maximumWindowLengthMeters,
        CancellationToken cancellationToken,
        out List<Window> windows,
        out string reason)
    {
        windows = new List<Window>();
        var candidates = new List<Window>();
        reason = string.Empty;
        for (int cornerIndex = 1; cornerIndex < anchors.Count - 1; cornerIndex++)
        {
            cancellationToken.ThrowIfCancellationRequested();
            if (!TryGetTravelTangent(anchors[cornerIndex - 1], anchors[cornerIndex], out Point2D entryTangent) ||
                !TryGetTravelTangent(anchors[cornerIndex], anchors[cornerIndex + 1], out Point2D exitTangent))
            {
                reason = "Bézier 转角包含零长度或非有限行进切向。";
                return false;
            }

            double cross = entryTangent.X * exitTangent.Y - entryTangent.Y * exitTangent.X;
            double dot = entryTangent.X * exitTangent.X + entryTangent.Y * exitTangent.Y;
            double turnRadians = Math.Atan2(Math.Abs(cross), dot);
            if (!NumericGuard.IsFinite(turnRadians))
            {
                reason = "Bézier 转角计算产生非有限数值。";
                return false;
            }
            if (turnRadians < cornerThresholdRadians) continue;

            int startIndex = cornerIndex - 1;
            int endIndex = cornerIndex + 1;
            double windowLength = anchors[endIndex].ArcLength - anchors[startIndex].ArcLength;
            if (!NumericGuard.IsPositiveFinite(windowLength))
            {
                reason = "Bézier 局部窗口弧长无效。";
                return false;
            }
            if (windowLength > maximumWindowLengthMeters + WindowToleranceMeters) continue;
            if (ContainsGearSwitch(anchors, startIndex, endIndex)) continue;

            candidates.Add(new Window(startIndex, endIndex));
        }

        MergeBoundedConnectedWindows(anchors, candidates, maximumWindowLengthMeters, windows);
        return true;
    }

    private static void MergeBoundedConnectedWindows(
        IReadOnlyList<SmoothingPoint2D> anchors,
        IReadOnlyList<Window> candidates,
        double maximumWindowLengthMeters,
        List<Window> windows)
    {
        int candidateIndex = 0;
        while (candidateIndex < candidates.Count)
        {
            Window merged = candidates[candidateIndex];
            candidateIndex++;
            while (candidateIndex < candidates.Count &&
                   candidates[candidateIndex].StartIndex <= merged.EndIndex + 1)
            {
                merged = new Window(merged.StartIndex,
                    Math.Max(merged.EndIndex, candidates[candidateIndex].EndIndex));
                candidateIndex++;
            }

            double mergedLength = anchors[merged.EndIndex].ArcLength - anchors[merged.StartIndex].ArcLength;
            if (mergedLength <= maximumWindowLengthMeters + WindowToleranceMeters)
            {
                windows.Add(merged);
            }
            // A connected group that exceeds the cap is declined as a whole. Splitting it into
            // adjacent local curves would introduce unrequested joins; accepting it would violate
            // the maximum-window contract. Its original anchors therefore remain unchanged.
        }
    }

    private static bool TryAppendWindowInterior(
        IReadOnlyList<SmoothingPoint2D> anchors,
        Window window,
        double handleLengthRatio,
        double strength,
        double reserveMeters,
        List<SmoothingPoint2D> output,
        CancellationToken cancellationToken,
        out string reason,
        out SmoothingCandidateStatus status)
    {
        reason = string.Empty;
        status = SmoothingCandidateStatus.Failed;
        SmoothingPoint2D p0 = anchors[window.StartIndex];
        SmoothingPoint2D p3 = anchors[window.EndIndex];
        if (!TryGetTravelTangent(p0, anchors[window.StartIndex + 1], out Point2D entryTangent) ||
            !TryGetTravelTangent(anchors[window.EndIndex - 1], p3, out Point2D exitTangent))
        {
            reason = "Bézier 窗口端点包含无效行进切向。";
            return false;
        }

        double arcLength = p3.ArcLength - p0.ArcLength;
        double chordLength = Distance(p0, p3);
        double handleLength = chordLength * handleLengthRatio * strength;
        if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(chordLength) ||
            !NumericGuard.IsPositiveFinite(handleLength))
        {
            reason = "Bézier 窗口弧长、端点弦长或控制柄长度无效。";
            return false;
        }

        Point2D control1 = new Point2D(
            p0.X + entryTangent.X * handleLength,
            p0.Y + entryTangent.Y * handleLength);
        Point2D control2 = new Point2D(
            p3.X - exitTangent.X * handleLength,
            p3.Y - exitTangent.Y * handleLength);
        if (!IsFinite(control1) || !IsFinite(control2))
        {
            reason = "Bézier 控制点产生非有限数值。";
            return false;
        }

        for (int index = window.StartIndex + 1; index < window.EndIndex; index++)
        {
            cancellationToken.ThrowIfCancellationRequested();
            SmoothingPoint2D anchor = anchors[index];
            double parameter = (anchor.ArcLength - p0.ArcLength) / arcLength;
            if (!NumericGuard.IsFinite(parameter) || parameter <= 0d || parameter >= 1d)
            {
                reason = "Bézier 窗口参数无效。";
                return false;
            }

            Point2D evaluated = Evaluate(p0, control1, control2, p3, parameter);
            if (!IsFinite(evaluated))
            {
                reason = "Bézier 评估产生非有限数值。";
                return false;
            }

            double referenceArcLength = p0.ArcLength + parameter * arcLength;
            if (!PathReferenceInterpolator.TryInterpolateByArcLength(
                    anchors,
                    referenceArcLength,
                    out SmoothingPoint2D reference,
                    out reason))
            {
                return false;
            }

            double displacement = Distance(evaluated, reference);
            if (!NumericGuard.IsFinite(displacement))
            {
                reason = "Bézier 评估点位移产生非有限数值。";
                return false;
            }
            double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters);
            if (displacement > allowedDisplacement)
            {
                reason = "Bézier 评估点超过对应原始弧长参考点的允许移动范围。";
                status = SmoothingCandidateStatus.RetryableInfeasible;
                return false;
            }

            output.Add(new SmoothingPoint2D(
                evaluated.X,
                evaluated.Y,
                reference.ArcLength,
                reference.Heading,
                reference.UnwrappedHeading,
                reference.BodyClearance,
                false,
                SmoothedPathPointSource.Interpolated));
        }
        return true;
    }

    private static bool ContainsGearSwitch(IReadOnlyList<SmoothingPoint2D> anchors, int startIndex, int endIndex)
    {
        for (int index = startIndex; index <= endIndex; index++)
        {
            if (anchors[index].IsGearSwitchPoint) return true;
        }
        return false;
    }

    private static bool TryGetTravelTangent(SmoothingPoint2D start, SmoothingPoint2D end, out Point2D tangent)
    {
        tangent = default;
        double deltaX = end.X - start.X;
        double deltaY = end.Y - start.Y;
        double length = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
        if (!NumericGuard.IsPositiveFinite(length)) return false;
        tangent = new Point2D(deltaX / length, deltaY / length);
        return IsFinite(tangent);
    }

    private static Point2D Evaluate(SmoothingPoint2D p0, Point2D p1, Point2D p2, SmoothingPoint2D p3, double parameter)
    {
        double oneMinusParameter = 1d - parameter;
        double p0Weight = oneMinusParameter * oneMinusParameter * oneMinusParameter;
        double p1Weight = 3d * oneMinusParameter * oneMinusParameter * parameter;
        double p2Weight = 3d * oneMinusParameter * parameter * parameter;
        double p3Weight = parameter * parameter * parameter;
        return new Point2D(
            p0Weight * p0.X + p1Weight * p1.X + p2Weight * p2.X + p3Weight * p3.X,
            p0Weight * p0.Y + p1Weight * p1.Y + p2Weight * p2.Y + p3Weight * p3.Y);
    }

    private static bool IsFinite(Point2D point)
    {
        return NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y);
    }

    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 static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
    {
        double deltaX = left.X - right.X;
        double deltaY = left.Y - right.Y;
        return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
    }

    private readonly struct Window
    {
        internal Window(int startIndex, int endIndex)
        {
            StartIndex = startIndex;
            EndIndex = endIndex;
        }

        internal int StartIndex { get; }

        internal int EndIndex { get; }
    }

    private readonly struct Point2D
    {
        internal Point2D(double x, double y)
        {
            X = x;
            Y = y;
        }

        internal double X { get; }

        internal double Y { get; }
    }
}