using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public sealed class QpSolveResult
{
    public QpSolveResult(
        QpSolveStatus status,
        IReadOnlyList<double> primal,
        double objective,
        double primalResidual,
        double dualResidual,
        int iterations,
        TimeSpan solveTime,
        string nativeStatus,
        string diagnostic)
    {
        if (!Enum.IsDefined(typeof(QpSolveStatus), status))
            throw new ArgumentOutOfRangeException(nameof(status));
        if (primal == null)
            throw new ArgumentNullException(nameof(primal));
        if (iterations < 0)
            throw new ArgumentOutOfRangeException(nameof(iterations));
        if (solveTime < TimeSpan.Zero)
            throw new ArgumentOutOfRangeException(nameof(solveTime));
        if (!NumericGuard.IsFinite(objective) || !NumericGuard.IsFinite(primalResidual) || !NumericGuard.IsFinite(dualResidual))
            throw new ArgumentOutOfRangeException(nameof(objective), "Solver metrics must be finite.");

        var copiedPrimal = new List<double>(primal.Count);
        for (int index = 0; index < primal.Count; index++)
        {
            if (!NumericGuard.IsFinite(primal[index]))
                throw new ArgumentOutOfRangeException(nameof(primal), "Primal values must be finite.");
            copiedPrimal.Add(primal[index]);
        }

        Status = status;
        Primal = new ReadOnlyCollection<double>(copiedPrimal);
        Objective = objective;
        PrimalResidual = primalResidual;
        DualResidual = dualResidual;
        Iterations = iterations;
        SolveTime = solveTime;
        NativeStatus = nativeStatus ?? string.Empty;
        Diagnostic = diagnostic ?? string.Empty;
    }

    public QpSolveStatus Status { get; }

    public IReadOnlyList<double> Primal { get; }

    public double Objective { get; }

    public double PrimalResidual { get; }

    public double DualResidual { get; }

    public int Iterations { get; }

    public TimeSpan SolveTime { get; }

    public string NativeStatus { get; }

    public string Diagnostic { get; }
}
