ParkingRobot/ClumsyPilot/tests/EMPlannerVerificationHost/OsqpChecks.cs

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Threading.Tasks;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class OsqpChecks
{
    public static void Run()
    {
        var pluginDirectories = new List<string>();
        try
        {
            string missingNativeDirectory = CreatePluginDirectory();
            pluginDirectories.Add(missingNativeDirectory);
            ProbeResult missingNative = RunProbe(missingNativeDirectory);
            EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.SolverUnavailable, missingNative.Status, "missing native status");
            EMPlannerVerificationHost.Verification.True(!string.IsNullOrEmpty(missingNative.Diagnostic), "missing native diagnostic");

            string corruptNativeDirectory = CreatePluginDirectory();
            pluginDirectories.Add(corruptNativeDirectory);
            File.WriteAllText(Path.Combine(corruptNativeDirectory, "osqp.dll"), "not a native library");
            ProbeResult corruptNative = RunProbe(corruptNativeDirectory);
            EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.SolverUnavailable, corruptNative.Status, "corrupt native status");
            EMPlannerVerificationHost.Verification.True(!string.IsNullOrEmpty(corruptNative.Diagnostic), "corrupt native diagnostic");
            EMPlannerVerificationHost.Verification.True(corruptNative.RawOutput.IndexOf("BadImageFormatException", StringComparison.Ordinal) < 0,
                "corrupt native does not escape BadImageFormatException");

            string realNativeDirectory = CreatePluginDirectory();
            pluginDirectories.Add(realNativeDirectory);
            string realNativePath = Path.GetFullPath(Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ThirdParty", "OSQP", "win-x64", "osqp.dll"));
            EMPlannerVerificationHost.Verification.True(File.Exists(realNativePath), "pinned native package exists");
            File.Copy(realNativePath, Path.Combine(realNativeDirectory, "osqp.dll"), false);
            ProbeResult realNative = RunProbe(realNativeDirectory);
            EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.Solved, realNative.Status, "real native status");
            EMPlannerVerificationHost.Verification.Equal("1.0.0", realNative.Version, "real native version");
            EMPlannerVerificationHost.Verification.True(realNative.ModuleHandle != IntPtr.Zero, "real native module handle");
            EMPlannerVerificationHost.Verification.Equal(true, realNative.ConcurrentHandleStable, "concurrent native module handle");
        }
        finally
        {
            for (int index = 0; index < pluginDirectories.Count; index++)
            {
                if (Directory.Exists(pluginDirectories[index]))
                    Directory.Delete(pluginDirectories[index], true);
            }
        }
    }

    public static void RunProbe()
    {
        var results = new OsqpNativeLoadResult[16];
        Parallel.For(0, results.Length, index => results[index] = OsqpNativeLoader.Load());

        OsqpNativeLoadResult first = results[0];
        bool sameHandle = true;
        for (int index = 1; index < results.Length; index++)
        {
            sameHandle = sameHandle && results[index].Status == first.Status && results[index].ModuleHandle == first.ModuleHandle;
        }

        Console.WriteLine("STATUS=" + first.Status);
        Console.WriteLine("VERSION=" + first.Version);
        Console.WriteLine("HANDLE=" + first.ModuleHandle.ToInt64());
        Console.WriteLine("DIAGNOSTIC=" + first.Diagnostic.Replace('\r', ' ').Replace('\n', ' '));
        Console.WriteLine("CONCURRENT_HANDLE_STABLE=" + sameHandle);
    }

    private static string CreatePluginDirectory()
    {
        string sourceDirectory = AppContext.BaseDirectory;
        string destinationDirectory = Path.Combine(Path.GetTempPath(), "em-planner-osqp-loader-" + Guid.NewGuid().ToString("N"));
        Directory.CreateDirectory(destinationDirectory);

        string[] hostFiles = Directory.GetFiles(sourceDirectory);
        for (int index = 0; index < hostFiles.Length; index++)
        {
            string destinationPath = Path.Combine(destinationDirectory, Path.GetFileName(hostFiles[index]));
            File.Copy(hostFiles[index], destinationPath, false);
        }

        EMPlannerVerificationHost.Verification.True(File.Exists(Path.Combine(destinationDirectory, "ClumsyPilot.dll")), "copied plugin ClumsyPilot.dll");
        return destinationDirectory;
    }

    private static ProbeResult RunProbe(string pluginDirectory)
    {
        var startInfo = new ProcessStartInfo
        {
            FileName = Path.Combine(pluginDirectory, "EMPlannerVerificationHost.exe"),
            Arguments = "osqp-probe",
            WorkingDirectory = pluginDirectory,
            UseShellExecute = false,
            CreateNoWindow = true,
            RedirectStandardOutput = true,
            RedirectStandardError = true,
        };

        using (var process = new Process { StartInfo = startInfo })
        {
            process.Start();
            string standardOutput = process.StandardOutput.ReadToEnd();
            string standardError = process.StandardError.ReadToEnd();
            process.WaitForExit();
            if (process.ExitCode != 0)
                throw new InvalidOperationException("OSQP loader probe exited " + process.ExitCode + ": " + standardError + standardOutput);
            return ProbeResult.Parse(standardOutput + standardError);
        }
    }

    private sealed class ProbeResult
    {
        private ProbeResult(QpSolveStatus status, string version, IntPtr moduleHandle, string diagnostic, bool concurrentHandleStable, string rawOutput)
        {
            Status = status;
            Version = version;
            ModuleHandle = moduleHandle;
            Diagnostic = diagnostic;
            ConcurrentHandleStable = concurrentHandleStable;
            RawOutput = rawOutput;
        }

        public QpSolveStatus Status { get; }

        public string Version { get; }

        public IntPtr ModuleHandle { get; }

        public string Diagnostic { get; }

        public bool ConcurrentHandleStable { get; }

        public string RawOutput { get; }

        public static ProbeResult Parse(string output)
        {
            string statusText = ReadValue(output, "STATUS=");
            string version = ReadValue(output, "VERSION=");
            string handleText = ReadValue(output, "HANDLE=");
            string diagnostic = ReadValue(output, "DIAGNOSTIC=");
            string stableText = ReadValue(output, "CONCURRENT_HANDLE_STABLE=");

            QpSolveStatus status;
            if (!Enum.TryParse(statusText, out status))
                throw new InvalidOperationException("Loader probe reported an invalid status: " + statusText);
            long handleValue;
            if (!long.TryParse(handleText, out handleValue))
                throw new InvalidOperationException("Loader probe reported an invalid handle: " + handleText);
            bool concurrentHandleStable;
            if (!bool.TryParse(stableText, out concurrentHandleStable))
                throw new InvalidOperationException("Loader probe reported an invalid concurrency flag: " + stableText);

            return new ProbeResult(status, version, new IntPtr(handleValue), diagnostic, concurrentHandleStable, output);
        }

        private static string ReadValue(string output, string prefix)
        {
            string[] lines = output.Replace("\r", string.Empty).Split('\n');
            for (int index = 0; index < lines.Length; index++)
            {
                if (lines[index].StartsWith(prefix, StringComparison.Ordinal))
                    return lines[index].Substring(prefix.Length);
            }

            throw new InvalidOperationException("Loader probe did not report " + prefix + ". Output: " + output);
        }
    }
}