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

using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal sealed class LongitudinalQpAuditResult
{
    internal LongitudinalQpAuditResult(bool isFeasible, double maximumResidual, int worstRow,
        string category, string unit)
    {
        IsFeasible = isFeasible;
        MaximumResidual = maximumResidual;
        WorstRow = worstRow;
        Category = category;
        Unit = unit;
    }

    internal bool IsFeasible { get; }

    internal double MaximumResidual { get; }

    internal int WorstRow { get; }

    internal string Category { get; }

    internal string Unit { get; }
}

internal static class LongitudinalQpFeasibilityAudit
{
    internal static LongitudinalQpAuditResult Evaluate(QuadraticProgram problem,
        LongitudinalCandidate candidate, double tolerance, LongitudinalVariableLayout layout,
        int stabilizationStart)
    {
        if (problem == null)
            throw new ArgumentNullException(nameof(problem));
        if (candidate == null)
            throw new ArgumentNullException(nameof(candidate));
        if (layout == null)
            throw new ArgumentNullException(nameof(layout));
        if (problem.VariableCount != layout.VariableCount || candidate.S.Count != layout.KnotCount ||
            candidate.U.Count != layout.KnotCount || candidate.A.Count != layout.KnotCount ||
            candidate.J.Count != layout.KnotCount - 1)
        {
            throw new ArgumentException("The QP, candidate, and longitudinal layout must have matching dimensions.");
        }

        double[] primal = ToPrimal(candidate, layout);
        var activity = new double[problem.ConstraintCount];
        bool allFinite = IsFinite(tolerance);
        SparseCscMatrix matrix = problem.ConstraintMatrix;
        for (int column = 0; column < matrix.ColumnCount; column++)
        {
            double value = primal[column];
            allFinite &= IsFinite(value);
            for (int entry = matrix.ColumnPointers[column]; entry < matrix.ColumnPointers[column + 1]; entry++)
                activity[matrix.RowIndices[entry]] += matrix.Values[entry] * value;
        }

        double maximumResidual = 0d;
        int worstRow = problem.ConstraintCount == 0 ? -1 : 0;
        for (int row = 0; row < problem.ConstraintCount; row++)
        {
            double residual;
            if (!IsFinite(activity[row]))
            {
                allFinite = false;
                residual = double.PositiveInfinity;
            }
            else
            {
                residual = Math.Max(0d, Math.Max(
                    problem.LowerBounds[row] - activity[row],
                    activity[row] - problem.UpperBounds[row]));
            }
            if (row == 0 || residual > maximumResidual)
            {
                maximumResidual = residual;
                worstRow = row;
            }
        }

        DescribeRow(worstRow, layout.KnotCount, stabilizationStart, out string category, out string unit);
        return new LongitudinalQpAuditResult(allFinite && maximumResidual <= tolerance,
            maximumResidual, worstRow, category, unit);
    }

    private static double[] ToPrimal(LongitudinalCandidate candidate, LongitudinalVariableLayout layout)
    {
        var primal = new double[layout.VariableCount];
        for (int index = 0; index < layout.KnotCount; index++)
        {
            primal[layout.S(index)] = candidate.S[index];
            primal[layout.U(index)] = candidate.U[index];
            primal[layout.A(index)] = candidate.A[index];
        }
        for (int index = 0; index < layout.KnotCount - 1; index++)
            primal[layout.J(index)] = candidate.J[index];
        return primal;
    }

    private static void DescribeRow(int targetRow, int knotCount, int stabilizationStart,
        out string category, out string unit)
    {
        int row = 0;
        for (int index = 0; index < knotCount; index++)
        {
            if (targetRow == row++)
            {
                category = "PathS trust";
                unit = "m";
                return;
            }
            if (targetRow == row++)
            {
                category = "speed";
                unit = "m/s";
                return;
            }
            if (index > 0 && targetRow == row++)
            {
                category = "speed envelope";
                unit = "m/s";
                return;
            }
            if (targetRow == row++)
            {
                category = "acceleration";
                unit = "m/s^2";
                return;
            }
        }

        for (int index = 0; index < knotCount - 1; index++)
        {
            if (targetRow == row++)
            {
                category = "jerk";
                unit = "m/s^3";
                return;
            }
        }
        for (int index = 0; index < knotCount; index++)
        {
            if (targetRow == row++)
            {
                category = "low-speed deceleration release";
                unit = "m/s";
                return;
            }
        }
        for (int index = 0; index < knotCount - 1; index++)
        {
            if (targetRow == row++)
            {
                category = "monotonic progress";
                unit = "m";
                return;
            }
        }
        for (int index = 0; index < knotCount - 1; index++)
        {
            if (targetRow == row++)
            {
                category = "exact dynamics";
                unit = "m/s^2";
                return;
            }
            if (targetRow == row++)
            {
                category = "exact dynamics";
                unit = "m/s";
                return;
            }
            if (targetRow == row++)
            {
                category = "exact dynamics";
                unit = "m";
                return;
            }
        }

        string[] stateUnits = { "m", "m/s", "m/s^2" };
        for (int state = 0; state < stateUnits.Length; state++)
        {
            if (targetRow == row++)
            {
                category = "exact start";
                unit = stateUnits[state];
                return;
            }
        }

        int stopKnotCount = stabilizationStart >= 0 && stabilizationStart < knotCount
            ? knotCount - stabilizationStart
            : 0;
        for (int index = 0; index < stopKnotCount; index++)
        {
            for (int state = 0; state < stateUnits.Length; state++)
            {
                if (targetRow == row++)
                {
                    category = "exact stop";
                    unit = stateUnits[state];
                    return;
                }
            }
        }

        category = "row accounting";
        unit = string.Empty;
    }

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