using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Threading;
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");
            EMPlannerVerificationHost.Verification.Equal(true, realNative.SolveChecksPassed, "native solve checks");
            Console.WriteLine("PASS osqp-solve");
        }
        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);

        if (first.Status == QpSolveStatus.Solved)
        {
            VerifyNativeSolveLifecycle();
            Console.WriteLine("SOLVE_CHECKS=PASS");
        }
    }

    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++)
        {
            if (string.Equals(Path.GetFileName(hostFiles[index]), "osqp.dll", StringComparison.OrdinalIgnoreCase))
                continue;
            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 void VerifyNativeSolveLifecycle()
    {
        var solver = new OsqpNativeSolver();

        QpSolveResult bounded = solver.Solve(
            CreateBoundedOptimumProblem(),
            CreateSettings(TimeSpan.FromMilliseconds(100)),
            new[] { 0d },
            CancellationToken.None);
        EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.Solved, bounded.Status, "bounded optimum status");
        AssertPopulatedResult(bounded, "bounded optimum");
        EMPlannerVerificationHost.Verification.Equal(1, bounded.Primal.Count, "bounded optimum primal count");
        AssertClose(1d, bounded.Primal[0], 1e-5d, "bounded optimum primal");
        AssertClose(-1.5d, bounded.Objective, 1e-5d, "bounded optimum objective");
        EMPlannerVerificationHost.Verification.True(bounded.PrimalResidual <= 1e-5d, "bounded optimum primal residual");
        EMPlannerVerificationHost.Verification.True(bounded.DualResidual <= 1e-5d, "bounded optimum dual residual");

        QpSolveResult equality = solver.Solve(
            CreateEqualityOptimumProblem(),
            CreateSettings(TimeSpan.FromMilliseconds(100)),
            new[] { 0.5d, 0.5d },
            CancellationToken.None);
        EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.Solved, equality.Status, "equality optimum status");
        AssertPopulatedResult(equality, "equality optimum");
        EMPlannerVerificationHost.Verification.Equal(2, equality.Primal.Count, "equality optimum primal count");
        AssertClose(0.5d, equality.Primal[0], 1e-5d, "equality optimum first primal");
        AssertClose(0.5d, equality.Primal[1], 1e-5d, "equality optimum second primal");
        AssertClose(0.5d, equality.Objective, 1e-5d, "equality optimum objective");
        EMPlannerVerificationHost.Verification.True(equality.PrimalResidual <= 1e-5d, "equality optimum primal residual");
        EMPlannerVerificationHost.Verification.True(equality.DualResidual <= 1e-5d, "equality optimum dual residual");

        QpSolveResult infeasible = solver.Solve(
            CreateInfeasibleProblem(),
            CreateSettings(TimeSpan.FromMilliseconds(100)),
            null,
            CancellationToken.None);
        EMPlannerVerificationHost.Verification.Equal(QpSolveStatus.PrimalInfeasible, infeasible.Status, "infeasible status");
        AssertPopulatedResult(infeasible, "infeasible");

        QpSolveResult tinyTimeLimit = solver.Solve(
            CreateEqualityOptimumProblem(),
            CreateSettings(TimeSpan.FromTicks(1)),
            null,
            CancellationToken.None);
        EMPlannerVerificationHost.Verification.True(
            tinyTimeLimit.Status == QpSolveStatus.TimeLimit ||
            tinyTimeLimit.Status == QpSolveStatus.Solved ||
            tinyTimeLimit.Status == QpSolveStatus.SolvedInaccurate,
            "tiny time-limit status maps to a time limit or solved state");
        AssertPopulatedResult(tinyTimeLimit, "tiny time-limit");
    }

    private static QuadraticProgram CreateBoundedOptimumProblem()
    {
        var hessian = new SparseTripletBuilder(1, 1, true);
        hessian.Add(0, 0, 1d);
        var constraints = new SparseTripletBuilder(1, 1);
        constraints.Add(0, 0, 1d);
        return new QuadraticProgram(hessian.Build(), new[] { -2d }, constraints.Build(), new[] { 0d }, new[] { 1d });
    }

    private static QuadraticProgram CreateEqualityOptimumProblem()
    {
        var hessian = new SparseTripletBuilder(2, 2, true);
        hessian.Add(0, 0, 2d);
        hessian.Add(1, 1, 2d);
        var constraints = new SparseTripletBuilder(1, 2);
        constraints.Add(0, 0, 1d);
        constraints.Add(0, 1, 1d);
        return new QuadraticProgram(hessian.Build(), new[] { 0d, 0d }, constraints.Build(), new[] { 1d }, new[] { 1d });
    }

    private static QuadraticProgram CreateInfeasibleProblem()
    {
        var hessian = new SparseTripletBuilder(1, 1, true);
        hessian.Add(0, 0, 1d);
        var constraints = new SparseTripletBuilder(2, 1);
        constraints.Add(0, 0, 1d);
        constraints.Add(1, 0, 1d);
        return new QuadraticProgram(
            hessian.Build(),
            new[] { 0d },
            constraints.Build(),
            new[] { 1d, -QuadraticProgram.MaximumFiniteBound },
            new[] { QuadraticProgram.MaximumFiniteBound, 0d });
    }

    private static QpSolverSettings CreateSettings(TimeSpan timeLimit)
    {
        return new QpSolverSettings(4000, 1e-6d, 1e-6d, timeLimit, true, true, false);
    }

    private static void AssertPopulatedResult(QpSolveResult result, string name)
    {
        EMPlannerVerificationHost.Verification.True(result.Iterations >= 0, name + " iterations");
        EMPlannerVerificationHost.Verification.True(result.SolveTime >= TimeSpan.Zero, name + " solve time");
        EMPlannerVerificationHost.Verification.True(!double.IsNaN(result.Objective) && !double.IsInfinity(result.Objective), name + " objective");
        EMPlannerVerificationHost.Verification.True(!double.IsNaN(result.PrimalResidual) && !double.IsInfinity(result.PrimalResidual), name + " primal residual");
        EMPlannerVerificationHost.Verification.True(!double.IsNaN(result.DualResidual) && !double.IsInfinity(result.DualResidual), name + " dual residual");
        EMPlannerVerificationHost.Verification.True(!string.IsNullOrWhiteSpace(result.NativeStatus), name + " native status");
    }

    private static void AssertClose(double expected, double actual, double tolerance, string name)
    {
        EMPlannerVerificationHost.Verification.True(Math.Abs(expected - actual) <= tolerance, name + " expected " + expected + " but was " + actual);
    }

    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, bool solveChecksPassed, string rawOutput)
        {
            Status = status;
            Version = version;
            ModuleHandle = moduleHandle;
            Diagnostic = diagnostic;
            ConcurrentHandleStable = concurrentHandleStable;
            SolveChecksPassed = solveChecksPassed;
            RawOutput = rawOutput;
        }

        public QpSolveStatus Status { get; }

        public string Version { get; }

        public IntPtr ModuleHandle { get; }

        public string Diagnostic { get; }

        public bool ConcurrentHandleStable { get; }

        public bool SolveChecksPassed { 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=");
            string solveChecksText = ReadOptionalValue(output, "SOLVE_CHECKS=");

            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,
                string.Equals(solveChecksText, "PASS", StringComparison.Ordinal), 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);
        }

        private static string ReadOptionalValue(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);
            }

            return string.Empty;
        }
    }
}