using System.Collections.Generic;
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))
        {
            return false;
        }

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

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