using System;
using System.Collections.Generic;
using System.Globalization;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Independently validates ST candidates directly in physical units before they may become fallbacks.</summary>
public sealed class LongitudinalSolutionValidator
{
    public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
        out LongitudinalCandidate validatedCandidate, out string failureReason)
    {
        return TryValidate(input, speedLimit, candidate, out validatedCandidate, out _, out failureReason);
    }

    public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
        out LongitudinalCandidate validatedCandidate, out EmPlanningStatus failureStatus, out string failureReason)
    {
        validatedCandidate = null;
        failureStatus = EmPlanningStatus.LongitudinalInfeasible;
        failureReason = string.Empty;
        if (input == null || speedLimit == null || candidate == null)
        {
            failureReason = "ST input, speed envelope, and candidate are required.";
            return false;
        }
        try
        {
            if (!PathSpeedLimitBuilder.TryGetLimits(input, out _, out double maximumAcceleration,
                    out double maximumDeceleration, out double maximumJerk, out _, out _, out failureReason))
            {
                return false;
            }
            IReadOnlyList<double> expectedTimes = input.KnotSchedule.KnotTimes;
            double tolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance, nameof(tolerance));
            if (!HasMatchingTimes(candidate.KnotTimes, expectedTimes, tolerance))
            {
                failureReason = "ST candidate knot times do not match the supplied knot schedule.";
                return false;
            }
            if (candidate.S.Count != expectedTimes.Count || candidate.U.Count != expectedTimes.Count ||
                candidate.A.Count != expectedTimes.Count || candidate.J.Count != expectedTimes.Count - 1)
            {
                failureReason = "ST candidate value counts do not match its time knots.";
                return false;
            }
            if (!candidate.SatisfiesExactDiscreteDynamics(tolerance))
            {
                failureReason = "ST candidate violates exact constant-jerk dynamics.";
                return false;
            }
            if (!LongitudinalContinuousProfileValidator.TryValidate(candidate, tolerance, out failureReason))
                return false;
            if (!AreClose(candidate.S[0], 0d, tolerance) ||
                !AreClose(candidate.U[0], input.InitialProgressSpeedMetersPerSecond, tolerance) ||
                !AreClose(candidate.A[0], input.InitialAccelerationMetersPerSecondSquared, tolerance))
            {
                failureReason = "ST candidate does not satisfy the exact initial state.";
                return false;
            }

            var canonicalS = new double[candidate.S.Count];
            var canonicalU = new double[candidate.U.Count];
            var canonicalA = new double[candidate.A.Count];
            for (int index = 0; index < candidate.S.Count; index++)
            {
                double progress = candidate.S[index];
                double speed = candidate.U[index];
                double acceleration = candidate.A[index];
                if (!IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration) || progress < -tolerance ||
                    progress > input.PathUpperBoundS + tolerance || speed < -tolerance ||
                    acceleration < -maximumDeceleration - tolerance || acceleration > maximumAcceleration + tolerance)
                {
                    failureReason = "ST candidate violates physical bounds at knot " + index +
                        " (S=" + progress + ", U=" + speed + ", A=" + acceleration + ").";
                    return false;
                }
                double speedLimitAtProgress = index == 0
                    ? input.DirectionMaximumSpeedMetersPerSecond
                    : speedLimit.MaximumSpeedAt(Math.Max(0d, Math.Min(input.PathUpperBoundS, progress)));
                if (speed > speedLimitAtProgress + tolerance)
                {
                    failureReason = "ST candidate violates the actual-PathS speed envelope at knot " + index +
                        " (S=" + progress + ", U=" + speed + ", limit=" + speedLimitAtProgress + ").";
                    return false;
                }
                if (index > 0 && progress < candidate.S[index - 1] - tolerance)
                {
                    failureReason = "ST candidate PathS decreases at knot " + index + ".";
                    return false;
                }
                canonicalS[index] = progress;
                canonicalU[index] = speed < 0d ? 0d : speed;
                canonicalA[index] = speed < 0d && acceleration < 0d && acceleration >= -tolerance
                    ? 0d
                    : acceleration;
            }
            for (int index = 0; index < candidate.J.Count; index++)
            {
                if (!IsFinite(candidate.J[index]) || Math.Abs(candidate.J[index]) > maximumJerk + tolerance)
                {
                    failureReason = "ST candidate violates the jerk bound at interval " + index + ".";
                    return false;
                }
            }
            bool requiresProgress = input.PathUpperBoundS >
                input.Configuration.Validation.TerminalPositionToleranceMeters;
            double achievedProgress = candidate.S[candidate.S.Count - 1] - candidate.S[0];
            if (input.PlanningScope == EmPlanningScope.FullDirectionSegment && requiresProgress &&
                achievedProgress <= input.Configuration.Validation.SpatialToleranceMeters)
            {
                failureStatus = EmPlanningStatus.NoProgress;
                failureReason = "NoProgress: a nonterminal full direction segment produced zero progress.";
                return false;
            }
            int stabilizationStart = candidate.S.Count;
            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
            {
                stabilizationStart = GetStabilizationStart(input, candidate.KnotTimes);
                for (int index = stabilizationStart; index < candidate.S.Count; index++)
                {
                    if (!AreClose(candidate.S[index], input.StopBoundaryPathS, tolerance) ||
                        !AreClose(candidate.U[index], 0d, tolerance) ||
                        !AreClose(candidate.A[index], 0d, tolerance))
                    {
                        failureReason = "ST candidate does not satisfy the exact stabilized S/U/A stop tail at knot " +
                            index + ".";
                        return false;
                    }
                }
            }
            if (input.Mode != EmLongitudinalMode.RollingContinuation)
            {
                for (int index = 0; index < candidate.S.Count; index++)
                {
                    bool hasStop = JerkLimitedStoppingMath.TryCalculate(canonicalU[index], canonicalA[index],
                        maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop,
                        out string stoppingFailure);
                    double stopDistance = hasStop ? stop.DistanceMeters : double.NaN;
                    double margin = hasStop
                        ? input.StopBoundaryPathS - canonicalS[index] - stopDistance
                        : double.NaN;
                    if (!hasStop || canonicalS[index] + stopDistance > input.StopBoundaryPathS + tolerance)
                    {
                        failureReason = "ST candidate leaves the jerk-limited stoppable set: knot=" +
                            index.ToString(CultureInfo.InvariantCulture) +
                            ";S=" + Invariant(canonicalS[index]) +
                            ";U=" + Invariant(canonicalU[index]) +
                            ";A=" + Invariant(canonicalA[index]) +
                            ";stopDistance=" + Invariant(stopDistance) +
                            ";stopBoundary=" + Invariant(input.StopBoundaryPathS) +
                            ";margin=" + Invariant(margin) +
                            (hasStop ? string.Empty : ";stoppingReason=" + stoppingFailure) + ".";
                        return false;
                    }
                }
            }

            canonicalS[0] = 0d;
            canonicalU[0] = input.InitialProgressSpeedMetersPerSecond;
            canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared;
            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
            {
                for (int index = stabilizationStart; index < candidate.S.Count; index++)
                {
                    canonicalS[index] = input.StopBoundaryPathS;
                    canonicalU[index] = 0d;
                    canonicalA[index] = 0d;
                }
            }
            var canonicalCandidate = new LongitudinalCandidate(candidate.KnotTimes, canonicalS, canonicalU, canonicalA,
                candidate.J);
            if (!canonicalCandidate.SatisfiesExactDiscreteDynamics(tolerance))
            {
                failureReason = "Canonical ST hard-boundary values exceed the dynamics tolerance.";
                return false;
            }
            validatedCandidate = canonicalCandidate;
            return true;
        }
        catch (ArgumentException exception)
        {
            failureReason = exception.Message;
            return false;
        }
    }

    private static bool HasMatchingTimes(IReadOnlyList<double> actual, IReadOnlyList<double> expected, double tolerance)
    {
        if (actual.Count != expected.Count)
            return false;
        for (int index = 0; index < expected.Count; index++)
        {
            if (!AreClose(actual[index], expected[index], tolerance))
                return false;
        }
        return true;
    }

    private static int GetStabilizationStart(LongitudinalPlanningInput input, IReadOnlyList<double> times)
    {
        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
        {
            if (input.KnotSchedule.TerminalHoldStartIndex < 1 ||
                input.KnotSchedule.TerminalHoldStartIndex >= times.Count)
            {
                throw new ArgumentException("Full-direction exact-stop schedules require an explicit terminal hold boundary.");
            }
            return input.KnotSchedule.TerminalHoldStartIndex;
        }
        return LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
            input.Configuration.Scheduling.OutputTimeStepSeconds);
    }

    private static bool AreClose(double actual, double expected, double tolerance)
    {
        return Math.Abs(actual - expected) <= tolerance;
    }

    private static string Invariant(double value)
    {
        return value.ToString("R", CultureInfo.InvariantCulture);
    }

    private static double RequireNonnegative(double value, string parameterName)
    {
        if (!IsFinite(value) || value < 0d)
            throw new ArgumentOutOfRangeException(parameterName);
        return value;
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}