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

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>通过专用服务路径生成、回滚并发布经独立验证的 Local G2 局部区域替换；不发布未经最终复核的候选。</summary>
internal sealed class LocalG2PreSmoothingPipeline
{
    private readonly CurvatureTransitionDetector _detector = new CurvatureTransitionDetector();
    private readonly LocalG2WindowPlanner _windowPlanner = new LocalG2WindowPlanner();
    private readonly LocalG2CandidateBuilder _builder = new LocalG2CandidateBuilder();
    private readonly LocalG2CandidateEvaluator _evaluator = new LocalG2CandidateEvaluator();
    private readonly LocalG2RegionWorkOrder _workOrder = new LocalG2RegionWorkOrder();
    private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
    private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();

    /// <summary>在预处理路径上检测曲率事件、评估候选、按确定顺序拼接，并执行全局回滚复核。</summary>
    /// <param name="request">不可变平滑请求，提供地图、车辆、配置和路径上下文。</param>
    /// <param name="preparedPath">按方向段准备并重采样的原始路径；局部弧长单位为 m。</param>
    /// <param name="rawBaseline">已独立复核的原始路径质量基线。</param>
    /// <param name="cancellationToken">调用方取消令牌；取消时返回空路径的 Cancelled 结果。</param>
    /// <returns>完整、部分、无需或保持原样时均只发布最终复核路径；失败/取消时不发布部分候选。</returns>
    internal PathSmoothingResult Smooth(
        PathSmoothingRequest request,
        PreparedPath preparedPath,
        RawPathBaseline rawBaseline,
        CancellationToken cancellationToken)
    {
        var stopwatch = Stopwatch.StartNew();
        try
        {
            cancellationToken.ThrowIfCancellationRequested();
            if (request == null || preparedPath == null || rawBaseline == null ||
                !VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double maximumCurvature))
            {
                return Failure(PathSmoothingStatus.Failed, stopwatch, "局部 G2 预平滑输入无效。");
            }

            var options = new LocalG2OptionsSnapshot(request.Configuration);
            if (!_detector.TryDetect(request, maximumCurvature, options, out IReadOnlyList<CurvatureTransition> transitions, out string reason) ||
                !_windowPlanner.TryPlan(preparedPath, transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out reason))
            {
                return Failure(PathSmoothingStatus.Failed, stopwatch, reason);
            }

            IReadOnlyList<LocalG2SmoothingRegion> reportOrder =
                new ReadOnlyCollection<LocalG2SmoothingRegion>(new List<LocalG2SmoothingRegion>(regions));
            if (!_workOrder.TryCreate(reportOrder, out IReadOnlyList<LocalG2SmoothingRegion> workRegions, out string orderReason))
                return Failure(PathSmoothingStatus.Failed, stopwatch, orderReason);

            PreparedPath current = preparedPath;
            var reportsByRegion = new Dictionary<LocalG2SmoothingRegion, PathSmoothingRegionReport>();
            var accepted = new List<AcceptedRegion>();
            int improvedCount = 0;

            foreach (LocalG2SmoothingRegion region in workRegions)
            {
                cancellationToken.ThrowIfCancellationRequested();
                IReadOnlyList<LocalG2CandidateGeometry> candidates = _builder.Build(
                    preparedPath.Segments[region.SegmentIndex], region,
                    request.Configuration.OutputSpacingMeters, options, cancellationToken);
                var evaluations = new List<LocalG2CandidateEvaluation>();
                for (int candidateIndex = 0; candidateIndex < candidates.Count; candidateIndex++)
                    evaluations.Add(_evaluator.Evaluate(
                        preparedPath, current, region, candidates[candidateIndex], request, options, cancellationToken));

                LocalG2CandidateEvaluation best = LocalG2CandidateEvaluator.SelectBest(evaluations);
                if (best.Accepted)
                {
                    accepted.Add(new AcceptedRegion(region, current, best));
                    current = best.SplicedPreparedPath;
                    improvedCount++;
                    reportsByRegion.Add(region, CreateImprovedReport(region, candidates.Count, best));
                }
                else
                {
                    reportsByRegion.Add(region, CreateRetainedReport(region, candidates.Count, best));
                }
            }

            if (!TryValidateFinal(current, preparedPath, rawBaseline, request, options, out IReadOnlyList<SmoothedPathPoint> path,
                    out IReadOnlyList<SmoothedPathSegment> segments, out PathQualityMetrics metrics, out reason))
            {
                for (int index = accepted.Count - 1; index >= 0; index--)
                {
                    AcceptedRegion rollback = accepted[index];
                    current = rollback.Before;
                    reportsByRegion[rollback.Region] = CreateRollbackReport(rollback.Region, rollback.Evaluation);
                    improvedCount--;
                    if (TryValidateFinal(current, preparedPath, rawBaseline, request, options, out path, out segments, out metrics, out reason))
                        break;
                }
            }

            if (metrics == null)
                return Failure(PathSmoothingStatus.Failed, stopwatch, reason);

            var reports = new List<PathSmoothingRegionReport>(reportOrder.Count);
            for (int reportIndex = 0; reportIndex < reportOrder.Count; reportIndex++)
                reports.Add(reportsByRegion[reportOrder[reportIndex]]);

            PathSmoothingStatus status;
            if (transitions.Count == 0) status = PathSmoothingStatus.NotNeeded;
            else if (improvedCount == regions.Count) status = PathSmoothingStatus.Complete;
            else if (improvedCount > 0) status = PathSmoothingStatus.PartialImprovement;
            else status = PathSmoothingStatus.Unchanged;
            return PathSmoothingResult.PublishLocalG2(
                status,
                path,
                segments,
                new PathSmoothingDiagnostics(metrics, stopwatch.Elapsed, reason ?? string.Empty),
                reports);
        }
        catch (OperationCanceledException)
        {
            return Failure(PathSmoothingStatus.Cancelled, stopwatch, "路径平滑已取消。");
        }
        catch (Exception exception)
        {
            return Failure(PathSmoothingStatus.Failed, stopwatch, exception.Message);
        }
    }

    private bool TryValidateFinal(
        PreparedPath current,
        PreparedPath rawPath,
        RawPathBaseline rawBaseline,
        PathSmoothingRequest request,
        LocalG2OptionsSnapshot options,
        out IReadOnlyList<SmoothedPathPoint> path,
        out IReadOnlyList<SmoothedPathSegment> segments,
        out PathQualityMetrics metrics,
        out string reason)
    {
        path = null;
        segments = null;
        metrics = null;
        reason = string.Empty;
        if (!_analyzer.TryAnalyze(current.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis analysis, out reason) ||
            !_validator.TryValidate(analysis.Path, analysis.Segments, rawPath, request.Map, request.Vehicle,
                request.Configuration.MaximumCollisionCheckStepMeters,
                request.Configuration.CurvatureLimitRadiusToleranceMeters,
                out IReadOnlyList<SmoothedPathPoint> safePath,
                out double minimumClearance, out reason) ||
            minimumClearance < request.Configuration.MinimumClearanceReserveMeters ||
            analysis.CurvatureVariationCost > rawBaseline.Metrics.CurvatureVariationCost *
                (1d + options.MaximumVariationCostRegressionRatio))
        {
            if (string.IsNullOrEmpty(reason)) reason = "局部 G2 完整路径复核未通过。";
            return false;
        }

        path = safePath;
        segments = analysis.Segments;
        metrics = new PathQualityMetrics(
            true,
            analysis.PathLengthMeters,
            analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
            analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
            analysis.RootMeanSquareVehicleCurvaturePerMeter,
            analysis.TotalAbsoluteCurvatureVariationPerMeter,
            analysis.CurvatureVariationCost,
            minimumClearance,
            0d, 0d, 0d, 0d);
        return true;
    }

    private static PathSmoothingRegionReport CreateImprovedReport(
        LocalG2SmoothingRegion region,
        int candidateCount,
        LocalG2CandidateEvaluation evaluation) =>
        CreateReport(region, candidateCount, evaluation, PathSmoothingRegionStatus.Improved, PathSmoothingRegionFailureReason.None);

    private static PathSmoothingRegionReport CreateRetainedReport(
        LocalG2SmoothingRegion region,
        int candidateCount,
        LocalG2CandidateEvaluation evaluation) =>
        CreateReport(region, candidateCount, evaluation, PathSmoothingRegionStatus.RetainedOriginal, evaluation.FailureReason);

    private static PathSmoothingRegionReport CreateRollbackReport(
        LocalG2SmoothingRegion region,
        LocalG2CandidateEvaluation evaluation) =>
        CreateReport(region, region.WindowVariants.Count, evaluation, PathSmoothingRegionStatus.RetainedOriginal,
            PathSmoothingRegionFailureReason.GlobalValidationRollback);

    private static PathSmoothingRegionReport CreateReport(
        LocalG2SmoothingRegion region,
        int candidateCount,
        LocalG2CandidateEvaluation evaluation,
        PathSmoothingRegionStatus status,
        PathSmoothingRegionFailureReason failureReason)
    {
        LocalG2WindowVariant window = region.WindowVariants[0];
        var jumps = new List<double>(region.Transitions.Count);
        for (int index = 0; index < region.Transitions.Count; index++)
            jumps.Add(region.Transitions[index].RightVehicleCurvaturePerMeter - region.Transitions[index].LeftVehicleCurvaturePerMeter);
        return new PathSmoothingRegionReport(
            region.SegmentIndex,
            window.StartArcLengthMeters,
            window.EndArcLengthMeters,
            jumps,
            window.EndArcLengthMeters - window.StartArcLengthMeters,
            window.EndArcLengthMeters - window.StartArcLengthMeters,
            window.LeftWindowLengthMeters,
            window.RightWindowLengthMeters,
            candidateCount,
            evaluation.CandidateIndex,
            status,
            failureReason,
            evaluation.RawPeakCurvatureDerivativePerSquareMeter,
            evaluation.ResultPeakCurvatureDerivativePerSquareMeter,
            evaluation.RawCurvatureVariationCost,
            evaluation.ResultCurvatureVariationCost,
            evaluation.MaximumDeviationMeters,
            evaluation.MinimumBodyClearanceMeters,
            evaluation.MaximumAbsoluteVehicleCurvaturePerMeter);
    }

    private static PathSmoothingResult Failure(PathSmoothingStatus status, Stopwatch stopwatch, string reason) =>
        PathSmoothingResult.Failure(status,
            new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, reason));

    private sealed class AcceptedRegion
    {
        internal AcceptedRegion(LocalG2SmoothingRegion region, PreparedPath before, LocalG2CandidateEvaluation evaluation)
        {
            Region = region;
            Before = before;
            Evaluation = evaluation;
        }

        internal LocalG2SmoothingRegion Region { get; }
        internal PreparedPath Before { get; }
        internal LocalG2CandidateEvaluation Evaluation { get; }
    }
}