using System;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.EMPlanner;

namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

public sealed class TrajectoryObservationDiagnostic
{
    public TrajectoryObservationDiagnostic(string text)
    {
        Text = text ?? string.Empty;
    }

    public string Text { get; }
}

public static class TrajectoryObservationDiagnostics
{
    public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration)
    {
        if (configuration == null || configuration.Scheduling == null || configuration.Solver == null)
            throw new ArgumentNullException(nameof(configuration));

        double horizon = configuration.Scheduling.TimeHorizonSeconds;
        double outputTimeStep = configuration.Scheduling.OutputTimeStepSeconds;
        int trajectoryKnots = checked((int)Math.Ceiling(horizon / outputTimeStep) + 1);
        return new TrajectoryObservationDiagnostic(
            "planning configuration:\n" +
            "timeHorizon=" + Format(horizon, "F2") + "s\n" +
            "distanceHorizon=" + Format(configuration.Scheduling.DistanceHorizonMeters, "F2") + "m\n" +
            "outputTimeStep=" + Format(outputTimeStep, "F2") + "s\n" +
            "outputFrequency=" + Format(1d / outputTimeStep, "F2") + "Hz\n" +
            "trajectoryKnots=" + trajectoryKnots.ToString(CultureInfo.InvariantCulture) + "\n" +
            "maximumOsqpIterations=" + configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture) + "\n" +
            "solverTimeout=" + Format(configuration.Scheduling.SolverTimeoutSeconds, "F2") + "s\n" +
            "replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s");
    }

    public static TrajectoryObservationDiagnostic Create(PlanningCycleResult latestCycle,
        TimeSpan elapsed, bool planningInFlight, EmTrajectory publishedTrajectory)
    {
        if (latestCycle == null)
            return new TrajectoryObservationDiagnostic(planningInFlight
                ? "planning status=pending"
                : "planning status=not-started");

        string text = "planning cycle=" + latestCycle.Version.ToString(CultureInfo.InvariantCulture) +
            " status=" + latestCycle.Result.Status +
            " published=" + latestCycle.Published +
            " elapsed=" + Math.Max(0d, elapsed.TotalMilliseconds).ToString("F0", CultureInfo.InvariantCulture) +
            "ms";
        if (planningInFlight)
            text += "\nreplan=pending";
        if (publishedTrajectory != null)
            text += "\ntrajectory=" + publishedTrajectory.Metadata.TrajectoryId;
        if (!string.IsNullOrWhiteSpace(latestCycle.Result.FailureReason))
            text += "\nreason=" + latestCycle.Result.FailureReason;
        return new TrajectoryObservationDiagnostic(text);
    }

    private static string Format(double value, string format)
    {
        return value.ToString(format, CultureInfo.InvariantCulture);
    }
}