using System;
using System.Collections.Generic;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Derives bounded full-direction ST knots from the physical PathS speed and stopping envelope.</summary>
public sealed class FullDirectionSegmentScheduleBuilder
{
    private const double Tolerance = 1e-10d;

    public EmPlanningStatus TryBuild(LateralPath path, PathSpeedLimit speedLimit,
        double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
        double desiredSpeedMetersPerSecond, EmPlannerConfiguration configuration,
        out LongitudinalKnotSchedule schedule, out string failureReason)
    {
        schedule = null;
        failureReason = string.Empty;
        if (path == null || speedLimit == null || configuration == null || configuration.Scheduling == null ||
            configuration.Longitudinal == null || !path.IsIndependentlyValidated || path.Points.Count < 2 ||
            !IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
            !IsFinite(initialAccelerationMetersPerSecondSquared) || !IsPositiveFinite(desiredSpeedMetersPerSecond))
        {
            failureReason = "Full-direction schedule inputs are invalid.";
            return EmPlanningStatus.InvalidInput;
        }
        if (!speedLimit.HasStopBoundary || Math.Abs(speedLimit.PathUpperBoundS -
                path.Points[path.Points.Count - 1].PathS) > Tolerance)
        {
            failureReason = "A full-direction schedule requires the matching real stop-boundary speed envelope.";
            return EmPlanningStatus.InvalidInput;
        }

        SchedulingConfiguration scheduling = configuration.Scheduling;
        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
        if (!IsPositiveFinite(scheduling.MaximumOptimizationTimeStepSeconds) ||
            !IsPositiveFinite(scheduling.MaximumOptimizationSpatialStepMeters) ||
            scheduling.MaximumOptimizationKnotCount < 3 ||
            !IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
            !IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
            !IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed))
        {
            failureReason = "Full-direction schedule limits are invalid.";
            return EmPlanningStatus.InvalidInput;
        }

        int stationCount = speedLimit.PathS.Count;
        var speeds = new double[stationCount];
        double desired = Math.Min(desiredSpeedMetersPerSecond, speedLimit.DirectionMaximumSpeedMetersPerSecond);
        speeds[0] = Math.Min(initialProgressSpeedMetersPerSecond, Math.Min(desired,
            speedLimit.MaximumSpeedMetersPerSecond[0]));
        for (int index = 1; index < stationCount; index++)
        {
            double distance = speedLimit.PathS[index] - speedLimit.PathS[index - 1];
            double reachable = Math.Sqrt(Math.Max(0d, speeds[index - 1] * speeds[index - 1] +
                2d * longitudinal.MaximumAccelerationMetersPerSecondSquared * distance));
            speeds[index] = Math.Min(reachable, Math.Min(desired, speedLimit.MaximumSpeedMetersPerSecond[index]));
        }
        speeds[stationCount - 1] = 0d;
        for (int index = stationCount - 2; index >= 0; index--)
        {
            double remainingDistance = speedLimit.PathUpperBoundS - speedLimit.PathS[index];
            double stopCap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(remainingDistance,
                Math.Max(0d, initialAccelerationMetersPerSecondSquared), longitudinal.MaximumDecelerationMetersPerSecondSquared,
                longitudinal.MaximumJerkMetersPerSecondCubed, speedLimit.DirectionMaximumSpeedMetersPerSecond);
            double distance = speedLimit.PathS[index + 1] - speedLimit.PathS[index];
            double decelerationCap = Math.Sqrt(Math.Max(0d, speeds[index + 1] * speeds[index + 1] +
                2d * longitudinal.MaximumDecelerationMetersPerSecondSquared * distance));
            speeds[index] = Math.Min(speeds[index], Math.Min(stopCap, decelerationCap));
        }

