ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs

using System;
using System.Collections.Generic;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Independently validates ST candidates directly in physical units before they may become fallbacks.</summary>
public sealed class LongitudinalSolutionValidator
{
    public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
        out LongitudinalCandidate validatedCandidate, out string failureReason)
    {
        validatedCandidate = null;
        failureReason = string.Empty;
        if (input == null || speedLimit == null || candidate == null)
        {
            failureReason = "ST input, speed envelope, and candidate are required.";
            return false;
        }
        try
        {
            if (!PathSpeedLimitBuilder.TryGetLimits(input, out _, out double maximumAcceleration,
                    out double maximumDeceleration, out double maximumJerk, out _, out _, out failureReason))
            {
                return false;
            }
            IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
                input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
            double tolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance, nameof(tolerance));
            if (!HasMatchingTimes(candidate.KnotTimes, expectedTimes, tolerance))
            {
                failureReason = "ST candidate knot times do not match the configured horizon.";
                return false;
            }
            if (candidate.S.Count != expectedTimes.Count || candidate.U.Count != expectedTimes.Count ||
                candidate.A.Count != expectedTimes.Count || candidate.J.Count != expectedTimes.Count - 1)
            {
                failureReason = "ST candidate value counts do not match its time knots.";
                return false;
            }
            if (!candidate.SatisfiesExactDiscreteDynamics(tolerance))
            {
                failureReason = "ST candidate violates exact constant-jerk dynamics.";
                return false;
            }
            if (!AreClose(candidate.S[0], 0d, tolerance) ||
                !AreClose(candidate.U[0], input.InitialProgressSpeedMetersPerSecond, tolerance) ||
                !AreClose(candidate.A[0], input.InitialAccelerationMetersPerSecondSquared, tolerance))
            {
                failureReason = "ST candidate does not satisfy the exact initial state.";
                return false;
            }

            for (int index = 0; index < candidate.S.Count; index++)
            {
                double progress = candidate.S[index];
                double speed = candidate.U[index];
                double acceleration = candidate.A[index];
                if (!IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration) || progress < -tolerance ||
                    progress > input.PathUpperBoundS + tolerance || speed < -tolerance ||
                    acceleration < -maximumDeceleration - tolerance || acceleration > maximumAcceleration + tolerance)
                {
                    failureReason = "ST candidate violates physical bounds at knot " + index + ".";
                    return false;
                }
                double speedLimitAtProgress = index == 0
                    ? input.DirectionMaximumSpeedMetersPerSecond
                    : speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, progress)));
                if (speed > speedLimitAtProgress + tolerance)
                {
                    failureReason = "ST candidate violates the actual-PathS speed envelope at knot " + index +
                        " (S=" + progress + ", U=" + speed + ", limit=" + speedLimitAtProgress + ").";
                    return false;
                }
                if (index > 0 && progress < candidate.S[index - 1] - tolerance)
                {
                    failureReason = "ST candidate PathS decreases at knot " + index + ".";
                    return false;
                }
            }
            for (int index = 0; index < candidate.J.Count; index++)
            {
                if (!IsFinite(candidate.J[index]) || Math.Abs(candidate.J[index]) > maximumJerk + tolerance)
                {
                    failureReason = "ST candidate violates the jerk bound at interval " + index + ".";
                    return false;
                }
            }
            int stabilizationStart = candidate.S.Count;
            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
            {
                stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
                    candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
                for (int index = stabilizationStart; index < candidate.S.Count; index++)
                {
                    if (!AreClose(candidate.S[index], input.StopBoundaryPathS, tolerance) ||
                        !AreClose(candidate.U[index], 0d, tolerance) ||
                        !AreClose(candidate.A[index], 0d, tolerance))
                    {
                        failureReason = "ST candidate does not satisfy the exact stabilized S/U/A stop tail at knot " +
                            index + ".";
                        return false;
                    }
                }
            }
            if (input.Mode != EmLongitudinalMode.RollingContinuation)
            {
                for (int index = 0; index < candidate.S.Count; index++)
                {
                    if (!JerkLimitedStoppingMath.TryCalculate(candidate.U[index], candidate.A[index],
                            maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _) ||
                        candidate.S[index] + stop.DistanceMeters > input.StopBoundaryPathS + tolerance)
                    {
                        failureReason = "ST candidate leaves the jerk-limited stoppable set at knot " + index + ".";
                        return false;
                    }
                }
            }

            var canonicalS = new double[candidate.S.Count];
            var canonicalU = new double[candidate.U.Count];
            var canonicalA = new double[candidate.A.Count];
            for (int index = 0; index < candidate.S.Count; index++)
            {
                canonicalS[index] = candidate.S[index];
                canonicalU[index] = candidate.U[index];
                canonicalA[index] = candidate.A[index];
            }
            canonicalS[0] = 0d;
            canonicalU[0] = input.InitialProgressSpeedMetersPerSecond;
            canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared;
            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
            {
                for (int index = stabilizationStart; index < candidate.S.Count; index++)
                {
                    canonicalS[index] = input.StopBoundaryPathS;
                    canonicalU[index] = 0d;
                    canonicalA[index] = 0d;
                }
            }
            var canonicalCandidate = new LongitudinalCandidate(candidate.KnotTimes, canonicalS, canonicalU, canonicalA,
                candidate.J);
            if (!canonicalCandidate.SatisfiesExactDiscreteDynamics(tolerance))
            {
                failureReason = "Canonical ST hard-boundary values exceed the dynamics tolerance.";
                return false;
            }
            validatedCandidate = canonicalCandidate;
            return true;
        }
        catch (ArgumentException exception)
        {
            failureReason = exception.Message;
            return false;
        }
    }

    private static bool HasMatchingTimes(IReadOnlyList<double> actual, IReadOnlyList<double> expected, double tolerance)
    {
        if (actual.Count != expected.Count)
            return false;
        for (int index = 0; index < expected.Count; index++)
        {
            if (!AreClose(actual[index], expected[index], tolerance))
                return false;
        }
        return true;
    }

    private static bool AreClose(double actual, double expected, double tolerance)
    {
        return Math.Abs(actual - expected) <= tolerance;
    }

    private static double RequireNonnegative(double value, string parameterName)
    {
        if (!IsFinite(value) || value < 0d)
            throw new ArgumentOutOfRangeException(parameterName);
        return value;
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}