ParkingRobot/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;

/// <summary>
/// 原始路径基线与一次 Local G2 平滑的离线比较入口。
/// 此类仅汇总已冻结请求的质量、确定性摘要和耗时，不参与实时控制或修改平滑结果。
/// </summary>
public sealed class PathSmoothingComparisonService
{
    private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
    private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
    private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
    private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();

    /// <summary>创建原始基线并比较一次 Local G2 运行的质量、耗时和推荐资格。</summary>
    /// <param name="request">比较请求，包含待平滑的不可变请求快照和比较设置；为 <see langword="null"/> 时返回带失败基线的结果。</param>
    /// <param name="cancellationToken">取消比较和重复计时的调用方令牌。</param>
    /// <returns>包含原始基线、候选条目和推荐方法的不可变比较结果；取消时 <c>Cancelled</c> 为 <see langword="true"/>。</returns>
    public PathSmoothingComparisonResult Compare(
        PathSmoothingComparisonRequest request,
        CancellationToken cancellationToken = default)
    {
        PathSmoothingComparisonEntry baseline = CreateRawPathBaseline(request, out string baselineReason);
        var entries = new List<PathSmoothingComparisonEntry>(1);

        if (cancellationToken.IsCancellationRequested)
            return Cancelled(baseline, entries);

        if (request == null || request.SmoothingRequest == null)
            return new PathSmoothingComparisonResult(baseline, entries, null, false, baselineReason);

        if (!TryCompareLocalG2(
                request.SmoothingRequest,
                baseline.Metrics,
                cancellationToken,
                out PathSmoothingComparisonEntry entry))
        {
            return Cancelled(baseline, entries);
        }

        entries.Add(entry);
        return new PathSmoothingComparisonResult(
            baseline,
            entries,
            entry.IsEligibleForRecommendation ? SmoothingMethod.LocalG2Quintic : null,
            false,
            baselineReason);
    }

