using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal sealed class TrajectorySampleSchedule
{
    private const double ZeroTolerance = 1e-12d;

    public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds)
    {
        if (candidate == null)
            throw new ArgumentNullException(nameof(candidate));
        if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d)
            throw new ArgumentOutOfRangeException(nameof(outputTimeStepSeconds));
        if (!IsFinite(holdDurationSeconds) || holdDurationSeconds < 0d)
            throw new ArgumentOutOfRangeException(nameof(holdDurationSeconds));
        if (Math.Abs(candidate.U[candidate.U.Count - 1]) > ZeroTolerance)
            throw new ArgumentException("A publishable trajectory requires an exact zero-speed terminal candidate.", nameof(candidate));

        var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
        double previousPathS = double.NegativeInfinity;
        for (int index = 0; index < candidate.KnotTimes.Count; index++)
        {
            if (candidate.S[index] < previousPathS)
                throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
            if (candidate.U[index] < -ZeroTolerance)
                throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));

            samples.Add(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], Math.Max(0d, candidate.U[index]),
                candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, false));
            previousPathS = candidate.S[index];
        }

        TrajectorySample terminal = samples[samples.Count - 1];
        double holdElapsed = 0d;
        while (holdElapsed < holdDurationSeconds - ZeroTolerance)
        {
            holdElapsed = Math.Min(holdDurationSeconds, holdElapsed + outputTimeStepSeconds);
            samples.Add(new TrajectorySample(terminal.TimeFromStart + holdElapsed, terminal.PathS, 0d, 0d, 0d, true));
        }

        Samples = new ReadOnlyCollection<TrajectorySample>(samples);
    }

    public IReadOnlyList<TrajectorySample> Samples { get; }

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

internal sealed class TrajectorySample
{
    public TrajectorySample(double timeFromStart, double pathS, double progressSpeed, double acceleration, double jerk,
        bool isHoldSample)
    {
        TimeFromStart = timeFromStart;
        PathS = pathS;
        ProgressSpeed = progressSpeed;
        Acceleration = acceleration;
        Jerk = jerk;
        IsHoldSample = isHoldSample;
    }

    public double TimeFromStart { get; }
    public double PathS { get; }
    public double ProgressSpeed { get; }
    public double Acceleration { get; }
    public double Jerk { get; }
    public bool IsHoldSample { get; }
}