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

using System;
using System.Collections.Generic;
using System.IO;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
using Newtonsoft.Json;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;

public sealed class LocalG2DiagnosticEvidenceLoader
{
    private const string SourceMeasurementBatchSha256 = "ac8166828813d85bf6f8b58f985839e5b2a049e75cb94186ed04f59d540e4eed";
    private const string FixtureSha256 = "3d05daee5a211b3e7aa0b77193423b5fa07d3135e241a4413be3518fc7efe563";
    private const string CandidateStableKey = "single-turn/s0/r0/w5/seed2";
    private const string CandidateSha256 = "7cefb76c77e48a49bf3212e7a2472e23036c3899daa94b5c6b68fa5db29a3392";
    private const double WindowStartArcLengthMeters = 1.9199999999999982d;
    private const double WindowEndArcLengthMeters = 2.1199999999999983d;
    private const double StartGeometricCurvaturePerMeter = 0.41666666666666663d;
    private const double EndGeometricCurvaturePerMeter = 0d;
    private const double StartVehicleCurvaturePerMeter = 0.41666666666666663d;
    private const double EndVehicleCurvaturePerMeter = 0d;

    private static readonly LocalG2DiagnosticEvidencePointContract[] ExpectedCandidatePoints =
    {
        new(2.7216397035494579d, 1.7279184433567971d, 1.9199999999999982d, 0.799999999999999d, 0.799999999999999d),
        new(2.7355188205174521d, 1.7423187041196302d, 1.9399999999999982d, 0.80751185024996908d, 0.80751185024996908d),
        new(2.7492871654421882d, 1.7568248978050907d, 1.9599999999999982d, 0.8157618125921976d, 0.8157618125921976d),
        new(2.7629234692741469d, 1.7714552535786874d, 1.9799999999999982d, 0.82542852165703573d, 0.82542852165703573d),
        new(2.7764244476704873d, 1.786210614200662d, 1.9999999999999982d, 0.83333333333333237d, 0.83333333333333237d),
        new(2.7925350453990334d, 1.803999635249544d, 2.0239999999999982d, 0.835253334801853d, 0.835253334801853d),
        new(2.80865418003672d, 1.8217809211927092d, 2.0479999999999983d, 0.83333333355178374d, 0.83333333355178374d),
        new(2.8248074262445737d, 1.8395312269498194d, 2.0719999999999983d, 0.8318933325232104d, 0.8318933325232104d),
        new(2.8409691920174533d, 1.8572737784943611d, 2.0959999999999983d, 0.83237333274470626d, 0.83237333274470626d),
        new(2.8571139169604551d, 1.8750318365841865d, 2.1199999999999983d, 0.83333333333333237d, 0.83333333333333237d),
    };

    public LocalG2DiagnosticEvidence LoadAndVerify(string evidencePath)
    {
        try
        {
            if (string.IsNullOrWhiteSpace(evidencePath))
                throw new InvalidDataException("Diagnostic evidence path is required.");

            LocalG2DiagnosticEvidence evidence = JsonConvert.DeserializeObject<LocalG2DiagnosticEvidence>(File.ReadAllText(evidencePath));
            Validate(evidence);
            return evidence;
        }
        catch (InvalidDataException)
        {
            throw;
        }
        catch (Exception exception)
        {
            throw new InvalidDataException("Local G2 diagnostic evidence is invalid.", exception);
        }
    }

