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

using System;
using System.Diagnostics;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;

/// <summary>
/// Local G2 路径平滑的公开、同步业务入口。
/// 它只消费调用方冻结的请求，依次执行输入校验、预处理、原始基线、局部 G2 和独立复核；从不发布未经验证的部分路径。
/// </summary>
public sealed class PathSmoothingService
{
    private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
    private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
    private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
    private readonly LocalG2PreSmoothingPipeline _localG2Pipeline = new LocalG2PreSmoothingPipeline();

    /// <summary>在请求携带的不可变粗路径、地图和车辆快照上执行一次 Local G2 平滑。</summary>
    /// <param name="request">平滑输入快照，包含粗路径/方向段、规划地图、车辆和配置；路径单位为 m/rad，曲率单位为 1/m。</param>
    /// <param name="cancellationToken">调用方取消令牌；取消时返回 <see cref="PathSmoothingStatus.Cancelled"/>，不会发布部分路径。</param>
    /// <returns>成功时携带完整独立复核路径的结果；输入、预处理、碰撞、曲率或运行失败时路径和分段为空，原因位于诊断中。</returns>
    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, reason);

            if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason))
                return Failure(PathSmoothingStatus.InvalidInput, stopwatch, reason);

            if (!RawPathBaselineBuilder.TryCreate(
                    request,
                    preparedPath,
                    _analyzer,
                    configuration.OutputSpacingMeters,
                    _validator,
                    configuration.MaximumCollisionCheckStepMeters,
                    out RawPathBaseline rawBaseline,
                    out reason))
            {
                return Failure(PathSmoothingStatus.InvalidInput, stopwatch, reason);
            }

            return _localG2Pipeline.Smooth(request, preparedPath, rawBaseline, cancellationToken);
        }
        catch (OperationCanceledException)
        {
            return Failure(PathSmoothingStatus.Cancelled, stopwatch, "Path smoothing was cancelled.");
        }
        catch (Exception exception)
        {
            return Failure(PathSmoothingStatus.Failed, stopwatch, exception.Message);
        }
    }

    private static bool TryValidateRequest(
        PathSmoothingRequest request,
        out PathSmoothingConfiguration configuration,
        out string reason)
    {
        configuration = null;
        reason = string.Empty;
        if (request == null)
        {
            reason = "Path smoothing request is required.";
            return false;
        }

        configuration = request.Configuration;
        VehicleParameters vehicle = request.Vehicle;
        if (configuration == null || request.Map == null || !request.Map.PlanningReady || vehicle == null)
        {
            reason = "A planning-ready map, vehicle, and configuration are required.";
            return false;
        }

        if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
            !NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
            !NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _))
        {
            reason = "Vehicle geometry or curvature constraints are invalid.";
            return false;
        }

        if (!NumericGuard.IsPositiveFinite(configuration.OutputSpacingMeters) ||
            !NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
            !NumericGuard.IsFinite(configuration.MinimumClearanceReserveMeters) ||
            configuration.MinimumClearanceReserveMeters < 0d ||
            !CurvatureLimitPolicy.TryGetAllowedMaximumVehicleCurvaturePerMeter(
                vehicle, configuration.CurvatureLimitRadiusToleranceMeters, out _))
        {
            reason = "Shared Local G2 configuration values are invalid.";
            return false;
        }

        return IsValidLocalG2Options(configuration.LocalG2Quintic, out reason);
    }

    private static bool IsValidLocalG2Options(LocalG2QuinticOptions options, out string reason)
    {
        reason = string.Empty;
        if (options != null &&
            NumericGuard.IsPositiveFinite(options.MinimumWindowLengthMeters) &&
            NumericGuard.IsFinite(options.PreferredWindowLengthMeters) &&
            options.PreferredWindowLengthMeters >= options.MinimumWindowLengthMeters &&
            NumericGuard.IsFinite(options.MaximumWindowLengthMeters) &&
            options.MaximumWindowLengthMeters >= options.PreferredWindowLengthMeters &&
            NumericGuard.IsPositiveFinite(options.MaximumDeviationMeters) &&
            NumericGuard.IsPositiveFinite(options.AbsoluteCurvatureJumpFloorPerMeter) &&
            NumericGuard.IsFinite(options.CurvatureJumpRatioOfMaximum) &&
            options.CurvatureJumpRatioOfMaximum > 0d && options.CurvatureJumpRatioOfMaximum <= 1d &&
            NumericGuard.IsFinite(options.MinimumPeakGradientImprovementRatio) &&
            options.MinimumPeakGradientImprovementRatio > 0d && options.MinimumPeakGradientImprovementRatio < 1d &&
            NumericGuard.IsFinite(options.MaximumVariationCostRegressionRatio) &&
            options.MaximumVariationCostRegressionRatio >= 0d && options.MaximumCandidatesPerRegion >= 1)
        {
            return true;
        }

        reason = "Local G2 configuration values are invalid.";
        return false;
    }

    private static PathSmoothingResult Failure(
        PathSmoothingStatus status,
        Stopwatch stopwatch,
        string reason)
    {
        return PathSmoothingResult.Failure(
            status,
            new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, reason));
    }
}