using System;
using System.Collections.Generic;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Builds curvature-aware, stopping-aware speed limits over actual optimized PathS.</summary>
public sealed class PathSpeedLimitBuilder
{
    internal const double CurvatureEpsilon = 1e-10d;
    private const double StopDistanceToleranceMeters = 1e-8d;
    private const double StationMergeToleranceMeters = 1e-12d;

    public EmPlanningStatus Build(LongitudinalPlanningInput input, out PathSpeedLimit speedLimit, out string failureReason)
    {
        speedLimit = null;
        failureReason = string.Empty;
        if (input == null)
        {
            failureReason = "Longitudinal planning input is required.";
            return EmPlanningStatus.InvalidInput;
        }

        if (!TryGetLimits(input, out double directionMaximum, out double maximumAcceleration, out double maximumDeceleration,
                out double maximumJerk, out double maximumLateralAcceleration, out double maximumCurvatureRate,
                out failureReason))
        {
            return EmPlanningStatus.InvalidInput;
        }
        if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
            input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
            input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
        {
            failureReason = "The initial longitudinal state violates the configured hard bounds.";
            return EmPlanningStatus.InvalidInput;
        }

        if (!JerkLimitedStoppingMath.TryCalculate(input.InitialProgressSpeedMetersPerSecond,
                input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk,
                out JerkLimitedStoppingProfile stopProfile, out failureReason))
        {
            return EmPlanningStatus.InvalidInput;
        }
        if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.TerminalPathS)
        {
            failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
            return EmPlanningStatus.StoppingDistanceInsufficient;
        }
        if (input.Configuration.Scheduling == null || !IsPositiveFinite(input.Configuration.Scheduling.OutputTimeStepSeconds))
        {
            failureReason = "The output time step required to refine the PathS speed envelope is invalid.";
            return EmPlanningStatus.InvalidInput;
        }

        double maximumStationSpacing = directionMaximum * input.Configuration.Scheduling.OutputTimeStepSeconds;
        var pathS = new List<double>();
        var maximum = new List<double>();
        var lateral = new List<double>();
        var curvatureRate = new List<double>();
        var stopping = new List<double>();
        for (int segmentIndex = 0; segmentIndex < input.Path.Points.Count - 1; segmentIndex++)
        {
            LateralPathPoint lowerPoint = input.Path.Points[segmentIndex];
            LateralPathPoint upperPoint = input.Path.Points[segmentIndex + 1];
            double span = upperPoint.PathS - lowerPoint.PathS;
            int subdivisions = Math.Max(1, checked((int)Math.Ceiling(span / maximumStationSpacing)));
            var segmentStations = new List<double>(subdivisions + 16);
            for (int subdivision = segmentIndex == 0 ? 0 : 1; subdivision <= subdivisions; subdivision++)
                segmentStations.Add(Interpolate(lowerPoint.PathS, upperPoint.PathS, (double)subdivision / subdivisions));
            AddDiscreteStoppingTailStations(lowerPoint.PathS, upperPoint.PathS, input.TerminalPathS, directionMaximum,
                maximumDeceleration, maximumJerk, input.Configuration.Scheduling.OutputTimeStepSeconds,
                segmentIndex == 0, segmentStations);
            segmentStations.Sort();
            double previousStation = double.NegativeInfinity;
            for (int stationIndex = 0; stationIndex < segmentStations.Count; stationIndex++)
            {
                double samplePathS = segmentStations[stationIndex];
                if (samplePathS <= previousStation + StationMergeToleranceMeters)
                    continue;
                previousStation = samplePathS;
                double fraction = (samplePathS - lowerPoint.PathS) / span;
                double curvature = Interpolate(lowerPoint.VehicleCurvature, upperPoint.VehicleCurvature, fraction);
                double curvatureDerivative = Interpolate(lowerPoint.VehicleCurvatureDerivative,
                    upperPoint.VehicleCurvatureDerivative, fraction);
                AddLimitSample(samplePathS, curvature, curvatureDerivative, input.TerminalPathS, directionMaximum,
                    maximumDeceleration, maximumLateralAcceleration, maximumCurvatureRate, pathS, maximum, lateral,
                    curvatureRate, stopping);
            }
        }