    private bool TryCompareLocalG2(
        PathSmoothingRequest smoothingRequest,
        PathQualityMetrics rawMetrics,
        CancellationToken cancellationToken,
        out PathSmoothingComparisonEntry entry)
    {
        entry = null;
        try
        {
            cancellationToken.ThrowIfCancellationRequested();
            PathSmoothingResult warmup = _smoothingService.Smooth(smoothingRequest, cancellationToken);
            if (warmup.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;

            var timings = new List<double>(5);
            var measuredResults = new List<PathSmoothingResult>(5);
            for (int sampleIndex = 0; sampleIndex < 5; sampleIndex++)
            {
                cancellationToken.ThrowIfCancellationRequested();
                Stopwatch stopwatch = Stopwatch.StartNew();
                PathSmoothingResult result = _smoothingService.Smooth(smoothingRequest, cancellationToken);
                stopwatch.Stop();
                if (result.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
                timings.Add(stopwatch.Elapsed.TotalMilliseconds);
                measuredResults.Add(result);
            }

            PathSmoothingResult canonical = measuredResults[0];
            string digest = StableGeometryDigest.Compute(canonical);
            SmoothingTimingSummary timing = SmoothingTimingSummary.FromMeasurements(timings, measuredResults);
            string diagnostic = string.IsNullOrWhiteSpace(timing.Diagnostic)
                ? canonical.Diagnostics.TerminationReason
                : timing.Diagnostic;
            PathQualityMetrics metrics = PathSmoothingComparisonEntry.IsPublishedLocalG2Status(canonical.Status)
                ? NormalizeMetrics(canonical.Diagnostics.Metrics, rawMetrics)
                : new PathQualityMetrics();
            entry = PathSmoothingComparisonEntry.CreateCandidate(
                SmoothingMethod.LocalG2Quintic,
                canonical.Status,
                metrics,
                timing,
                digest,
                diagnostic,
                canonical.Path,
                canonical.Segments);
            return true;
        }
        catch (OperationCanceledException)
        {
            return false;
        }
        catch (Exception exception)
        {
            entry = PathSmoothingComparisonEntry.CreateCandidate(
                SmoothingMethod.LocalG2Quintic,
                PathSmoothingStatus.Failed,
                new PathQualityMetrics(),
                new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, exception.GetType().Name),
                string.Empty,
                exception.Message,
                null,
                null);
            return true;
        }
    }

    private PathSmoothingComparisonEntry CreateRawPathBaseline(
        PathSmoothingComparisonRequest request,
        out string reason)
    {
        reason = string.Empty;
        if (request == null || request.SmoothingRequest == null)
            return FailedBaseline(PathSmoothingStatus.InvalidInput, "Comparison request is required.", out reason);

        PathSmoothingRequest smoothingRequest = request.SmoothingRequest;
        PathSmoothingConfiguration configuration = smoothingRequest.Configuration;
        if (configuration == null || smoothingRequest.Map == null || smoothingRequest.Vehicle == null)
        {
            return FailedBaseline(
                PathSmoothingStatus.InvalidInput,
                "A planning-ready map, vehicle, and configuration are required.",
                out reason);
        }

        if (!_preprocessor.TryPrepare(smoothingRequest, out PreparedPath preparedPath, out reason))
            return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
        if (!RawPathBaselineBuilder.TryCreate(
                smoothingRequest,
                preparedPath,
                _analyzer,
                configuration.OutputSpacingMeters,
                _validator,
                configuration.MaximumCollisionCheckStepMeters,
                out RawPathBaseline rawPath,
                out reason))
        {
            return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
        }

        string digest = StableGeometryDigest.Compute(PathSmoothingStatus.NotNeeded, null, rawPath.Path, rawPath.Segments);
        return PathSmoothingComparisonEntry.CreateRawPathBaseline(
            PathSmoothingStatus.NotNeeded,
            rawPath.Metrics,
            digest,
            string.Empty,
            rawPath.Path,
            rawPath.Segments);
    }

    private static PathSmoothingComparisonEntry FailedBaseline(
        PathSmoothingStatus status,
        string failureReason,
        out string reason)
    {
        reason = failureReason ?? string.Empty;
        return PathSmoothingComparisonEntry.CreateRawPathBaseline(
            status,
            new PathQualityMetrics(),
            StableGeometryDigest.Compute(status, null, null, null),
            reason,
            null,
            null);
    }

    private static PathSmoothingComparisonResult Cancelled(
        PathSmoothingComparisonEntry baseline,
        IReadOnlyList<PathSmoothingComparisonEntry> entries)
    {
        return new PathSmoothingComparisonResult(baseline, entries, null, true, "Path smoothing comparison was cancelled.");
    }

    private static PathQualityMetrics NormalizeMetrics(
        PathQualityMetrics candidate,
        PathQualityMetrics raw)
    {
        if (candidate == null || raw == null || !candidate.IsFeasible)
            return new PathQualityMetrics();

        return new PathQualityMetrics(
            true,
            candidate.PathLengthMeters,
            candidate.MaximumAbsoluteVehicleCurvaturePerMeter,
            candidate.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
            candidate.RootMeanSquareVehicleCurvaturePerMeter,
            candidate.TotalAbsoluteCurvatureVariationPerMeter,
            candidate.CurvatureVariationEnergy,
            candidate.MinimumBodyClearanceMeters,
            RelativePercentOrAbsoluteDelta(candidate.PathLengthMeters, raw.PathLengthMeters),
            RelativePercentOrAbsoluteDelta(
                candidate.MaximumAbsoluteVehicleCurvaturePerMeter,
                raw.MaximumAbsoluteVehicleCurvaturePerMeter),
            RelativePercentOrAbsoluteDelta(
                candidate.TotalAbsoluteCurvatureVariationPerMeter,
                raw.TotalAbsoluteCurvatureVariationPerMeter),
            candidate.MinimumBodyClearanceMeters - raw.MinimumBodyClearanceMeters);
    }

    private static double RelativePercentOrAbsoluteDelta(double candidate, double raw)
    {
        double delta = candidate - raw;
        return Math.Abs(raw) < 1e-12d ? delta : delta / raw * 100d;
    }
}