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

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;

/// <summary>在每个单独方向段内统一重采样、恢复航向并计算几何曲率。</summary>
public sealed class PathGeometryAnalyzer
{
    private const double MinimumDistanceMeters = 1e-10d;
    private const double BoundaryToleranceMeters = 1e-8d;

    /// <summary>
    /// 对候选方向段进行确定性几何分析。换向点两侧永不参与同一次差分。
    /// </summary>
    public bool TryAnalyze(
        IReadOnlyList<PreparedDirectionSegment> candidateSegments,
        double spacingMeters,
        out PathGeometryAnalysis analysis,
        out string reason)
    {
        analysis = null;
        reason = string.Empty;
        if (candidateSegments == null || candidateSegments.Count == 0 ||
            !NumericGuard.IsPositiveFinite(spacingMeters))
        {
            reason = "候选方向段或输出采样间距无效。";
            return false;
        }

        var outputPath = new List<SmoothedPathPoint>();
        var outputSegments = new List<SmoothedPathSegment>();
        double cumulativeArcLength = 0d;
        double previousOutputHeading = 0d;
        double previousOutputUnwrappedHeading = 0d;
        bool hasPreviousOutputHeading = false;
        double maximumAbsoluteVehicleCurvature = 0d;
        double maximumAbsoluteVehicleCurvatureDerivative = 0d;
        double curvatureSquareSum = 0d;
        int curvatureSampleCount = 0;
        double totalCurvatureVariation = 0d;
        double curvatureVariationEnergy = 0d;
        double minimumClearance = double.PositiveInfinity;

        for (int segmentIndex = 0; segmentIndex < candidateSegments.Count; segmentIndex++)
        {
            PreparedDirectionSegment segment = candidateSegments[segmentIndex];
            if (!IsValidSegment(segment, segmentIndex, out reason)) return false;
            if (!TryValidateBoundary(candidateSegments, segmentIndex, out reason)) return false;

            if (!TryResampleByGeometry(segment.Points, spacingMeters, out IReadOnlyList<SmoothingPoint2D> samples, out reason))
                return false;

            if (!TryAnalyzeSegment(
                    segment,
                    samples,
                    ref cumulativeArcLength,
                    ref previousOutputHeading,
                    ref previousOutputUnwrappedHeading,
                    ref hasPreviousOutputHeading,
                    outputPath,
                    out double segmentMaximumCurvature,
                    out double segmentMaximumCurvatureDerivative,
                    out double segmentCurvatureSquareSum,
                    out int segmentCurvatureSampleCount,
                    out double segmentVariation,
                    out double segmentVariationEnergy,
                    out double segmentMinimumClearance,
                    out reason))
            {
                return false;
            }

            maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, segmentMaximumCurvature);
            maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
                maximumAbsoluteVehicleCurvatureDerivative,
                segmentMaximumCurvatureDerivative);
            curvatureSquareSum += segmentCurvatureSquareSum;
            curvatureSampleCount += segmentCurvatureSampleCount;
            totalCurvatureVariation += segmentVariation;
            curvatureVariationEnergy += segmentVariationEnergy;
            minimumClearance = Math.Min(minimumClearance, segmentMinimumClearance);
            int endIndex = outputPath.Count - 1;
            int startIndex = endIndex - samples.Count + 1;
            outputSegments.Add(new SmoothedPathSegment(
                segment.SegmentIndex,
                segment.Direction,
                startIndex,
                endIndex,
                segment.StartsAtGearSwitch,
                segment.EndsAtGearSwitch));
        }

        double rmsCurvature = curvatureSampleCount == 0 ? 0d : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
        analysis = new PathGeometryAnalysis(
            outputPath,
            outputSegments,
            cumulativeArcLength,
            maximumAbsoluteVehicleCurvature,
            maximumAbsoluteVehicleCurvatureDerivative,
            rmsCurvature,
            totalCurvatureVariation,
            curvatureVariationEnergy,
            minimumClearance);
        return true;
    }

    private static bool TryAnalyzeSegment(
        PreparedDirectionSegment segment,
        IReadOnlyList<SmoothingPoint2D> samples,
        ref double cumulativeArcLength,
        ref double previousOutputHeading,
        ref double previousOutputUnwrappedHeading,
        ref bool hasPreviousOutputHeading,
        List<SmoothedPathPoint> output,
        out double maximumAbsoluteVehicleCurvature,
        out double maximumAbsoluteVehicleCurvatureDerivative,
        out double curvatureSquareSum,
        out int curvatureSampleCount,
        out double totalCurvatureVariation,
        out double curvatureVariationEnergy,
        out double minimumClearance,
        out string reason)
    {
        maximumAbsoluteVehicleCurvature = 0d;
        maximumAbsoluteVehicleCurvatureDerivative = 0d;
        curvatureSquareSum = 0d;
        curvatureSampleCount = 0;
        totalCurvatureVariation = 0d;
        curvatureVariationEnergy = 0d;
        minimumClearance = double.PositiveInfinity;
        reason = string.Empty;
        int count = samples.Count;
        var localArcLengths = new double[count];
        var headings = new double[count];
        var unwrappedHeadings = new double[count];
        var geometricCurvatures = new double[count];
        var vehicleCurvatures = new double[count];
        var curvatureDerivatives = new double[count];

        for (int index = 1; index < count; index++)
        {
            double distance = Distance(samples[index - 1], samples[index]);
            if (!NumericGuard.IsFinite(distance) || distance <= MinimumDistanceMeters)
            {
                reason = "同一方向段中包含重复或退化的路径点。";
                return false;
            }
            localArcLengths[index] = localArcLengths[index - 1] + distance;
        }

        for (int index = 0; index < count; index++)
        {
            double travelHeading;
            if (index == 0 || index == count - 1)
            {
                // Coarse-path endpoints encode the vehicle pose at the exact integration anchor.
                // A chord across a finite integration step is not that pose's heading.
                travelHeading = segment.Direction == TravelDirection.Forward
                    ? samples[index].Heading
                    : samples[index].Heading - Math.PI;
            }
            else
            {
                travelHeading = Math.Atan2(samples[index + 1].Y - samples[index - 1].Y,
                    samples[index + 1].X - samples[index - 1].X);
            }

            double heading = AngleMath.NormalizeRadians(
                segment.Direction == TravelDirection.Forward ? travelHeading : travelHeading + Math.PI);
            if (!NumericGuard.IsFinite(heading))
            {
                reason = "候选路径航向无法归一化。";
                return false;
            }

            headings[index] = heading;
            if (index == 0 && !hasPreviousOutputHeading)
            {
                unwrappedHeadings[index] = heading;
            }
            else if (index == 0)
            {
                unwrappedHeadings[index] = previousOutputUnwrappedHeading +
                    AngleMath.ShortestSignedDifference(previousOutputHeading, heading);
            }
            else
            {
                unwrappedHeadings[index] = unwrappedHeadings[index - 1] +
                    AngleMath.ShortestSignedDifference(headings[index - 1], heading);
            }
        }

        for (int index = 0; index < count; index++)
        {
            if (count == 1)
            {
                geometricCurvatures[index] = 0d;
            }
            else
            {
                int leftIndex = index == 0 ? 0 : index - 1;
                int rightIndex = index == count - 1 ? count - 1 : index + 1;
                if (!TryEstimateGeometricCurvature(
                        samples,
                        unwrappedHeadings,
                        leftIndex,
                        rightIndex,
                        out geometricCurvatures[index],
                        out reason))
                {
                    return false;
                }
            }

            if (!NumericGuard.IsFinite(geometricCurvatures[index]))
            {
                reason = "候选路径曲率计算产生了非法数值。";
                return false;
            }
        }

        for (int index = 0; index < count; index++)
        {
            double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
            vehicleCurvatures[index] = directionSign * geometricCurvatures[index];
        }

        if (segment.StartVehicleCurvaturePerMeter.HasValue)
        {
            double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
            vehicleCurvatures[0] = segment.StartVehicleCurvaturePerMeter.Value;
            geometricCurvatures[0] = directionSign * vehicleCurvatures[0];
        }

        for (int index = 0; index < count; index++)
        {
            if (count == 1)
            {
                curvatureDerivatives[index] = 0d;
            }
            else if (index == 0)
            {
                curvatureDerivatives[index] =
                    (vehicleCurvatures[1] - vehicleCurvatures[0]) /
                    (localArcLengths[1] - localArcLengths[0]);
            }
            else if (index == count - 1)
            {
                curvatureDerivatives[index] =
                    (vehicleCurvatures[index] - vehicleCurvatures[index - 1]) /
                    (localArcLengths[index] - localArcLengths[index - 1]);
            }
            else
            {
                curvatureDerivatives[index] =
                    (vehicleCurvatures[index + 1] - vehicleCurvatures[index - 1]) /
                    (localArcLengths[index + 1] - localArcLengths[index - 1]);
            }

            if (!NumericGuard.IsFinite(curvatureDerivatives[index]))
            {
                reason = "候选路径曲率导数计算产生了非法数值。";
                return false;
            }
        }

        for (int index = 0; index < count; index++)
        {
            SmoothingPoint2D sample = samples[index];
            double vehicleCurvature = vehicleCurvatures[index];
            double arcLength = cumulativeArcLength + localArcLengths[index];
            bool isGearSwitch = index == 0 && segment.StartsAtGearSwitch;
            SmoothedPathPointSource source = isGearSwitch ? SmoothedPathPointSource.GearSwitch : sample.Source;
            output.Add(new SmoothedPathPoint(
                sample.X,
                sample.Y,
                headings[index],
                unwrappedHeadings[index],
                arcLength,
                segment.Direction,
                geometricCurvatures[index],
                vehicleCurvature,
                curvatureDerivatives[index],
                sample.BodyClearance,
                isGearSwitch,
                source));

            maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(vehicleCurvature));
            maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
                maximumAbsoluteVehicleCurvatureDerivative,
                Math.Abs(curvatureDerivatives[index]));
            curvatureSquareSum += vehicleCurvature * vehicleCurvature;
            curvatureSampleCount++;
            minimumClearance = Math.Min(minimumClearance, sample.BodyClearance);
            if (index > 0)
            {
                double deltaCurvature = geometricCurvatures[index] - geometricCurvatures[index - 1];
                double deltaArc = localArcLengths[index] - localArcLengths[index - 1];
                totalCurvatureVariation += Math.Abs(deltaCurvature);
                curvatureVariationEnergy += (deltaCurvature / deltaArc) * (deltaCurvature / deltaArc) * deltaArc;
            }
        }

        cumulativeArcLength += localArcLengths[count - 1];
        previousOutputHeading = headings[count - 1];
        previousOutputUnwrappedHeading = unwrappedHeadings[count - 1];
        hasPreviousOutputHeading = true;
        return true;
    }

    private static bool TryEstimateGeometricCurvature(
        IReadOnlyList<SmoothingPoint2D> samples,
        IReadOnlyList<double> unwrappedHeadings,
        int leftIndex,
        int rightIndex,
        out double curvature,
        out string reason)
    {
        curvature = 0d;
        reason = string.Empty;
        double chordLength = Distance(samples[leftIndex], samples[rightIndex]);
        if (!NumericGuard.IsFinite(chordLength) || chordLength <= 0d)
        {
            reason = "候选路径曲率估计弦长无效。";
            return false;
        }

        double deltaHeading = unwrappedHeadings[rightIndex] - unwrappedHeadings[leftIndex];
        if (!NumericGuard.IsFinite(deltaHeading))
        {
            reason = "候选路径曲率估计航向差无效。";
            return false;
        }
        if (Math.Abs(deltaHeading) >= Math.PI)
        {
            reason = "候选路径曲率估计航向差存在π歧义。";
            return false;
        }

        curvature = 2d * Math.Sin(deltaHeading / 2d) / chordLength;
        if (!NumericGuard.IsFinite(curvature))
        {
            reason = "候选路径曲率计算产生了非法数值。";
            return false;
        }
        return true;
    }

    private static bool TryResampleByGeometry(
        IReadOnlyList<SmoothingPoint2D> input,
        double spacingMeters,
        out IReadOnlyList<SmoothingPoint2D> samples,
        out string reason)
    {
        samples = null;
        reason = string.Empty;
        var normalized = new List<SmoothingPoint2D>(input.Count);
        double localArcLength = 0d;
        for (int index = 0; index < input.Count; index++)
        {
            SmoothingPoint2D point = input[index];
            if (!IsValidPoint(point))
            {
                reason = "候选路径点包含非法数值。";
                return false;
            }

            if (index > 0)
            {
                double distance = Distance(input[index - 1], point);
                if (!NumericGuard.IsFinite(distance) || distance <= MinimumDistanceMeters)
                {
                    reason = "同一方向段中包含重复或退化的路径点。";
                    return false;
                }
                localArcLength += distance;
            }

            normalized.Add(new SmoothingPoint2D(
                point.X, point.Y, localArcLength, point.Heading, point.UnwrappedHeading,
                point.BodyClearance, point.IsGearSwitchPoint, point.Source));
        }

        if (normalized.Count == 1)
        {
            samples = normalized;
            return true;
        }

        var result = new List<SmoothingPoint2D> { normalized[0] };
        double finalArc = normalized[normalized.Count - 1].ArcLength;
        int rightIndex = 1;
        for (double targetArc = spacingMeters; targetArc < finalArc - MinimumDistanceMeters; targetArc += spacingMeters)
        {
            while (rightIndex < normalized.Count - 1 && normalized[rightIndex].ArcLength < targetArc)
                rightIndex++;
            SmoothingPoint2D left = normalized[rightIndex - 1];
            SmoothingPoint2D right = normalized[rightIndex];
            double ratio = (targetArc - left.ArcLength) / (right.ArcLength - left.ArcLength);
            double unwrappedHeading = left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading);
            result.Add(new SmoothingPoint2D(
                left.X + ratio * (right.X - left.X),
                left.Y + ratio * (right.Y - left.Y),
                targetArc,
                AngleMath.NormalizeRadians(unwrappedHeading),
                unwrappedHeading,
                Math.Min(left.BodyClearance, right.BodyClearance),
                false,
                SmoothedPathPointSource.Interpolated));
        }

        result.Add(normalized[normalized.Count - 1]);
        samples = result;
        return true;
    }

    private static bool IsValidSegment(PreparedDirectionSegment segment, int expectedIndex, out string reason)
    {
        reason = string.Empty;
        if (segment == null || segment.SegmentIndex != expectedIndex ||
            (segment.Direction != TravelDirection.Forward && segment.Direction != TravelDirection.Reverse) ||
            segment.Points == null || segment.Points.Count == 0)
        {
            reason = "候选方向段索引、方向或点集无效。";
            return false;
        }
        return true;
    }

    private static bool TryValidateBoundary(
        IReadOnlyList<PreparedDirectionSegment> segments,
        int segmentIndex,
        out string reason)
    {
        reason = string.Empty;
        PreparedDirectionSegment current = segments[segmentIndex];
        if (segmentIndex == segments.Count - 1 && current.EndsAtGearSwitch)
        {
            reason = "末个方向段不得声明不存在的后续换向点。";
            return false;
        }
        if (segmentIndex == 0)
        {
            if (current.StartsAtGearSwitch)
            {
                reason = "首个方向段不得从换向点开始。";
                return false;
            }
            return true;
        }

        PreparedDirectionSegment previous = segments[segmentIndex - 1];
        if (previous == null || !previous.EndsAtGearSwitch || !current.StartsAtGearSwitch ||
            previous.Direction == current.Direction)
        {
            reason = "方向段边界必须是前后成对且方向相反的换向点。";
            return false;
        }

        SmoothingPoint2D previousEnd = previous.Points[previous.Points.Count - 1];
        SmoothingPoint2D currentStart = current.Points[0];
        if (!currentStart.IsGearSwitchPoint ||
            Math.Abs(previousEnd.X - currentStart.X) > BoundaryToleranceMeters ||
            Math.Abs(previousEnd.Y - currentStart.Y) > BoundaryToleranceMeters ||
            Math.Abs(AngleMath.ShortestSignedDifference(previousEnd.Heading, currentStart.Heading)) > BoundaryToleranceMeters)
        {
            reason = "换向点两侧必须保留同一位姿和航向。";
            return false;
        }
        return true;
    }

    private static bool IsValidPoint(SmoothingPoint2D point)
    {
        return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
            NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
            NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.BodyClearance) &&
            point.BodyClearance >= 0d;
    }

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