        var times = new List<double> { 0d };
        var pathS = new List<double> { 0d };
        var referenceSpeeds = new List<double> { speeds[0] };
        IReadOnlyList<int> scheduleStations = SelectScheduleStations(speedLimit.PathS, speeds);
        int minimumIntervalsPerSegment = Math.Max(1,
            (3 + scheduleStations.Count - 2) / (scheduleStations.Count - 1));
        for (int stationIndex = 1; stationIndex < scheduleStations.Count; stationIndex++)
        {
            int startIndex = scheduleStations[stationIndex - 1];
            int endIndex = scheduleStations[stationIndex];
            double startS = speedLimit.PathS[startIndex];
            double endS = speedLimit.PathS[endIndex];
            double startSpeed = speeds[startIndex];
            double endSpeed = speeds[endIndex];
            double distance = endS - startS;
            double denominator = startSpeed + endSpeed;
            double duration = denominator > Tolerance ? 2d * distance / denominator :
                Math.Sqrt(2d * distance / Math.Max(Tolerance, longitudinal.MaximumAccelerationMetersPerSecondSquared));
            int subdivisionCount = Math.Max(minimumIntervalsPerSegment, Math.Max(
                checked((int)Math.Ceiling(distance / scheduling.MaximumOptimizationSpatialStepMeters)),
                checked((int)Math.Ceiling(duration / scheduling.MaximumOptimizationTimeStepSeconds))));
            for (int subdivision = 1; subdivision <= subdivisionCount; subdivision++)
            {
                double fraction = (double)subdivision / subdivisionCount;
                times.Add(times[times.Count - 1] + duration / subdivisionCount);
                pathS.Add(startS + distance * fraction);
                referenceSpeeds.Add(startSpeed + (endSpeed - startSpeed) * fraction);
            }
        }
        referenceSpeeds[referenceSpeeds.Count - 1] = 0d;
        pathS[pathS.Count - 1] = speedLimit.PathUpperBoundS;
        EnsureJerkReachableReferenceTimes(times, referenceSpeeds, longitudinal);
        EnsureMinimumExactStopDuration(times, speedLimit.PathUpperBoundS, initialProgressSpeedMetersPerSecond,
            initialAccelerationMetersPerSecondSquared, longitudinal);
        if (!IsFinite(longitudinal.ZeroSpeedHoldSeconds) || longitudinal.ZeroSpeedHoldSeconds < 0d)
        {
            failureReason = "The full-direction zero-speed hold duration is invalid.";
            return EmPlanningStatus.InvalidInput;
        }
        int terminalHoldStartIndex = times.Count - 1;
        double remainingHold = longitudinal.ZeroSpeedHoldSeconds;
        while (remainingHold > Tolerance)
        {
            double holdStep = Math.Min(remainingHold, scheduling.MaximumOptimizationTimeStepSeconds);
            times.Add(times[times.Count - 1] + holdStep);
            pathS.Add(speedLimit.PathUpperBoundS);
            referenceSpeeds.Add(0d);
            remainingHold -= holdStep;
        }
        if (times.Count > scheduling.MaximumOptimizationKnotCount)
        {
            failureReason = "Full-direction schedule required knots=" + times.Count + ", configured maximum=" +
                scheduling.MaximumOptimizationKnotCount + ".";
            return EmPlanningStatus.FullSegmentResourceLimitExceeded;
        }
        try
        {
            schedule = LongitudinalKnotSchedule.CreateAdaptive(times, pathS, referenceSpeeds,
                terminalHoldStartIndex);
            return EmPlanningStatus.Success;
        }
        catch (ArgumentException exception)
        {
            failureReason = exception.Message;
            return EmPlanningStatus.InvalidInput;
        }
    }

    private static void EnsureMinimumExactStopDuration(IList<double> times, double stopBoundaryPathS,
        double initialSpeed, double initialAcceleration, LongitudinalConfiguration configuration)
    {
        if (initialSpeed <= Tolerance || !JerkLimitedStoppingMath.TryCalculate(initialSpeed, initialAcceleration,
                configuration.MaximumDecelerationMetersPerSecondSquared,
                configuration.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile stop, out _))
        {
            return;
        }
        double cruiseDistance = Math.Max(0d, stopBoundaryPathS - stop.DistanceMeters);
        double requiredDuration = stop.DurationSeconds + cruiseDistance / initialSpeed;
        double stopSpeedTolerance = configuration.StopSpeedToleranceMetersPerSecond;
        if (IsPositiveFinite(stopSpeedTolerance) && JerkLimitedStoppingMath.TryCalculate(stopSpeedTolerance, 0d,
                configuration.MaximumDecelerationMetersPerSecondSquared,
                configuration.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile settlingStop, out _))
        {
            double envelopeTraverseDuration = 2d * stopBoundaryPathS / (initialSpeed + stopSpeedTolerance);
            requiredDuration = Math.Max(requiredDuration, envelopeTraverseDuration + settlingStop.DurationSeconds);
        }
        double currentDuration = times[times.Count - 1];
        if (currentDuration + Tolerance >= requiredDuration)
            return;
        double scale = requiredDuration / currentDuration;
        for (int index = 1; index < times.Count; index++)
            times[index] *= scale;
    }

    private static void EnsureJerkReachableReferenceTimes(IList<double> times, IReadOnlyList<double> referenceSpeeds,
        LongitudinalConfiguration configuration)
    {
        double adjustedTime = 0d;
        for (int index = 1; index < times.Count; index++)
        {
            double requestedDuration = times[index] - times[index - 1];
            double speedChange = Math.Abs(referenceSpeeds[index] - referenceSpeeds[index - 1]);
            double accelerationLimit = referenceSpeeds[index] >= referenceSpeeds[index - 1]
                ? configuration.MaximumAccelerationMetersPerSecondSquared
                : configuration.MaximumDecelerationMetersPerSecondSquared;
            double accelerationDuration = speedChange / accelerationLimit;
            double triangularJerkDuration = speedChange <= Tolerance
                ? 0d
                : 2d * Math.Sqrt(speedChange / configuration.MaximumJerkMetersPerSecondCubed);
            adjustedTime += Math.Max(requestedDuration, Math.Max(accelerationDuration, triangularJerkDuration));
            times[index] = adjustedTime;
        }
    }

    private static IReadOnlyList<int> SelectScheduleStations(IReadOnlyList<double> pathS,
        IReadOnlyList<double> speeds)
    {
        var stations = new List<int> { 0 };
        for (int index = 1; index < pathS.Count - 1; index++)
        {
            double previousSlope = (speeds[index] - speeds[index - 1]) / (pathS[index] - pathS[index - 1]);
            double nextSlope = (speeds[index + 1] - speeds[index]) / (pathS[index + 1] - pathS[index]);
            if (previousSlope * nextSlope < 0d)
                stations.Add(index);
        }
        stations.Add(pathS.Count - 1);
        return stations;
    }

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