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 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 segmentCurvatureSquareSum,
                    out int segmentCurvatureSampleCount,
                    out double segmentVariation,
                    out double segmentVariationEnergy,
                    out double segmentMinimumClearance,
                    out reason))
            {
                return false;
            }

            maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, segmentMaximumCurvature);
            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,
            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 curvatureSquareSum,
        out int curvatureSampleCount,
        out double totalCurvatureVariation,
        out double curvatureVariationEnergy,
        out double minimumClearance,
        out string reason)
    {
        maximumAbsoluteVehicleCurvature = 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];

        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 if (index == 0)
            {
                geometricCurvatures[index] = (unwrappedHeadings[1] - unwrappedHeadings[0]) /
                    (localArcLengths[1] - localArcLengths[0]);
            }
            else if (index == count - 1)
            {
                geometricCurvatures[index] = (unwrappedHeadings[index] - unwrappedHeadings[index - 1]) /
                    (localArcLengths[index] - localArcLengths[index - 1]);
            }
            else
            {
                geometricCurvatures[index] = (unwrappedHeadings[index + 1] - unwrappedHeadings[index - 1]) /
                    (localArcLengths[index + 1] - localArcLengths[index - 1]);
            }

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

        for (int index = 0; index < count; index++)
        {
            SmoothingPoint2D sample = samples[index];
            double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
            double vehicleCurvature = directionSign * geometricCurvatures[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,
                sample.BodyClearance,
                isGearSwitch,
                source));

            maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(vehicleCurvature));
            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 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);
    }
}