using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Threading;
using EMPlannerVerificationHost;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class LateralIntegrationChecks
{
    public static void Run()
    {
        VerifiesValidatedCandidateSurvivesLaterTimeout();
        VerifiesInvalidVectorsAndInaccurateResidualsNeverBecomeFallbacks();
        VerifiesTrustRegionWarmStartAndOuterIterationLimit();
        VerifiesCancellationAndTimeoutWithoutCandidate();
        VerifiesLateralPlannerDelegatesToTheSequentialOptimizer();
    }

    public static void RunRealOsqp()
    {
        foreach (LateralScenario scenario in CreateRealOsqpScenarios())
        {
            LateralPlanningResult first = new LateralPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
                CancellationToken.None);
            LateralPlanningResult second = new LateralPlanner(new OsqpNativeSolver()).Plan(scenario.Input,
                CancellationToken.None);
            VerifyRealScenarioResult(scenario, first);
            VerifyRealScenarioResult(scenario, second);
            VerifyDeterministicResult(scenario.Name, first, second);
            if (scenario.RequiresSeedConnectedInterval)
            {
                for (int index = 0; index < first.Path.Points.Count; index++)
                    Verification.True(first.Path.Points[index].L <= -0.05d + 1e-10d,
                        scenario.Name + " remains in the seed-connected obstacle corridor");
            }
        }
    }

    public static void RunRealOsqpInCleanPluginBundle()
    {
        string pluginDirectory = Path.Combine(Path.GetTempPath(), "em-planner-lateral-real-" + Guid.NewGuid().ToString("N"));
        try
        {
            Directory.CreateDirectory(pluginDirectory);
            foreach (string sourcePath in Directory.GetFiles(AppContext.BaseDirectory))
                File.Copy(sourcePath, Path.Combine(pluginDirectory, Path.GetFileName(sourcePath)), false);
            string nativeSource = Path.GetFullPath(Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ThirdParty",
                "OSQP", "win-x64", "osqp.dll"));
            Verification.True(File.Exists(nativeSource), "pinned OSQP DLL is available for the real lateral bundle");
            File.Copy(nativeSource, Path.Combine(pluginDirectory, "osqp.dll"), false);

            var startInfo = new ProcessStartInfo
            {
                FileName = Path.Combine(pluginDirectory, "EMPlannerVerificationHost.exe"),
                Arguments = "lateral-real-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 || standardOutput.IndexOf("PASS lateral-real-osqp", StringComparison.Ordinal) < 0)
                {
                    throw new InvalidOperationException("Real lateral OSQP clean-plugin probe exited " + process.ExitCode + ": " +
                        standardError + standardOutput);
                }
            }
        }
        finally
        {
            if (Directory.Exists(pluginDirectory))
                Directory.Delete(pluginDirectory, true);
        }
    }

    private static void VerifiesValidatedCandidateSurvivesLaterTimeout()
    {
        LateralPlanningInput input = CreateInput();
        double[] valid = CreatePrimal(input, 0.02d);
        var solver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.Solved, valid, 10d),
            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
        });

        LateralPlanningResult result = new SequentialConvexOptimizer(solver).Optimize(input, CancellationToken.None);

        Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
            "timeout after an independently validated candidate returns fallback success");
        LateralPath fallbackPath = result.Path ?? throw new InvalidOperationException("Fallback path was not returned.");
        Verification.True(fallbackPath.IsIndependentlyValidated,
            "fallback path remains independently validated");
        Verification.NearlyEqual(0.02d, fallbackPath.Points[1].L,
            "first valid candidate remains the fallback path");
    }

    private static void VerifiesInvalidVectorsAndInaccurateResidualsNeverBecomeFallbacks()
    {
        LateralPlanningInput input = CreateInput();
        double[] valid = CreatePrimal(input, 0.02d);
        double[] invalid = CreatePrimal(input, 0.40d);

        var solver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.Solved, valid, 10d),
            Result(QpSolveStatus.Solved, invalid, 9d),
            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 9d),
        });
        LateralPlanningResult preserved = new SequentialConvexOptimizer(solver).Optimize(input, CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SuccessWithFallback, preserved.Status,
            "invalid solved vector does not discard an earlier fallback");
        Verification.NearlyEqual(0.02d, preserved.Path.Points[1].L,
            "invalid solved vector does not replace the fallback candidate");

        var inaccurateResidual = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.SolvedInaccurate, valid, 10d, 2e-5d, 0d),
            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
        });
        LateralPlanningResult rejectedResidual = new SequentialConvexOptimizer(inaccurateResidual).Optimize(input,
            CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SolverTimedOut, rejectedResidual.Status,
            "SolvedInaccurate above strict residual threshold is rejected");
        Verification.True(ReferenceEquals(null, rejectedResidual.Path),
            "rejected inaccurate result does not publish a path");

        var inaccurateGeometry = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.SolvedInaccurate, invalid, 10d, 0d, 0d),
            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
        });
        LateralPlanningResult rejectedGeometry = new SequentialConvexOptimizer(inaccurateGeometry).Optimize(input,
            CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SolverTimedOut, rejectedGeometry.Status,
            "SolvedInaccurate still requires full independent lateral validation");
    }

    private static void VerifiesTrustRegionWarmStartAndOuterIterationLimit()
    {
        LateralPlanningInput input = CreateInput();
        var trustSolver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.Solved, CreatePrimal(input, 0.02d), 10d),
            Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 10d),
        });
        new SequentialConvexOptimizer(trustSolver).Optimize(input, CancellationToken.None);

        var layout = new LateralVariableLayout(input.ReferenceStations.Count);
        FindSingleVariableBounds(trustSolver.Problems[0], layout.L(1), out double initialLower, out double initialUpper);
        FindSingleVariableBounds(trustSolver.Problems[1], layout.L(1), out double nextLower, out double nextUpper);
        Verification.NearlyEqual(-0.05d, initialLower, "initial trust-region lower bound");
        Verification.NearlyEqual(0.05d, initialUpper, "initial trust-region upper bound");
        Verification.NearlyEqual(-0.03d, nextLower, "trust region is centered on previous iterate");
        Verification.NearlyEqual(0.07d, nextUpper, "trust region never exceeds 0.05m around previous iterate");
        Verification.Equal(layout.VariableCount, trustSolver.WarmStarts[1].Count,
            "next QP receives the complete previous primal warm start");
        Verification.NearlyEqual(0.02d, trustSolver.WarmStarts[1][layout.L(1)],
            "warm start retains the prior lateral iterate");

        var limitResults = new List<QpSolveResult>();
        for (int index = 1; index <= 5; index++)
            limitResults.Add(Result(QpSolveStatus.Solved, CreatePrimal(input, 0.02d * index), 100d - index));
        var limitSolver = new FakeQpSolver(limitResults);
        LateralPlanningResult limited = new SequentialConvexOptimizer(limitSolver).Optimize(input, CancellationToken.None);
        Verification.Equal(5, limitSolver.SolveCallCount, "outer loop stops after at most five QP calls");
        Verification.Equal(EmPlanningStatus.Success, limited.Status, "last feasible candidate succeeds at outer iteration limit");
    }

    private static void VerifiesCancellationAndTimeoutWithoutCandidate()
    {
        LateralPlanningInput input = CreateInput();
        var cancellationSolver = new FakeQpSolver(Result(QpSolveStatus.Solved, CreatePrimal(input, 0d), 1d));
        using var cancellation = new CancellationTokenSource();
        cancellation.Cancel();
        LateralPlanningResult cancelled = new SequentialConvexOptimizer(cancellationSolver).Optimize(input,
            cancellation.Token);
        Verification.Equal(EmPlanningStatus.Cancelled, cancelled.Status, "cancellation before a solver call is cancelled");
        Verification.Equal(0, cancellationSolver.SolveCallCount, "cancelled solve does not invoke the solver");

        var timeoutSolver = new FakeQpSolver(Result(QpSolveStatus.TimeLimit, Array.Empty<double>(), 1d));
        LateralPlanningResult timeout = new SequentialConvexOptimizer(timeoutSolver).Optimize(input, CancellationToken.None);
        Verification.Equal(EmPlanningStatus.SolverTimedOut, timeout.Status,
            "timeout without a feasible candidate is solver timed out");
        Verification.True(ReferenceEquals(null, timeout.Path), "timeout without candidate does not publish a path");
    }

    private static void VerifiesLateralPlannerDelegatesToTheSequentialOptimizer()
    {
        LateralPlanningInput input = CreateInput();
        double[] zero = CreatePrimal(input, 0d);
        var solver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.Solved, zero, 1d),
            Result(QpSolveStatus.Solved, zero, 1d),
        });
        LateralPlanningResult result = new LateralPlanner(solver).Plan(input, CancellationToken.None);
        Verification.Equal(EmPlanningStatus.Success, result.Status, "lateral planner returns SQP success");
    }

    private static IReadOnlyList<LateralScenario> CreateRealOsqpScenarios()
    {
        return new[]
        {
            CreateScenario("straight-empty-forward", TravelDirection.Forward, 0d, EmTerminalType.Goal, -0.3d, 0.3d, 0d, false),
            CreateScenario("straight-empty-reverse", TravelDirection.Reverse, 0d, EmTerminalType.Goal, -0.3d, 0.3d, 0d, false),
            CreateScenario("gentle-curve", TravelDirection.Forward, 0.05d, EmTerminalType.Goal, -0.3d, 0.3d, 0d, false),
            CreateScenario("static-obstacle-narrowing", TravelDirection.Forward, 0d, EmTerminalType.RollingSafetyStop,
                -0.3d, -0.05d, -0.10d, true),
            CreateScenario("gear-switch-terminal", TravelDirection.Forward, 0d, EmTerminalType.GearSwitch, -0.3d, 0.3d, 0d, false),
            CreateScenario("rolling-terminal", TravelDirection.Forward, 0d, EmTerminalType.RollingSafetyStop, -0.3d, 0.3d, 0d, false),
        };
    }

    private static LateralScenario CreateScenario(string name, TravelDirection direction, double geometricCurvature,
        EmTerminalType terminal, double corridorMinimum, double corridorMaximum, double seedL,
        bool requiresSeedConnectedInterval)
    {
        double[] stations = { 0d, 0.5d, 1d, 1.5d, 2d };
        var points = new List<SmoothedPathPoint>(stations.Length);
        var intervals = new List<LateralInterval>(stations.Length);
        var seed = new List<FrenetProjection>(requiresSeedConnectedInterval ? stations.Length : 0);
        for (int index = 0; index < stations.Length; index++)
        {
            double referenceS = stations[index];
            double travelYaw = geometricCurvature * referenceS;
            double x = Math.Abs(geometricCurvature) <= 1e-12d ? referenceS : Math.Sin(travelYaw) / geometricCurvature;
            double y = Math.Abs(geometricCurvature) <= 1e-12d ? 0d : (1d - Math.Cos(travelYaw)) / geometricCurvature;
            double vehicleYaw = direction == TravelDirection.Forward ? travelYaw : travelYaw - Math.PI;
            points.Add(new SmoothedPathPoint(x, y, vehicleYaw, vehicleYaw, referenceS, direction, geometricCurvature,
                direction == TravelDirection.Forward ? geometricCurvature : -geometricCurvature, 0d, 1d, false,
                SmoothedPathPointSource.Anchor));
            intervals.Add(new LateralInterval(referenceS, corridorMinimum, corridorMaximum, seedL));
        }
        var segment = new DirectionSegmentView(0, direction, points,
            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
            new ReferenceBoundary(0, 2d, terminal == EmTerminalType.GearSwitch ? EmBoundaryType.GearSwitchApproach : EmBoundaryType.Goal,
                2d), 0d);
        if (requiresSeedConnectedInterval)
        {
            for (int index = 0; index < stations.Length; index++)
                seed.Add(new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, stations[index]), seedL, 0d, 0d));
        }
        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
        configuration.Scheduling.SolverTimeoutSeconds = 1d;
        configuration.Validation.SpatialToleranceMeters = 1e-5d;
        configuration.Validation.KinematicTolerance = 1e-5d;
        var vehicle = new VehicleParameters
        {
            LengthMeters = 0.1d,
            WidthMeters = 0.1d,
            SafetyMarginMeters = 0d,
            MaximumCurvaturePerMeter = 1d,
        };
        return new LateralScenario(name, new LateralPlanningInput(segment, new StaticCorridor(intervals),
            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), seedL, 0d, 0d), terminal, vehicle,
            configuration, seed), requiresSeedConnectedInterval);
    }

    private static void VerifyRealScenarioResult(LateralScenario scenario, LateralPlanningResult result)
    {
        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
            scenario.Name + " is solved or has a documented fallback: " + result.FailureReason);
        LateralPath path = result.Path ?? throw new InvalidOperationException(scenario.Name + " returned no lateral path.");
        Verification.True(path.IsIndependentlyValidated, scenario.Name + " path is independently validated");
        Verification.Equal(scenario.Input.ReferenceStations.Count, path.Points.Count, scenario.Name + " point count");
        double maximumCurvature = scenario.Input.Vehicle.MaximumCurvaturePerMeter.GetValueOrDefault();
        Verification.True(maximumCurvature > 0d, scenario.Name + " has a maximum vehicle curvature");
        for (int index = 0; index < path.Points.Count; index++)
        {
            LateralPathPoint point = path.Points[index];
            LateralInterval interval = scenario.Input.Corridor.Stations[index];
            Verification.True(point.L >= interval.MinimumL - 1e-10d && point.L <= interval.MaximumL + 1e-10d,
                scenario.Name + " remains in corridor at station " + index);
            Verification.True(Math.Abs(point.VehicleCurvature) <= maximumCurvature + 1e-10d,
                scenario.Name + " respects vehicle curvature at station " + index);
        }
        Verification.NearlyEqual(scenario.Input.ReferenceStations[scenario.Input.ReferenceStations.Count - 1],
            path.Points[path.Points.Count - 1].ReferenceS, scenario.Name + " ends at exact ReferenceS anchor");
    }

    private static void VerifyDeterministicResult(string name, LateralPlanningResult first, LateralPlanningResult second)
    {
        Verification.Equal(first.Status, second.Status, name + " deterministic status");
        LateralPath firstPath = first.Path ?? throw new InvalidOperationException(name + " first path was missing.");
        LateralPath secondPath = second.Path ?? throw new InvalidOperationException(name + " second path was missing.");
        Verification.Equal(firstPath.Points.Count, secondPath.Points.Count, name + " deterministic point count");
        for (int index = 0; index < firstPath.Points.Count; index++)
            ComparePoint(firstPath.Points[index], secondPath.Points[index], name + " deterministic point " + index);
    }

    private static void ComparePoint(LateralPathPoint left, LateralPathPoint right, string name)
    {
        double[] leftValues =
        {
            left.ReferenceS, left.PathS, left.L, left.DL, left.DDL, left.DDDL, left.X, left.Y, left.VehicleYaw,
            left.GeometricCurvature, left.VehicleCurvature, left.VehicleCurvatureDerivative,
        };
        double[] rightValues =
        {
            right.ReferenceS, right.PathS, right.L, right.DL, right.DDL, right.DDDL, right.X, right.Y, right.VehicleYaw,
            right.GeometricCurvature, right.VehicleCurvature, right.VehicleCurvatureDerivative,
        };
        for (int index = 0; index < leftValues.Length; index++)
            Verification.True(Math.Abs(leftValues[index] - rightValues[index]) <= 1e-10d, name + " value " + index);
    }

    private static LateralPlanningInput CreateInput()
    {
        var points = new List<SmoothedPathPoint>
        {
            Point(0d, 0d),
            Point(1d, 1d),
            Point(2d, 2d),
        };
        var segment = new DirectionSegmentView(0, TravelDirection.Forward, points,
            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
            new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d);
        var corridor = new StaticCorridor(new[]
        {
            new LateralInterval(0d, -0.3d, 0.3d, 0d),
            new LateralInterval(1d, -0.3d, 0.3d, 0d),
            new LateralInterval(2d, -0.3d, 0.3d, 0d),
        });
        var vehicle = new VehicleParameters
        {
            LengthMeters = 0.1d,
            WidthMeters = 0.1d,
            SafetyMarginMeters = 0d,
            MaximumCurvaturePerMeter = 1d,
        };
        return new LateralPlanningInput(segment, corridor,
            new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0d), 0d, 0d, 0d),
            EmTerminalType.Goal, vehicle, EmPlannerConfiguration.CreateDefault(), Array.Empty<FrenetProjection>());
    }

    private static SmoothedPathPoint Point(double x, double pathS)
    {
        return new SmoothedPathPoint(x, 0d, 0d, 0d, pathS, TravelDirection.Forward, 0d, 0d, 0d, 1d,
            false, SmoothedPathPointSource.Anchor);
    }

    private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal, double objective,
        double primalResidual = 0d, double dualResidual = 0d)
    {
        return new QpSolveResult(status, primal, objective, primalResidual, dualResidual, 1, TimeSpan.Zero,
            status.ToString(), string.Empty);
    }

    private static double[] CreatePrimal(LateralPlanningInput input, double middleL)
    {
        var layout = new LateralVariableLayout(input.ReferenceStations.Count);
        double c = 6d * middleL;
        var primal = new double[layout.VariableCount];
        primal[layout.L(0)] = 0d;
        primal[layout.L(1)] = middleL;
        primal[layout.L(2)] = 0d;
        primal[layout.DL(0)] = 0d;
        primal[layout.DL(1)] = 0d;
        primal[layout.DL(2)] = 0d;
        primal[layout.DDL(0)] = c;
        primal[layout.DDL(1)] = -c;
        primal[layout.DDL(2)] = c;
        primal[layout.DDDL(0)] = -2d * c;
        primal[layout.DDDL(1)] = 2d * c;
        return primal;
    }

    private static void FindSingleVariableBounds(QuadraticProgram problem, int variable, out double lower, out double upper)
    {
        for (int row = 0; row < problem.ConstraintCount; row++)
        {
            int matchingEntries = 0;
            double coefficient = 0d;
            for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
            {
                for (int index = problem.ConstraintMatrix.ColumnPointers[column];
                    index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
                {
                    if (problem.ConstraintMatrix.RowIndices[index] == row)
                    {
                        matchingEntries++;
                        if (column == variable)
                            coefficient = problem.ConstraintMatrix.Values[index];
                    }
                }
            }
            if (matchingEntries == 1 && Math.Abs(coefficient - 1d) <= 1e-12d &&
                Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) > 1e-12d)
            {
                lower = problem.LowerBounds[row];
                upper = problem.UpperBounds[row];
                return;
            }
        }
        throw new InvalidOperationException("Expected single-variable lateral trust-region row was not found.");
    }

    private sealed class LateralScenario
    {
        public LateralScenario(string name, LateralPlanningInput input, bool requiresSeedConnectedInterval)
        {
            Name = name;
            Input = input;
            RequiresSeedConnectedInterval = requiresSeedConnectedInterval;
        }

        public string Name { get; }
        public LateralPlanningInput Input { get; }
        public bool RequiresSeedConnectedInterval { get; }
    }
}