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

using System;
using System.Collections.Generic;
using System.Globalization;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Proves the constant-jerk profile used by publication is continuously forward-progressing.</summary>
internal static class LongitudinalContinuousProfileValidator
{
    public static bool TryValidate(LongitudinalCandidate candidate, double tolerance, out string failureReason)
    {
        failureReason = string.Empty;
        if (candidate == null)
        {
            failureReason = "A longitudinal candidate is required.";
            return false;
        }
        if (!IsFinite(tolerance) || tolerance < 0d)
        {
            failureReason = "A finite nonnegative continuous-profile tolerance is required.";
            return false;
        }

        double highWater = candidate.S[0];
        for (int interval = 0; interval < candidate.J.Count; interval++)
        {
            double duration = candidate.KnotTimes[interval + 1] - candidate.KnotTimes[interval];
            double initialS = candidate.S[interval];
            double initialU = candidate.U[interval];
            double initialA = candidate.A[interval];
            double jerk = candidate.J[interval];
            var evaluationTimes = new List<double>(5) { 0d, duration };
            if (jerk != 0d)
                AddIfInside(evaluationTimes, -initialA / jerk, duration);
            AddSpeedRoots(evaluationTimes, initialU, initialA, jerk, duration);
            evaluationTimes.Sort();

            double previousTime = double.NegativeInfinity;
            for (int point = 0; point < evaluationTimes.Count; point++)
            {
                double localTime = evaluationTimes[point];
                if (localTime == previousTime)
                    continue;
                previousTime = localTime;
                Evaluate(initialS, initialU, initialA, jerk, localTime,
                    out double progress, out double speed, out double acceleration);
                double regression = highWater - progress;
                if (!IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration) ||
                    speed < -tolerance || regression > tolerance)
                {
                    string kind = !IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration)
                        ? "non-finite"
                        : speed < -tolerance ? "negative-speed" : "PathS-regression";
                    failureReason = FormatFailure(kind, interval, localTime, progress, speed,
                        acceleration, jerk, regression, string.Empty);
                    return false;
                }
                if (progress > highWater)
                    highWater = progress;
            }

            Evaluate(initialS, initialU, initialA, jerk, duration,
                out double integratedS, out double integratedU, out double integratedA);
            double sMismatch = Math.Abs(integratedS - candidate.S[interval + 1]);
            double uMismatch = Math.Abs(integratedU - candidate.U[interval + 1]);
            double aMismatch = Math.Abs(integratedA - candidate.A[interval + 1]);
            if (!IsFinite(sMismatch) || !IsFinite(uMismatch) || !IsFinite(aMismatch) ||
                sMismatch > tolerance || uMismatch > tolerance || aMismatch > tolerance)
            {
                string detail = ";endpointMismatchS=" + Invariant(sMismatch) +
                    ";endpointMismatchU=" + Invariant(uMismatch) +
                    ";endpointMismatchA=" + Invariant(aMismatch);
                failureReason = FormatFailure("endpoint-mismatch", interval, duration, integratedS,
                    integratedU, integratedA, jerk, highWater - integratedS, detail);
                return false;
            }
        }
        return true;
    }

    private static void AddSpeedRoots(ICollection<double> times, double initialU, double initialA,
        double jerk, double duration)
    {
        if (jerk == 0d)
        {
            if (initialA != 0d)
                AddIfInside(times, -initialU / initialA, duration);
            return;
        }

        double discriminant = initialA * initialA - 2d * jerk * initialU;
        if (!IsFinite(discriminant) || discriminant < 0d)
            return;
        double rootTerm = Math.Sqrt(discriminant);
        AddIfInside(times, (-initialA - rootTerm) / jerk, duration);
        AddIfInside(times, (-initialA + rootTerm) / jerk, duration);
    }

    private static void AddIfInside(ICollection<double> times, double localTime, double duration)
    {
        if (IsFinite(localTime) && localTime >= 0d && localTime <= duration)
            times.Add(localTime);
    }

    private static void Evaluate(double initialS, double initialU, double initialA, double jerk,
        double localTime, out double progress, out double speed, out double acceleration)
    {
        acceleration = initialA + jerk * localTime;
        speed = initialU + initialA * localTime + 0.5d * jerk * localTime * localTime;
        progress = initialS + initialU * localTime + 0.5d * initialA * localTime * localTime +
            jerk * localTime * localTime * localTime / 6d;
    }

    private static string FormatFailure(string kind, int interval, double localTime, double progress,
        double speed, double acceleration, double jerk, double regression, string detail)
    {
        return "Continuous ST profile rejected: kind=" + kind +
            ";interval=" + interval.ToString(CultureInfo.InvariantCulture) +
            ";localTime=" + Invariant(localTime) +
            ";S=" + Invariant(progress) +
            ";U=" + Invariant(speed) +
            ";A=" + Invariant(acceleration) +
            ";J=" + Invariant(jerk) +
            ";regression=" + Invariant(regression) + detail + ".";
    }

    private static string Invariant(double value)
    {
        return value.ToString("R", CultureInfo.InvariantCulture);
    }

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