using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Immutable longitudinal inputs derived only from an independently validated lateral PathS path.</summary>
public sealed class LongitudinalPlanningInput
{
    private const double PathSTolerance = 1e-12d;

    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
        EmPlannerConfiguration configuration,
        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
        : this(path, direction, initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared,
            terminalType, mode, configuration, EmPlanningScope.RollingHorizon,
            CreateRollingSchedule(configuration), previousPathS, previousProgressSpeedMetersPerSecond)
    {
    }

    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
        EmPlannerConfiguration configuration, EmPlanningScope planningScope, LongitudinalKnotSchedule knotSchedule,
        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
    {
        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
            throw new ArgumentException("Longitudinal planning requires an independently validated lateral path with at least two points.",
                nameof(path));
        if (!Enum.IsDefined(typeof(TravelDirection), direction))
            throw new ArgumentOutOfRangeException(nameof(direction));
        if (!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d)
            throw new ArgumentOutOfRangeException(nameof(initialProgressSpeedMetersPerSecond));
        if (!IsFinite(initialAccelerationMetersPerSecondSquared))
            throw new ArgumentOutOfRangeException(nameof(initialAccelerationMetersPerSecondSquared));
        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
            throw new ArgumentOutOfRangeException(nameof(terminalType));
        if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode))
            throw new ArgumentOutOfRangeException(nameof(mode));
        if (mode == EmLongitudinalMode.RollingContinuation && terminalType != EmTerminalType.RollingSafetyStop)
            throw new ArgumentException("Rolling continuation requires a rolling window boundary.");
        if (mode != EmLongitudinalMode.RollingContinuation && terminalType == EmTerminalType.RollingSafetyStop)
            throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch.");
        if (configuration == null)
            throw new ArgumentNullException(nameof(configuration));
        if (!Enum.IsDefined(typeof(EmPlanningScope), planningScope))
            throw new ArgumentOutOfRangeException(nameof(planningScope));
        if (knotSchedule == null)
            throw new ArgumentNullException(nameof(knotSchedule));
        if ((planningScope == EmPlanningScope.FullDirectionSegment) != knotSchedule.IsAdaptive)
            throw new ArgumentException("Full-direction planning requires an adaptive schedule and rolling planning requires a rolling schedule.",
                nameof(knotSchedule));

        Path = CopyAndValidatePath(path);
        Direction = direction;
        InitialProgressSpeedMetersPerSecond = initialProgressSpeedMetersPerSecond;
        InitialAccelerationMetersPerSecondSquared = initialAccelerationMetersPerSecondSquared;
        TerminalType = terminalType;
        Mode = mode;
        Configuration = configuration.Copy();
        PlanningScope = planningScope;
        KnotSchedule = knotSchedule.Copy();
        PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
        PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
            nameof(previousProgressSpeedMetersPerSecond));
        if (PreviousPathS.Count != PreviousProgressSpeedMetersPerSecond.Count)
            throw new ArgumentException("Previous path-S and progress-speed samples must have matching counts.",
                nameof(previousProgressSpeedMetersPerSecond));
    }

    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
        : this(path, direction, initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, terminalType,
            terminalType == EmTerminalType.RollingSafetyStop
                ? EmLongitudinalMode.RollingContinuation
                : EmLongitudinalMode.ExactStopAtBoundary,
            configuration, previousPathS, previousProgressSpeedMetersPerSecond)
    {
    }

    public LateralPath Path { get; }

    public TravelDirection Direction { get; }

    public double InitialProgressSpeedMetersPerSecond { get; }

    public double InitialAccelerationMetersPerSecondSquared { get; }

    public EmTerminalType TerminalType { get; }

    public EmLongitudinalMode Mode { get; }

    public EmPlannerConfiguration Configuration { get; }

    public EmPlanningScope PlanningScope { get; }

    public LongitudinalKnotSchedule KnotSchedule { get; }

    public IReadOnlyList<double> PreviousPathS { get; }

    public IReadOnlyList<double> PreviousProgressSpeedMetersPerSecond { get; }

    public double PathUpperBoundS { get { return Path.Points[Path.Points.Count - 1].PathS; } }

    public bool HasStopBoundary { get { return TerminalType != EmTerminalType.RollingSafetyStop; } }

    public double StopBoundaryPathS { get { return HasStopBoundary ? PathUpperBoundS : double.NaN; } }

    public EmBoundaryType StopBoundaryType
    {
        get
        {
            return TerminalType == EmTerminalType.Goal
                ? EmBoundaryType.Goal
                : TerminalType == EmTerminalType.GearSwitch
                    ? EmBoundaryType.GearSwitchApproach
                    : EmBoundaryType.None;
        }
    }

    public double TerminalPathS { get { return PathUpperBoundS; } }

    public double DirectionMaximumSpeedMetersPerSecond
    {
        get
        {
            return Direction == TravelDirection.Forward
                ? Configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond
                : Configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond;
        }
    }

    private static LateralPath CopyAndValidatePath(LateralPath source)
    {
        var copy = new List<LateralPathPoint>(source.Points.Count);
        double previousPathS = double.NegativeInfinity;
        for (int index = 0; index < source.Points.Count; index++)
        {
            LateralPathPoint point = source.Points[index];
            if (point == null || !IsFinite(point.PathS) || point.PathS <= previousPathS)
                throw new ArgumentException("Lateral PathS must be finite and strictly increasing for ST planning.", nameof(source));
            if (index == 0 && Math.Abs(point.PathS) > PathSTolerance)
                throw new ArgumentException("The lateral PathS supplied to ST must begin at zero.", nameof(source));
            copy.Add(new LateralPathPoint(point.ReferenceS, point.PathS, point.L, point.DL, point.DDL, point.DDDL,
                point.X, point.Y, point.VehicleYaw, point.GeometricCurvature, point.VehicleCurvature,
                point.VehicleCurvatureDerivative));
            previousPathS = point.PathS;
        }
        return new LateralPath(copy, true);
    }

    private static IReadOnlyList<double> CopyFiniteNonnegative(IReadOnlyList<double> source, string parameterName)
    {
        var copy = new List<double>(source == null ? 0 : source.Count);
        if (source != null)
        {
            for (int index = 0; index < source.Count; index++)
            {
                if (!IsFinite(source[index]) || source[index] < 0d)
                    throw new ArgumentOutOfRangeException(parameterName);
                copy.Add(source[index]);
            }
        }
        return new ReadOnlyCollection<double>(copy);
    }

    private static LongitudinalKnotSchedule CreateRollingSchedule(EmPlannerConfiguration configuration)
    {
        if (configuration == null || configuration.Scheduling == null)
            throw new ArgumentNullException(nameof(configuration));
        return LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds,
            configuration.Scheduling.OutputTimeStepSeconds);
    }

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