using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;

/// <summary>正式单算法路径平滑入口，负责输入校验、有限重试与经过复核的粗路径回退。</summary>
public sealed class PathSmoothingService
{
    private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
    private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner();
    private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
    private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
    private readonly IPathSmoother _bSpline = new CubicBSplineSmoother();
    private readonly IPathSmoother _bezier = new LocalCubicBezierSmoother();
    private readonly IPathSmoother _quintic = new PiecewiseQuinticSmoother();

    /// <summary>执行一次经过完整安全复核的单算法平滑。</summary>
    public PathSmoothingResult Smooth(
        PathSmoothingRequest request,
        CancellationToken cancellationToken = default)
    {
        var stopwatch = Stopwatch.StartNew();
        try
        {
            cancellationToken.ThrowIfCancellationRequested();
            if (!TryValidateRequest(request, out PathSmoothingConfiguration configuration, out string reason))
                return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);

            if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason))
                return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);

            cancellationToken.ThrowIfCancellationRequested();
            var input = new SmoothingAlgorithmInput(
                preparedPath,
                request.Map,
                request.Vehicle,
                configuration.MaximumCollisionCheckStepMeters,
                configuration.MinimumClearanceReserveMeters,
                new SmoothingOptionsSnapshot(configuration));
            SmoothingAlgorithmRunner.AlgorithmRunResult runResult = _runner.Run(
                Resolve(configuration.Method), input, configuration, cancellationToken);
            int retryCount = GetRetryCount(runResult.AttemptedStrengths);
            PathSmoothingDiagnostics diagnostics = new PathSmoothingDiagnostics(
                runResult.Metrics,
                stopwatch.Elapsed,
                retryCount,
                runResult.AcceptedStrength,
                runResult.Reason);

            if (runResult.Status == PathSmoothingStatus.Success)
            {
                return PathSmoothingResult.Success(
                    configuration.Method,
                    runResult.Path,
                    runResult.Segments,
                    diagnostics);
            }

            if (!configuration.AllowFallbackToCoarsePath)
                return PathSmoothingResult.Failure(runResult.Status, diagnostics);

            cancellationToken.ThrowIfCancellationRequested();
            if (!TryCreateVerifiedFallback(
                    request,
                    configuration,
                    cancellationToken,
                    out IReadOnlyList<SmoothedPathPoint> fallbackPath,
                    out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
                    out PathQualityMetrics fallbackMetrics,
                    out reason))
            {
                return PathSmoothingResult.Failure(runResult.Status, diagnostics);
            }

            var fallbackDiagnostics = new PathSmoothingDiagnostics(
                fallbackMetrics,
                stopwatch.Elapsed,
                retryCount,
                runResult.AcceptedStrength,
                runResult.Reason);
            return PathSmoothingResult.Fallback(
                configuration.Method,
                fallbackPath,
                fallbackSegments,
                fallbackDiagnostics);
        }
        catch (OperationCanceledException)
        {
            return Failure(PathSmoothingStatus.Cancelled, stopwatch, 0, 0d, "路径平滑已取消。");
        }
        catch (Exception exception)
        {
            return Failure(PathSmoothingStatus.Failed, stopwatch, 0, 0d, exception.Message);
        }
    }

    private bool TryCreateVerifiedFallback(
        PathSmoothingRequest request,
        PathSmoothingConfiguration configuration,
        CancellationToken cancellationToken,
        out IReadOnlyList<SmoothedPathPoint> fallbackPath,
        out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
        out PathQualityMetrics fallbackMetrics,
        out string reason)
    {
        fallbackPath = null;
        fallbackSegments = null;
        fallbackMetrics = null;
        reason = string.Empty;

        // Reprepare from the immutable request instead of reusing the algorithm input: fallback is a
        // separately published output and must repeat the coarse-path contract validation.
        if (!_preprocessor.TryPrepare(request, out PreparedPath revalidatedPath, out reason)) return false;
        cancellationToken.ThrowIfCancellationRequested();

        if (!_analyzer.TryAnalyze(
                ToFallbackSegments(revalidatedPath),
                configuration.OutputSpacingMeters,
                out PathGeometryAnalysis analysis,
                out reason))
        {
            return false;
        }

        IReadOnlyList<SmoothedPathPoint> fallbackSourcePath = ToFallbackPoints(analysis.Path);
        cancellationToken.ThrowIfCancellationRequested();
        if (!_validator.TryValidate(
                fallbackSourcePath,
                analysis.Segments,
                revalidatedPath,
                request.Map,
                request.Vehicle,
                configuration.MaximumCollisionCheckStepMeters,
                out IReadOnlyList<SmoothedPathPoint> safePath,
                out double minimumClearanceMeters,
                out reason))
        {
            return false;
        }

        fallbackPath = safePath;
        fallbackSegments = analysis.Segments;
        fallbackMetrics = CreateMetrics(analysis, minimumClearanceMeters);
        return true;
    }

    private IPathSmoother Resolve(SmoothingMethod method)
    {
        return method switch
        {
            SmoothingMethod.CubicBSpline => _bSpline,
            SmoothingMethod.LocalCubicBezier => _bezier,
            SmoothingMethod.PiecewiseQuintic => _quintic,
            _ => throw new ArgumentOutOfRangeException(nameof(method)),
        };
    }

    private static bool TryValidateRequest(
        PathSmoothingRequest request,
        out PathSmoothingConfiguration configuration,
        out string reason)
    {
        configuration = null;
        reason = string.Empty;
        if (request == null)
        {
            reason = "平滑请求为空。";
            return false;
        }

        configuration = request.Configuration;
        VehicleParameters vehicle = request.Vehicle;
        if (configuration == null || request.Map == null || !request.Map.PlanningReady || vehicle == null)
        {
            reason = "平滑请求缺少可用的地图、车辆或配置。";
            return false;
        }

        if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
            !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
            !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _))
        {
            reason = "平滑请求中的车辆几何或曲率约束无效。";
            return false;
        }

        if (!Enum.IsDefined(typeof(SmoothingMethod), configuration.Method))
        {
            reason = "平滑方法无效。";
            return false;
        }

        if (!NumericGuard.IsPositiveFinite(configuration.OutputSpacingMeters) ||
            !NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
            !NumericGuard.IsFinite(configuration.MinimumClearanceReserveMeters) ||
            configuration.MinimumClearanceReserveMeters < 0d ||
            !NumericGuard.IsPositiveFinite(configuration.SmoothingStrength) ||
            !NumericGuard.IsPositiveFinite(configuration.CubicBSpline.EndpointTangentScale) ||
            !IsValidBezierThreshold(configuration.LocalCubicBezier.CornerHeadingThresholdRadians) ||
            !NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.MaximumWindowLengthMeters) ||
            !NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.HandleLengthRatio) ||
            !NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.KnotSpacingMeters) ||
            !NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.MinimumKnotSpacingMeters) ||
            configuration.PiecewiseQuintic.KnotSpacingMeters < configuration.PiecewiseQuintic.MinimumKnotSpacingMeters)
        {
            reason = "平滑配置包含非法数值或不满足方法契约。";
            return false;
        }
        return true;
    }

    private static bool IsValidBezierThreshold(double thresholdRadians)
    {
        return NumericGuard.IsFinite(thresholdRadians) && thresholdRadians > 0d && thresholdRadians <= Math.PI;
    }

    private static IReadOnlyList<PreparedDirectionSegment> ToFallbackSegments(PreparedPath path)
    {
        var segments = new List<PreparedDirectionSegment>(path.Segments.Count);
        for (int segmentIndex = 0; segmentIndex < path.Segments.Count; segmentIndex++)
        {
            PreparedDirectionSegment segment = path.Segments[segmentIndex];
            var points = new List<SmoothingPoint2D>(segment.Points.Count);
            for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
            {
                SmoothingPoint2D point = segment.Points[pointIndex];
                points.Add(new SmoothingPoint2D(
                    point.X,
                    point.Y,
                    point.ArcLength,
                    point.Heading,
                    point.UnwrappedHeading,
                    point.BodyClearance,
                    point.IsGearSwitchPoint,
                    SmoothedPathPointSource.CoarsePathFallback));
            }
            segments.Add(new PreparedDirectionSegment(
                segment.SegmentIndex,
                segment.Direction,
                points,
                segment.StartsAtGearSwitch,
                segment.EndsAtGearSwitch));
        }
        return segments;
    }

    private static IReadOnlyList<SmoothedPathPoint> ToFallbackPoints(IReadOnlyList<SmoothedPathPoint> path)
    {
        var points = new List<SmoothedPathPoint>(path.Count);
        for (int index = 0; index < path.Count; index++)
        {
            SmoothedPathPoint point = path[index];
            points.Add(new SmoothedPathPoint(
                point.X,
                point.Y,
                point.Heading,
                point.UnwrappedHeading,
                point.ArcLength,
                point.Direction,
                point.GeometricCurvature,
                point.VehicleCurvature,
                point.BodyClearance,
                point.IsGearSwitchPoint,
                SmoothedPathPointSource.CoarsePathFallback));
        }
        return points;
    }

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

    private static int GetRetryCount(IReadOnlyList<double> attemptedStrengths)
    {
        return attemptedStrengths == null || attemptedStrengths.Count == 0 ? 0 : attemptedStrengths.Count - 1;
    }

    private static PathSmoothingResult Failure(
        PathSmoothingStatus status,
        Stopwatch stopwatch,
        int retryCount,
        double acceptedStrength,
        string reason)
    {
        return PathSmoothingResult.Failure(
            status,
            new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, retryCount, acceptedStrength, reason));
    }
}