    private static void Validate(LocalG2DiagnosticEvidence evidence)
    {
        if (evidence == null ||
            !string.Equals(evidence.SourceMeasurementBatchSha256, SourceMeasurementBatchSha256, StringComparison.Ordinal) ||
            !string.Equals(evidence.FixtureSha256, FixtureSha256, StringComparison.Ordinal) ||
            !string.Equals(evidence.ScenarioId, "single-turn", StringComparison.Ordinal) ||
            !string.Equals(evidence.CandidateStableKey, CandidateStableKey, StringComparison.Ordinal) ||
            !string.Equals(evidence.CandidateSha256, CandidateSha256, StringComparison.Ordinal) ||
            evidence.CandidateIndex != 5 || evidence.SegmentIndex != 0 ||
            !string.Equals(evidence.EvaluatorResult, "InsufficientClearance", StringComparison.Ordinal) ||
            !string.Equals(evidence.StopGate, "Clearance", StringComparison.Ordinal) ||
            !string.Equals(evidence.PublishedStatus, "Unchanged", StringComparison.Ordinal) ||
            !IsExpected(evidence.WindowStartArcLengthMeters, WindowStartArcLengthMeters) ||
            !IsExpected(evidence.WindowEndArcLengthMeters, WindowEndArcLengthMeters) ||
            !IsExpected(evidence.StartGeometricCurvaturePerMeter, StartGeometricCurvaturePerMeter) ||
            !IsExpected(evidence.EndGeometricCurvaturePerMeter, EndGeometricCurvaturePerMeter) ||
            !IsExpected(evidence.StartVehicleCurvaturePerMeter, StartVehicleCurvaturePerMeter) ||
            !IsExpected(evidence.EndVehicleCurvaturePerMeter, EndVehicleCurvaturePerMeter) ||
            evidence.CandidatePoints == null || evidence.CandidatePoints.Count != ExpectedCandidatePoints.Length)
        {
            throw new InvalidDataException("Local G2 diagnostic evidence contract is invalid.");
        }

        double previousArc = 0d;
        for (int index = 0; index < evidence.CandidatePoints.Count; index++)
        {
            LocalG2DiagnosticEvidencePoint point = evidence.CandidatePoints[index];
            LocalG2DiagnosticEvidencePointContract expected = ExpectedCandidatePoints[index];
            if (point == null || !IsExpected(point.X, expected.X) || !IsExpected(point.Y, expected.Y) ||
                !IsExpected(point.ReferenceArcLengthMeters, expected.ReferenceArcLengthMeters) ||
                !IsExpected(point.HeadingRadians, expected.HeadingRadians) ||
                !IsExpected(point.UnwrappedHeadingRadians, expected.UnwrappedHeadingRadians) ||
                point.Source != (int)SmoothedPathPointSource.LocalG2Transition ||
                (index > 0 && point.ReferenceArcLengthMeters.Value <= previousArc))
            {
                throw new InvalidDataException("Local G2 diagnostic evidence point contract is invalid.");
            }

            previousArc = point.ReferenceArcLengthMeters.Value;
        }

        if (Math.Abs(evidence.CandidatePoints[0].ReferenceArcLengthMeters.Value - evidence.WindowStartArcLengthMeters.Value) > 1e-8d ||
            Math.Abs(evidence.CandidatePoints[evidence.CandidatePoints.Count - 1].ReferenceArcLengthMeters.Value - evidence.WindowEndArcLengthMeters.Value) > 1e-8d)
        {
            throw new InvalidDataException("Local G2 diagnostic evidence window contract is invalid.");
        }
    }

    private static bool IsFinite(double? value)
    {
        return value.HasValue && IsFinite(value.Value);
    }

    private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);

    private static bool IsExpected(double? value, double expected)
    {
        return IsFinite(value) && value.Value == expected;
    }

    private readonly struct LocalG2DiagnosticEvidencePointContract
    {
        public LocalG2DiagnosticEvidencePointContract(double x, double y, double referenceArcLengthMeters, double headingRadians, double unwrappedHeadingRadians)
        {
            X = x;
            Y = y;
            ReferenceArcLengthMeters = referenceArcLengthMeters;
            HeadingRadians = headingRadians;
            UnwrappedHeadingRadians = unwrappedHeadingRadians;
        }

        public double X { get; }
        public double Y { get; }
        public double ReferenceArcLengthMeters { get; }
        public double HeadingRadians { get; }
        public double UnwrappedHeadingRadians { get; }
    }
}

public sealed class LocalG2DiagnosticEvidence
{
    public string SourceMeasurementBatchSha256 { get; set; }
    public string FixtureSha256 { get; set; }
    public string ScenarioId { get; set; }
    public string CandidateStableKey { get; set; }
    public string CandidateSha256 { get; set; }
    public int? CandidateIndex { get; set; }
    public int? SegmentIndex { get; set; }
    public double? WindowStartArcLengthMeters { get; set; }
    public double? WindowEndArcLengthMeters { get; set; }
    public double? StartGeometricCurvaturePerMeter { get; set; }
    public double? EndGeometricCurvaturePerMeter { get; set; }
    public double? StartVehicleCurvaturePerMeter { get; set; }
    public double? EndVehicleCurvaturePerMeter { get; set; }
    public List<LocalG2DiagnosticEvidencePoint> CandidatePoints { get; set; }
    public string EvaluatorResult { get; set; }
    public string StopGate { get; set; }
    public string PublishedStatus { get; set; }
}

public sealed class LocalG2DiagnosticEvidencePoint
{
    public double? X { get; set; }
    public double? Y { get; set; }
    public double? ReferenceArcLengthMeters { get; set; }
    public double? HeadingRadians { get; set; }
    public double? UnwrappedHeadingRadians { get; set; }
    public int? Source { get; set; }
}