using System;
using System.Collections.Generic;
using System.Globalization;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
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 (latestCycle.Published && latestCycle.Result.Trajectory != null)
            text += "\n" + CreateTrajectorySummary(latestCycle.Result.Trajectory);
        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);
    }

    private static string CreateTrajectorySummary(EmTrajectory trajectory)
    {
        IReadOnlyList<EmTrajectoryPoint> points = trajectory.Points;
        double maximumSpeed = 0d;
        double maximumAcceleration = 0d;
        double maximumJerk = 0d;
        double previousAcceleration = 0d;
        bool hasPreviousAcceleration = false;
        double directionSign = trajectory.Metadata.Direction == TravelDirection.Forward ? 1d : -1d;
        for (int index = 0; index < points.Count; index++)
        {
            EmTrajectoryPoint point = points[index];
            maximumSpeed = Math.Max(maximumSpeed, Math.Abs(point.SignedLongitudinalVelocity));
            if (index == 0) continue;

            EmTrajectoryPoint previous = points[index - 1];
            double dt = point.TimeFromStart - previous.TimeFromStart;
            if (!IsPositiveFinite(dt)) continue;
            double acceleration = (directionSign * point.SignedLongitudinalVelocity -
                directionSign * previous.SignedLongitudinalVelocity) / dt;
            maximumAcceleration = Math.Max(maximumAcceleration, Math.Abs(acceleration));
            if (hasPreviousAcceleration)
                maximumJerk = Math.Max(maximumJerk, Math.Abs((acceleration - previousAcceleration) / dt));
            previousAcceleration = acceleration;
            hasPreviousAcceleration = true;
        }

        EmTrajectoryPoint first = points[0];
        EmTrajectoryPoint last = points[points.Count - 1];
        return "trajectory summary:\n" +
            "trajectoryId=" + trajectory.Metadata.TrajectoryId + ", points=" +
            points.Count.ToString(CultureInfo.InvariantCulture) + ", duration=" +
            Format(last.TimeFromStart, "F3") + "s, pathLength=" +
            Format(last.PathS - first.PathS, "F3") + "m\n" +
            "maxSpeed=" + Format(maximumSpeed, "F3") + "m/s, maxAcceleration=" +
            Format(maximumAcceleration, "F3") + "m/s2, maxJerk=" +
            Format(maximumJerk, "F3") + "m/s3";
    }

    private static bool IsPositiveFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
    }
}