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;

    /// <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;
        }

        Point2D[] controls = CreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
            cancellationToken);
        if (controls == null)
        {
            reason = "B 样条控制点构造产生非法数值。";
            return false;
        }

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

        result = sampled;
        return true;
    }

    private static Point2D[] CreateControls(
        IReadOnlyList<SmoothingPoint2D> anchors,
        TravelDirection direction,
        double strength,
        double reserveMeters,
        CancellationToken cancellationToken)
    {
        var 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);
        Point2D startProposed = new Point2D(
            anchors[0].X + startHandleLength * Math.Cos(startTravelHeading),
            anchors[0].Y + startHandleLength * Math.Sin(startTravelHeading));
        controls[1] = ClampDisplacement(anchors[1], startProposed, GetAllowedRadius(anchors[1], reserveMeters));

        int finalIndex = anchors.Count - 1;
        double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * EndpointTangentScale * strength;
        double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction);
        Point2D endProposed = new Point2D(
            anchors[finalIndex].X - endHandleLength * Math.Cos(endTravelHeading),
            anchors[finalIndex].Y - endHandleLength * Math.Sin(endTravelHeading));
        controls[finalIndex - 1] = ClampDisplacement(
            anchors[finalIndex - 1], endProposed, GetAllowedRadius(anchors[finalIndex - 1], reserveMeters));

        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))
                return null;
        }
        return controls;
    }

    private static void AddSample(
        double parameter,
        IReadOnlyList<SmoothingPoint2D> anchors,
        Point2D[] controls,
        double[] knots,
        List<SmoothingPoint2D> output,
        CancellationToken cancellationToken)
    {
        cancellationToken.ThrowIfCancellationRequested();
        Point2D evaluated = Evaluate(controls, knots, parameter);
        SmoothingPoint2D reference = InterpolateAnchor(anchors, parameter);
        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));
    }

    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 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);
        }
    }
}