ParkingRobot/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/LocalG2DiagnosticVisualizationDemo.cs

using System;
using System.Collections.Generic;
using System.IO;
using System.Security.Cryptography;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;

public sealed class LocalG2DiagnosticVisualizationDemo
{
    private readonly LocalG2DiagnosticEvidenceLoader _evidenceLoader = new LocalG2DiagnosticEvidenceLoader();
    private readonly PathSmoothingComparisonService _comparisonService = new PathSmoothingComparisonService();
    private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
    private readonly LocalG2PathSplicer _splicer = new LocalG2PathSplicer();
    private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
    private readonly SmoothingFigureModelBuilder _figureBuilder = new SmoothingFigureModelBuilder();
    private readonly SmoothingReportExporter _exporter = new SmoothingReportExporter();

    public SmoothingReportExportResult Export(
        string fixturePath,
        string evidencePath,
        string outputDirectory,
        CancellationToken cancellationToken = default)
    {
        LocalG2DiagnosticEvidence evidence = _evidenceLoader.LoadAndVerify(evidencePath);
        RequireFixtureHash(fixturePath, evidence.FixtureSha256);
        PathSmoothingComparisonRequest request = FindFixtureRequest(fixturePath, evidence.ScenarioId);
        PathSmoothingComparisonResult comparison = _comparisonService.Compare(request, cancellationToken);
        PathSmoothingComparisonEntry localG2 = FindEntry(comparison, SmoothingMethod.LocalG2Quintic);
        Require(localG2 != null && localG2.Status == PathSmoothingStatus.Unchanged,
            "Strict Local G2 result must be Unchanged for the diagnostic evidence.");
        RequireSameGeometry(comparison.RawPathBaseline.Path, localG2.Path);
        Require(_preprocessor.TryPrepare(request.SmoothingRequest, out PreparedPath prepared, out string reason), reason);
        LocalG2CandidateGeometry candidate = CreateCandidate(evidence);
        Require(_splicer.TryReplace(prepared, candidate, out PreparedPath spliced, out reason), reason);
        Require(_analyzer.TryAnalyze(spliced.Segments, request.SmoothingRequest.Configuration.OutputSpacingMeters,
            out PathGeometryAnalysis analysis, out reason), reason);
        CoarsePathPoint first = request.SmoothingRequest.CoarsePath[0];
        CoarsePathPoint last = request.SmoothingRequest.CoarsePath[request.SmoothingRequest.CoarsePath.Count - 1];
        SmoothingFigureModel normal = _figureBuilder.Build(
            comparison,
            request.SmoothingRequest.Map,
            new Pose2D(first.X, first.Y, first.Heading),
            new Pose2D(last.X, last.Y, last.Heading),
            evidence.ScenarioId,
            evidence.ScenarioId);
        SmoothingFigureModel augmented = normal.WithAdditionalSeries(CreateDiagnosticSeries(analysis.Path));
        return _exporter.Export(new SmoothingReportExportRequest
        {
            Model = augmented,
            OutputDirectory = outputDirectory,
            FileStem = "comparison",
        });
    }

    private static void RequireFixtureHash(string fixturePath, string expectedFixtureSha256)
    {
        byte[] fixtureBytes = File.ReadAllBytes(fixturePath);
        byte[] hash;
        using (SHA256 sha256 = SHA256.Create())
            hash = sha256.ComputeHash(fixtureBytes);
        string actualFixtureSha256 = BitConverter.ToString(hash).Replace("-", string.Empty).ToLowerInvariant();
        Require(string.Equals(actualFixtureSha256, expectedFixtureSha256, StringComparison.Ordinal),
            "Diagnostic evidence fixture SHA-256 does not match the supplied fixture.");
    }

    private static PathSmoothingComparisonRequest FindFixtureRequest(string fixturePath, string scenarioId)
    {
        IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
        IReadOnlyList<PathSmoothingComparisonRequest> requests = SmoothingScenarioFactory.CreateFixtureRequests(fixturePath);
        Require(fixtures.Count == requests.Count, "Fixture requests do not match fixture records.");

        int fixtureIndex = -1;
        for (int index = 0; index < fixtures.Count; index++)
        {
            if (!string.Equals(fixtures[index].Id, scenarioId, StringComparison.Ordinal)) continue;
            Require(fixtureIndex < 0, "Diagnostic fixture ID must be unique: " + scenarioId);
            fixtureIndex = index;
        }
        Require(fixtureIndex >= 0, "Diagnostic fixture ID was not found: " + scenarioId);
        return requests[fixtureIndex];
    }