        try
        {
            speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum);
            return EmPlanningStatus.Success;
        }
        catch (ArgumentException exception)
        {
            failureReason = exception.Message;
            return EmPlanningStatus.InvalidInput;
        }
    }

    internal static bool TryGetLimits(LongitudinalPlanningInput input, out double directionMaximum,
        out double maximumAcceleration, out double maximumDeceleration, out double maximumJerk,
        out double maximumLateralAcceleration, out double maximumCurvatureRate, out string failureReason)
    {
        directionMaximum = 0d;
        maximumAcceleration = 0d;
        maximumDeceleration = 0d;
        maximumJerk = 0d;
        maximumLateralAcceleration = 0d;
        maximumCurvatureRate = 0d;
        failureReason = string.Empty;
        if (input.Configuration == null || input.Configuration.Longitudinal == null)
        {
            failureReason = "Longitudinal configuration is required.";
            return false;
        }

        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
        directionMaximum = input.DirectionMaximumSpeedMetersPerSecond;
        maximumAcceleration = configuration.MaximumAccelerationMetersPerSecondSquared;
        maximumDeceleration = configuration.MaximumDecelerationMetersPerSecondSquared;
        maximumJerk = configuration.MaximumJerkMetersPerSecondCubed;
        maximumLateralAcceleration = configuration.MaximumLateralAccelerationMetersPerSecondSquared;
        maximumCurvatureRate = configuration.MaximumCurvatureRatePerMeterPerSecond;
        if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) ||
            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
            !IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate))
        {
            failureReason = "Longitudinal limits must be positive and finite.";
            return false;
        }
        return true;
    }

    private static void AddDiscreteStoppingTailStations(double lowerPathS, double upperPathS, double terminalPathS,
        double directionMaximum, double maximumDeceleration, double maximumJerk, double timeStep, bool includeLower,
        IList<double> stations)
    {
        double speedIncrement = maximumDeceleration * timeStep;
        int tailStationCount = Math.Max(1, checked((int)Math.Ceiling(directionMaximum / speedIncrement)));
        for (int step = 1; step <= tailStationCount; step++)
        {
            double stopDuration = step * timeStep;
            double remainingDistance = 0.5d * maximumDeceleration * stopDuration * stopDuration;
            if (remainingDistance >= terminalPathS)
                break;
            double station = terminalPathS - remainingDistance;
            bool aboveLower = includeLower
                ? station >= lowerPathS - StationMergeToleranceMeters
                : station > lowerPathS + StationMergeToleranceMeters;
            if (aboveLower && station <= upperPathS + StationMergeToleranceMeters)
                stations.Add(Math.Max(lowerPathS, Math.Min(upperPathS, station)));
        }
        int jerkTailStationCount = Math.Max(1, checked((int)Math.Ceiling(
            maximumDeceleration / (maximumJerk * timeStep))));
        for (int step = 1; step <= jerkTailStationCount; step++)
        {
            double releaseDuration = step * timeStep;
            double remainingDistance = maximumJerk * releaseDuration * releaseDuration * releaseDuration / 6d;
            if (remainingDistance >= terminalPathS)
                break;
            double station = terminalPathS - remainingDistance;
            bool aboveLower = includeLower
                ? station >= lowerPathS - StationMergeToleranceMeters
                : station > lowerPathS + StationMergeToleranceMeters;
            if (aboveLower && station <= upperPathS + StationMergeToleranceMeters)
                stations.Add(Math.Max(lowerPathS, Math.Min(upperPathS, station)));
        }
    }

    private static void AddLimitSample(double samplePathS, double curvature, double curvatureDerivative, double terminalPathS,
        double directionMaximum, double maximumDeceleration, double maximumLateralAcceleration, double maximumCurvatureRate,
        IList<double> pathS, IList<double> maximum, IList<double> lateral, IList<double> curvatureRate, IList<double> stopping)
    {
        bool terminal = samplePathS >= terminalPathS;
        double lateralLimit = Math.Sqrt(maximumLateralAcceleration / Math.Max(Math.Abs(curvature), CurvatureEpsilon));
        double curvatureRateLimit = maximumCurvatureRate / Math.Max(Math.Abs(curvatureDerivative), CurvatureEpsilon);
        double stoppingLimit = Math.Sqrt(2d * maximumDeceleration * Math.Max(0d, terminalPathS - samplePathS));
        double lateralValue = ClampFinite(lateralLimit, directionMaximum);
        double curvatureRateValue = ClampFinite(curvatureRateLimit, directionMaximum);
        double stoppingValue = terminal ? 0d : ClampFinite(stoppingLimit, directionMaximum);
        pathS.Add(samplePathS);
        lateral.Add(lateralValue);
        curvatureRate.Add(curvatureRateValue);
        stopping.Add(stoppingValue);
        maximum.Add(terminal ? 0d : Math.Min(directionMaximum,
            Math.Min(lateralValue, Math.Min(curvatureRateValue, stoppingValue))));
    }

    private static double Interpolate(double lower, double upper, double fraction)
    {
        return lower + (upper - lower) * fraction;
    }

    private static double ClampFinite(double value, double maximum)
    {
        if (!IsFinite(value) || value < 0d)
            throw new ArgumentOutOfRangeException(nameof(value));
        return Math.Min(maximum, value);
    }

    private static bool IsPositiveFinite(double value)
    {
        return IsFinite(value) && value > 0d;
    }

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