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

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

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;

/// <summary>以固定预热和五次测量隔离比较所有请求平滑方法的离线入口。</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>比较所有请求方法；一个方法的失败不会阻止其他方法，取消会停止后续启动。</summary>
    public PathSmoothingComparisonResult Compare(
        PathSmoothingComparisonRequest request,
        CancellationToken cancellationToken = default)
    {
        PathSmoothingComparisonEntry baseline = CreateRawPathBaseline(request, out string baselineReason);
        var entries = new List<PathSmoothingComparisonEntry>();

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

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

        for (int methodIndex = 0; methodIndex < request.Methods.Count; methodIndex++)
        {
            if (cancellationToken.IsCancellationRequested)
                return Cancelled(baseline, entries);

            SmoothingMethod method = request.Methods[methodIndex];
            if (!TryCompareMethod(
                    request.SmoothingRequest,
                    method,
                    baseline.Metrics,
                    cancellationToken,
                    out PathSmoothingComparisonEntry entry))
                return Cancelled(baseline, entries);
            entries.Add(entry);
        }

        return new PathSmoothingComparisonResult(
            baseline,
            entries,
            SmoothingMethodRanker.Rank(entries),
            false,
            baselineReason);
    }

    private bool TryCompareMethod(
        PathSmoothingRequest sourceRequest,
        SmoothingMethod method,
        PathQualityMetrics rawMetrics,
        CancellationToken cancellationToken,
        out PathSmoothingComparisonEntry entry)
    {
        entry = null;
        try
        {
            PathSmoothingRequest methodRequest = CreateMethodRequest(sourceRequest, method);
            cancellationToken.ThrowIfCancellationRequested();
            PathSmoothingResult warmup = _smoothingService.Smooth(methodRequest, 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(methodRequest, 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 = canonical.Status == PathSmoothingStatus.Success
                ? NormalizeMetrics(canonical.Diagnostics.Metrics, rawMetrics)
                : new PathQualityMetrics();
            entry = PathSmoothingComparisonEntry.CreateCandidate(
                method,
                canonical.Status,
                metrics,
                timing,
                digest,
                diagnostic,
                canonical.Path,
                canonical.Segments);
            return true;
        }
        catch (OperationCanceledException)
        {
            return false;
        }
        catch (Exception exception)
        {
            entry = PathSmoothingComparisonEntry.CreateCandidate(
                method,
                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, "比较请求为空。", out reason);

        PathSmoothingRequest smoothingRequest = request.SmoothingRequest;
        PathSmoothingConfiguration configuration = smoothingRequest.Configuration;
        if (configuration == null || smoothingRequest.Map == null || smoothingRequest.Vehicle == null)
            return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求缺少可用的地图、车辆或配置。", 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.Success, null, rawPath.Path, rawPath.Segments);
        return PathSmoothingComparisonEntry.CreateRawPathBaseline(
            PathSmoothingStatus.Success,
            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, "路径平滑比较已取消。");
    }

    private static PathSmoothingRequest CreateMethodRequest(PathSmoothingRequest source, SmoothingMethod method)
    {
        PathSmoothingConfiguration configuration = source.Configuration;
        configuration.Method = method;
        configuration.AllowFallbackToCoarsePath = false;
        return new PathSmoothingRequest(
            source.CoarsePath,
            source.Segments,
            source.Map,
            source.Vehicle,
            configuration);
    }

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