    private static PathSmoothingComparisonEntry FindEntry(PathSmoothingComparisonResult comparison, SmoothingMethod method)
    {
        if (comparison == null) return null;
        for (int index = 0; index < comparison.Entries.Count; index++)
        {
            if (comparison.Entries[index].Method == method) return comparison.Entries[index];
        }
        return null;
    }

    private static LocalG2CandidateGeometry CreateCandidate(LocalG2DiagnosticEvidence evidence)
    {
        Require(evidence != null && evidence.CandidatePoints != null, "Diagnostic evidence is required.");
        var points = new List<SmoothingPoint2D>(evidence.CandidatePoints.Count);
        for (int index = 0; index < evidence.CandidatePoints.Count; index++)
        {
            LocalG2DiagnosticEvidencePoint point = evidence.CandidatePoints[index];
            points.Add(new SmoothingPoint2D(
                point.X.Value, point.Y.Value, point.ReferenceArcLengthMeters.Value,
                point.HeadingRadians.Value, point.UnwrappedHeadingRadians.Value,
                0d, false, SmoothedPathPointSource.LocalG2Transition));
        }
        return new LocalG2CandidateGeometry(
            evidence.CandidateIndex.Value,
            evidence.SegmentIndex.Value,
            evidence.WindowStartArcLengthMeters.Value,
            evidence.WindowEndArcLengthMeters.Value,
            0d,
            0d,
            points,
            evidence.StartVehicleCurvaturePerMeter.Value,
            evidence.EndVehicleCurvaturePerMeter.Value,
            evidence.StartGeometricCurvaturePerMeter.Value,
            evidence.EndGeometricCurvaturePerMeter.Value,
            true);
    }

    private static SmoothingFigureSeries CreateDiagnosticSeries(IReadOnlyList<SmoothedPathPoint> path)
    {
        var points = new List<SmoothingFigurePoint>(path == null ? 0 : path.Count);
        if (path != null)
        {
            for (int index = 0; index < path.Count; index++)
            {
                SmoothedPathPoint point = path[index];
                points.Add(new SmoothingFigurePoint(point.X, point.Y, point.ArcLength, point.VehicleCurvature));
            }
        }
        return new SmoothingFigureSeries(
            null,
            "local-g2-diagnostic",
            "G2 诊断候选（净空拒绝，未发布）",
            PathSmoothingStatus.Infeasible,
            IeeeFigureStyle.LocalG2DiagnosticColor,
            string.Empty,
            false,
            points,
            Array.Empty<SmoothingFigurePoint>());
    }

    private static void RequireSameGeometry(IReadOnlyList<SmoothedPathPoint> expected, IReadOnlyList<SmoothedPathPoint> actual)
    {
        Require(expected != null && actual != null && expected.Count == actual.Count,
            "Strict Local G2 output must retain raw-path geometry.");
        for (int index = 0; index < expected.Count; index++)
        {
            SmoothedPathPoint left = expected[index];
            SmoothedPathPoint right = actual[index];
            Require(left.X == right.X && left.Y == right.Y && left.Heading == right.Heading &&
                left.UnwrappedHeading == right.UnwrappedHeading && left.ArcLength == right.ArcLength &&
                left.Direction == right.Direction && left.GeometricCurvature == right.GeometricCurvature &&
                left.VehicleCurvature == right.VehicleCurvature && left.VehicleCurvatureDerivative == right.VehicleCurvatureDerivative &&
                left.BodyClearance == right.BodyClearance && left.IsGearSwitchPoint == right.IsGearSwitchPoint && left.Source == right.Source,
                "Strict Local G2 output must retain raw-path geometry.");
        }
    }

    private static void Require(bool condition, string reason)
    {
        if (!condition) throw new InvalidOperationException(reason);
    }
}