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 PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
    private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner();
    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);

            if (!RawPathBaselineBuilder.TryCreate(
                    request,
                    preparedPath,
                    _analyzer,
                    configuration.OutputSpacingMeters,
                    _validator,
                    configuration.MaximumCollisionCheckStepMeters,
                    out _,
                    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 (!RawPathBaselineBuilder.TryCreate(
                request,
                revalidatedPath,
                _analyzer,
                configuration.OutputSpacingMeters,
                _validator,
                configuration.MaximumCollisionCheckStepMeters,
                out RawPathBaseline rawPath,
                out reason))
        {
            return false;
        }

        fallbackPath = ToFallbackPoints(rawPath.Path);
        fallbackSegments = rawPath.Segments;
        fallbackMetrics = rawPath.Metrics;
        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<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.VehicleCurvatureDerivative,
                point.BodyClearance,
                point.IsGearSwitchPoint,
                SmoothedPathPointSource.CoarsePathFallback));
        }
        return points;
    }

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