using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;

/// <summary>通过候选路径同一几何分析器和验证器构建安全、可公平比较的原始基线。</summary>
internal static class RawPathBaselineBuilder
{
    internal static bool TryCreate(
        PathSmoothingRequest request,
        PreparedPath preparedPath,
        PathGeometryAnalyzer analyzer,
        double outputSpacingMeters,
        SmoothedPathValidator validator,
        double maximumCollisionCheckStepMeters,
        out RawPathBaseline baseline,
        out string reason)
    {
        baseline = null;
        reason = string.Empty;
        if (request == null || preparedPath == null || analyzer == null || validator == null)
        {
            reason = "原始粗路径基线缺少请求、预处理路径、几何分析器或安全验证器。";
            return false;
        }

        if (!analyzer.TryAnalyze(preparedPath.Segments, outputSpacingMeters, out PathGeometryAnalysis analysis, out reason))
            return false;

        if (validator.TryValidate(
                analysis.Path,
                analysis.Segments,
                preparedPath,
                request.Map,
                request.Vehicle,
                maximumCollisionCheckStepMeters,
                out IReadOnlyList<SmoothedPathPoint> safePath,
                out double minimumClearanceMeters,
                out reason))
        {
            baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, minimumClearanceMeters));
            return true;
        }

        if (reason != "平滑路径包含非法数值或超限车辆曲率。" ||
            !HasTrustedVehicleCurvaturesWithinLimit(request) ||
            !TryCreateTrustedRawAnalysis(request, out analysis, out reason) ||
            !validator.TryValidate(
                analysis.Path,
                analysis.Segments,
                preparedPath,
                request.Map,
                request.Vehicle,
                maximumCollisionCheckStepMeters,
                out safePath,
                out minimumClearanceMeters,
                out reason))
        {
            return false;
        }

        baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, minimumClearanceMeters));
        return true;
    }

    private static bool HasTrustedVehicleCurvaturesWithinLimit(PathSmoothingRequest request)
    {
        if (request?.CoarsePath == null || request.CoarsePath.Count == 0 ||
            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(
                request.Vehicle,
                out double maximumCurvaturePerMeter))
        {
            return false;
        }

        for (int index = 0; index < request.CoarsePath.Count; index++)
        {
            CoarsePathPoint point = request.CoarsePath[index];
            if (point == null || !IsFinite(point.VehicleCurvature) ||
                Math.Abs(point.VehicleCurvature) > maximumCurvaturePerMeter)
            {
                return false;
            }
        }
        return true;
    }

    private static bool TryCreateTrustedRawAnalysis(
        PathSmoothingRequest request,
        out PathGeometryAnalysis analysis,
        out string reason)
    {
        analysis = null;
        reason = string.Empty;
        if (request.CoarsePath == null || request.Segments == null ||
            request.CoarsePath.Count == 0 || request.Segments.Count == 0)
        {
            reason = "原始粗路径基线缺少可信路径或方向分段。";
            return false;
        }

        var path = new List<SmoothedPathPoint>(request.CoarsePath.Count);
        var segments = new List<SmoothedPathSegment>(request.Segments.Count);
        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 < request.Segments.Count; segmentIndex++)
        {
            PathSegment segment = request.Segments[segmentIndex];
            if (segment == null || segment.SegmentIndex != segmentIndex ||
                segment.StartIndex != path.Count || segment.EndIndex < segment.StartIndex ||
                segment.EndIndex >= request.CoarsePath.Count)
            {
                reason = "原始粗路径基线方向分段无效。";
                return false;
            }

            for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
            {
                CoarsePathPoint point = request.CoarsePath[pointIndex];
                if (point == null || point.Direction != segment.Direction ||
                    !IsFinite(point.X) || !IsFinite(point.Y) || !IsFinite(point.Heading) ||
                    !IsFinite(point.UnwrappedHeading) || !IsFinite(point.ArcLength) || point.ArcLength < 0d ||
                    !IsFinite(point.VehicleCurvature) || !IsFinite(point.BodyClearance) || point.BodyClearance < 0d)
                {
                    reason = "原始粗路径基线包含非法可信点。";
                    return false;
                }

                double derivative = EstimateVehicleCurvatureDerivative(request.CoarsePath, segment, pointIndex, out bool validDerivative);
                if (!validDerivative)
                {
                    reason = "原始粗路径基线曲率导数弧长无效。";
                    return false;
                }

                double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
                double geometricCurvature = directionSign * point.VehicleCurvature;
                SmoothedPathPointSource source = point.IsGearSwitchPoint
                    ? SmoothedPathPointSource.GearSwitch
                    : SmoothedPathPointSource.CoarsePathFallback;
                path.Add(new SmoothedPathPoint(
                    point.X,
                    point.Y,
                    point.Heading,
                    point.UnwrappedHeading,
                    point.ArcLength,
                    point.Direction,
                    geometricCurvature,
                    point.VehicleCurvature,
                    derivative,
                    point.BodyClearance,
                    point.IsGearSwitchPoint,
                    source));

                maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(point.VehicleCurvature));
                maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
                    maximumAbsoluteVehicleCurvatureDerivative,
                    Math.Abs(derivative));
                curvatureSquareSum += point.VehicleCurvature * point.VehicleCurvature;
                curvatureSampleCount++;
                minimumClearance = Math.Min(minimumClearance, point.BodyClearance);
                if (pointIndex > segment.StartIndex)
                {
                    CoarsePathPoint previous = request.CoarsePath[pointIndex - 1];
                    double deltaArc = point.ArcLength - previous.ArcLength;
                    double deltaCurvature = geometricCurvature -
                        (directionSign * previous.VehicleCurvature);
                    totalCurvatureVariation += Math.Abs(deltaCurvature);
                    curvatureVariationEnergy +=
                        (deltaCurvature / deltaArc) * (deltaCurvature / deltaArc) * deltaArc;
                }
            }

            segments.Add(new SmoothedPathSegment(
                segment.SegmentIndex,
                segment.Direction,
                segment.StartIndex,
                segment.EndIndex,
                segment.StartsAtGearSwitch,
                segment.EndsAtGearSwitch));
        }

        double rmsCurvature = curvatureSampleCount == 0 ? 0d : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
        analysis = new PathGeometryAnalysis(
            path,
            segments,
            request.CoarsePath[request.CoarsePath.Count - 1].ArcLength,
            maximumAbsoluteVehicleCurvature,
            maximumAbsoluteVehicleCurvatureDerivative,
            rmsCurvature,
            totalCurvatureVariation,
            curvatureVariationEnergy,
            minimumClearance);
        return true;
    }

    private static double EstimateVehicleCurvatureDerivative(
        IReadOnlyList<CoarsePathPoint> path,
        PathSegment segment,
        int pointIndex,
        out bool valid)
    {
        valid = true;
        if (segment.StartIndex == segment.EndIndex) return 0d;

        int leftIndex = pointIndex == segment.StartIndex ? pointIndex : pointIndex - 1;
        int rightIndex = pointIndex == segment.EndIndex ? pointIndex : pointIndex + 1;
        double deltaArc = path[rightIndex].ArcLength - path[leftIndex].ArcLength;
        if (!IsFinite(deltaArc) || deltaArc <= 0d)
        {
            valid = false;
            return 0d;
        }
        return (path[rightIndex].VehicleCurvature - path[leftIndex].VehicleCurvature) / deltaArc;
    }

    private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);

    private static PathQualityMetrics CreateMetrics(
        PathGeometryAnalysis analysis,
        double minimumClearanceMeters)
    {
        return new PathQualityMetrics(
            true,
            analysis.PathLengthMeters,
            analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
            analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
            analysis.RootMeanSquareVehicleCurvaturePerMeter,
            analysis.TotalAbsoluteCurvatureVariationPerMeter,
            analysis.CurvatureVariationEnergy,
            minimumClearanceMeters,
            0d,
            0d,
            0d,
            0d);
    }
}

/// <summary>已通过完整车体复核的原始粗路径及其比较指标。</summary>
internal sealed class RawPathBaseline
{
    internal RawPathBaseline(
        IReadOnlyList<SmoothedPathPoint> path,
        IReadOnlyList<SmoothedPathSegment> segments,
        PathQualityMetrics metrics)
    {
        Path = path;
        Segments = segments;
        Metrics = metrics;
    }

    internal IReadOnlyList<SmoothedPathPoint> Path { get; }
    internal IReadOnlyList<SmoothedPathSegment> Segments { get; }
    internal PathQualityMetrics Metrics { get; }
}