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>按方向段独立生成夹持三次 B 样条原始几何候选。</summary>
internal sealed class CubicBSplineSmoother : IPathSmoother
{
    private const int Degree = 3;
    private const int SamplesPerSpan = 64;
    private const double StraightToleranceMeters = 1e-9d;
    private const double EndpointTangentScale = 1d / 3d;
    private const double EndpointProbeParameter = 1e-6d;
    private const double MinimumTangentHandleLengthMeters = 1e-10d;

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

    /// <inheritdoc />
    public SmoothingCandidate Smooth(
        SmoothingAlgorithmInput input,
        double effectiveStrength,
        CancellationToken cancellationToken)
    {
        if (input == null || input.OriginalPath == null ||
            !NumericGuard.IsPositiveFinite(effectiveStrength) ||
            !NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
            input.MinimumClearanceReserveMeters < 0d)
        {
            return SmoothingCandidate.Failed("B 样条输入、强度或净空预留无效。");
        }

        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,
                    cancellationToken, out IReadOnlyList<SmoothingPoint2D> points, out string reason))
            {
                return 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 strength,
        double reserveMeters,
        CancellationToken cancellationToken,
        out IReadOnlyList<SmoothingPoint2D> result,
        out string reason)
    {
        result = null;
        reason = string.Empty;
        if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
        {
            reason = "B 样条方向段为空。";
            return false;
        }

        IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
        for (int index = 0; index < anchors.Count; index++)
        {
            cancellationToken.ThrowIfCancellationRequested();
            if (!IsValidAnchor(anchors[index]))
            {
                reason = "B 样条方向段包含非法锚点。";
                return false;
            }
        }

        if (anchors.Count <= Degree || IsStraight(anchors))
        {
            result = anchors;
            return true;
        }

        if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
                cancellationToken, out Point2D[] controls, out reason))
        {
            return false;
        }

        double[] knots = CreateClampedKnots(controls.Length);
        var sampled = new List<SmoothingPoint2D>();
        int spanCount = controls.Length - Degree;
        int uniformIntervals = spanCount * SamplesPerSpan;
        if (!TryAddSample(0d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason))
            return false;
        if (!TryAddSample(EndpointProbeParameter, anchors, controls, knots, reserveMeters,
                sampled, cancellationToken, out reason))
            return false;
        for (int index = 1; index < uniformIntervals; index++)
        {
            if (!TryAddSample((double)index / uniformIntervals, anchors, controls, knots, reserveMeters,
                    sampled, cancellationToken, out reason))
            {
                return false;
            }
        }
        if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters,
                sampled, cancellationToken, out reason))
            return false;
        if (!TryAddSample(1d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason))
            return false;

        result = sampled;
        return true;
    }

    private static bool TryCreateControls(
        IReadOnlyList<SmoothingPoint2D> anchors,
        TravelDirection direction,
        double strength,
        double reserveMeters,
        CancellationToken cancellationToken,
        out Point2D[] controls,
        out string reason)
    {
        reason = string.Empty;
        controls = new Point2D[anchors.Count];
        controls[0] = Point2D.FromAnchor(anchors[0]);
        controls[controls.Length - 1] = Point2D.FromAnchor(anchors[anchors.Count - 1]);

        double startHandleLength = Distance(anchors[0], anchors[1]) * EndpointTangentScale * strength;
        double startTravelHeading = GetTravelHeading(anchors[0], direction);
        if (!TryConstrainTangentHandle(
                Point2D.FromAnchor(anchors[0]),
                Math.Cos(startTravelHeading),
                Math.Sin(startTravelHeading),
                startHandleLength,
                anchors[1],
                GetAllowedRadius(anchors[1], reserveMeters),
                out controls[1]))
        {
            reason = "B 样条起点切向手柄无法同时满足相邻锚点移动范围。";
            return false;
        }

        int finalIndex = anchors.Count - 1;
        double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * EndpointTangentScale * strength;
        double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction);
        if (!TryConstrainTangentHandle(
                Point2D.FromAnchor(anchors[finalIndex]),
                -Math.Cos(endTravelHeading),
                -Math.Sin(endTravelHeading),
                endHandleLength,
                anchors[finalIndex - 1],
                GetAllowedRadius(anchors[finalIndex - 1], reserveMeters),
                out controls[finalIndex - 1]))
        {
            reason = "B 样条终点切向手柄无法同时满足相邻锚点移动范围。";
            return false;
        }

        for (int index = 2; index < finalIndex - 1; index++)
        {
            cancellationToken.ThrowIfCancellationRequested();
            SmoothingPoint2D previous = anchors[index - 1];
            SmoothingPoint2D current = anchors[index];
            SmoothingPoint2D next = anchors[index + 1];
            Point2D target = new Point2D(
                (previous.X + current.X + next.X) / 3d,
                (previous.Y + current.Y + next.Y) / 3d);
            Point2D proposed = new Point2D(
                current.X + strength * (target.X - current.X),
                current.Y + strength * (target.Y - current.Y));
            controls[index] = ClampDisplacement(current, proposed, GetAllowedRadius(current, reserveMeters));
        }

        for (int index = 0; index < controls.Length; index++)
        {
            if (!NumericGuard.IsFinite(controls[index].X) || !NumericGuard.IsFinite(controls[index].Y))
            {
                reason = "B 样条控制点构造产生非法数值。";
                return false;
            }
        }
        return true;
    }

    private static bool TryAddSample(
        double parameter,
        IReadOnlyList<SmoothingPoint2D> anchors,
        Point2D[] controls,
        double[] knots,
        double reserveMeters,
        List<SmoothingPoint2D> output,
        CancellationToken cancellationToken,
        out string reason)
    {
        reason = string.Empty;
        cancellationToken.ThrowIfCancellationRequested();
        Point2D evaluated = Evaluate(controls, knots, parameter);
        SmoothingPoint2D reference = InterpolateAnchor(anchors, parameter);
        double displacement = Distance(evaluated, reference);
        if (!NumericGuard.IsFinite(displacement) || displacement > GetAllowedRadius(reference, reserveMeters))
        {
            reason = "B 样条评估点超过对应原始参考点的允许移动范围。";
            return false;
        }
        bool endpoint = parameter == 0d || parameter == 1d;
        output.Add(new SmoothingPoint2D(
            evaluated.X,
            evaluated.Y,
            reference.ArcLength,
            reference.Heading,
            reference.UnwrappedHeading,
            reference.BodyClearance,
            endpoint && reference.IsGearSwitchPoint,
            endpoint ? reference.Source : SmoothedPathPointSource.Interpolated));
        return true;
    }

    private static bool TryConstrainTangentHandle(
        Point2D endpoint,
        double rayDirectionX,
        double rayDirectionY,
        double desiredLength,
        SmoothingPoint2D adjacentAnchor,
        double allowedRadius,
        out Point2D control)
    {
        control = default;
        double offsetX = adjacentAnchor.X - endpoint.X;
        double offsetY = adjacentAnchor.Y - endpoint.Y;
        double projectedLength = offsetX * rayDirectionX + offsetY * rayDirectionY;
        double perpendicularX = offsetX - projectedLength * rayDirectionX;
        double perpendicularY = offsetY - projectedLength * rayDirectionY;
        double discriminant = allowedRadius * allowedRadius -
            (perpendicularX * perpendicularX + perpendicularY * perpendicularY);
        if (!NumericGuard.IsFinite(discriminant) || discriminant < 0d) return false;

        double halfInterval = Math.Sqrt(discriminant);
        double minimumLength = Math.Max(MinimumTangentHandleLengthMeters, projectedLength - halfInterval);
        double maximumLength = projectedLength + halfInterval;
        if (!NumericGuard.IsFinite(maximumLength) || maximumLength < minimumLength) return false;

        double constrainedLength = Math.Max(minimumLength, Math.Min(desiredLength, maximumLength));
        control = new Point2D(
            endpoint.X + constrainedLength * rayDirectionX,
            endpoint.Y + constrainedLength * rayDirectionY);
        return NumericGuard.IsFinite(control.X) && NumericGuard.IsFinite(control.Y);
    }

    private static Point2D Evaluate(Point2D[] controls, double[] knots, double parameter)
    {
        if (parameter <= 0d) return controls[0];
        if (parameter >= 1d) return controls[controls.Length - 1];

        var point = new Point2D(0d, 0d);
        for (int index = 0; index < controls.Length; index++)
        {
            double basis = EvaluateBasis(index, Degree, parameter, knots);
            point = new Point2D(point.X + basis * controls[index].X, point.Y + basis * controls[index].Y);
        }
        return point;
    }

    private static double EvaluateBasis(int index, int degree, double parameter, double[] knots)
    {
        if (degree == 0)
            return knots[index] <= parameter && parameter < knots[index + 1] ? 1d : 0d;

        double left = 0d;
        double leftDenominator = knots[index + degree] - knots[index];
        if (leftDenominator > 0d)
            left = (parameter - knots[index]) / leftDenominator * EvaluateBasis(index, degree - 1, parameter, knots);

        double right = 0d;
        double rightDenominator = knots[index + degree + 1] - knots[index + 1];
        if (rightDenominator > 0d)
            right = (knots[index + degree + 1] - parameter) / rightDenominator *
                EvaluateBasis(index + 1, degree - 1, parameter, knots);
        return left + right;
    }

    private static double[] CreateClampedKnots(int controlCount)
    {
        var knots = new double[controlCount + Degree + 1];
        for (int index = Degree + 1; index < controlCount; index++)
            knots[index] = (double)(index - Degree) / (controlCount - Degree);
        for (int index = controlCount; index < knots.Length; index++) knots[index] = 1d;
        return knots;
    }

    private static SmoothingPoint2D InterpolateAnchor(IReadOnlyList<SmoothingPoint2D> anchors, double parameter)
    {
        if (parameter <= 0d) return anchors[0];
        if (parameter >= 1d) return anchors[anchors.Count - 1];

        double scaled = parameter * (anchors.Count - 1);
        int leftIndex = (int)Math.Floor(scaled);
        double ratio = scaled - leftIndex;
        SmoothingPoint2D left = anchors[leftIndex];
        SmoothingPoint2D right = anchors[leftIndex + 1];
        return new SmoothingPoint2D(
            left.X + ratio * (right.X - left.X),
            left.Y + ratio * (right.Y - left.Y),
            left.ArcLength + ratio * (right.ArcLength - left.ArcLength),
            left.Heading + ratio * (right.Heading - left.Heading),
            left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading),
            left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance),
            false,
            SmoothedPathPointSource.Interpolated);
    }

    private static Point2D ClampDisplacement(SmoothingPoint2D anchor, Point2D proposed, double allowedRadius)
    {
        double deltaX = proposed.X - anchor.X;
        double deltaY = proposed.Y - anchor.Y;
        double distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
        if (!NumericGuard.IsFinite(distance) || distance <= allowedRadius) return proposed;
        if (distance == 0d || allowedRadius == 0d) return Point2D.FromAnchor(anchor);
        double scale = allowedRadius / distance;
        return new Point2D(anchor.X + deltaX * scale, anchor.Y + deltaY * scale);
    }

    private static bool IsStraight(IReadOnlyList<SmoothingPoint2D> anchors)
    {
        if (anchors.Count < 3) return true;
        SmoothingPoint2D first = anchors[0];
        SmoothingPoint2D last = anchors[anchors.Count - 1];
        double directionX = last.X - first.X;
        double directionY = last.Y - first.Y;
        double length = Math.Sqrt(directionX * directionX + directionY * directionY);
        if (!NumericGuard.IsFinite(length) || length <= StraightToleranceMeters) return false;
        for (int index = 1; index < anchors.Count - 1; index++)
        {
            double offsetX = anchors[index].X - first.X;
            double offsetY = anchors[index].Y - first.Y;
            double perpendicularDeviation = Math.Abs(directionX * offsetY - directionY * offsetX) / length;
            if (perpendicularDeviation >= StraightToleranceMeters) return false;
        }
        return true;
    }

    private static bool IsValidAnchor(SmoothingPoint2D point)
    {
        return 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.BodyClearance >= 0d;
    }

    private static double GetAllowedRadius(SmoothingPoint2D anchor, double reserveMeters)
    {
        return Math.Max(0d, anchor.BodyClearance - reserveMeters);
    }

    private static double GetTravelHeading(SmoothingPoint2D point, TravelDirection direction)
    {
        return direction == TravelDirection.Forward ? point.Heading : point.Heading - Math.PI;
    }

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

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

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

        internal double X { get; }
        internal double Y { get; }

        internal static Point2D FromAnchor(SmoothingPoint2D anchor)
        {
            return new Point2D(anchor.X, anchor.Y);
        }
    }
}