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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>One discrete lateral iterate expressed against exact reference-S stations.</summary>
public sealed class LateralCandidate
{
    public LateralCandidate(IReadOnlyList<double> referenceStations, IReadOnlyList<double> l, IReadOnlyList<double> dl,
        IReadOnlyList<double> ddl, IReadOnlyList<double> dddl)
    {
        ReferenceStations = CopyStations(referenceStations);
        int stationCount = ReferenceStations.Count;
        L = CopyValues(l, stationCount, nameof(l));
        DL = CopyValues(dl, stationCount, nameof(dl));
        DDL = CopyValues(ddl, stationCount, nameof(ddl));
        DDDL = CopyValues(dddl, stationCount - 1, nameof(dddl));
    }

    public IReadOnlyList<double> ReferenceStations { get; }

    public IReadOnlyList<double> L { get; }

    public IReadOnlyList<double> DL { get; }

    public IReadOnlyList<double> DDL { get; }

    public IReadOnlyList<double> DDDL { get; }

    public static LateralCandidate Integrate(IReadOnlyList<double> referenceStations, double initialL, double initialDL,
        double initialDDL, IReadOnlyList<double> dddl)
    {
        IReadOnlyList<double> stations = CopyStations(referenceStations);
        if (!IsFinite(initialL) || !IsFinite(initialDL) || !IsFinite(initialDDL))
            throw new ArgumentOutOfRangeException(nameof(initialL));
        IReadOnlyList<double> copiedJerk = CopyValues(dddl, stations.Count - 1, nameof(dddl));

        var l = new double[stations.Count];
        var dl = new double[stations.Count];
        var ddl = new double[stations.Count];
        l[0] = initialL;
        dl[0] = initialDL;
        ddl[0] = initialDDL;
        for (int index = 0; index < copiedJerk.Count; index++)
        {
            double ds = stations[index + 1] - stations[index];
            double jerk = copiedJerk[index];
            ddl[index + 1] = ddl[index] + ds * jerk;
            dl[index + 1] = dl[index] + ds * ddl[index] + 0.5d * ds * ds * jerk;
            l[index + 1] = l[index] + ds * dl[index] + 0.5d * ds * ds * ddl[index] +
                ds * ds * ds * jerk / 6d;
        }
        return new LateralCandidate(stations, l, dl, ddl, copiedJerk);
    }

    public bool SatisfiesExactDiscreteDynamics(double tolerance)
    {
        if (!IsFinite(tolerance) || tolerance < 0d)
            throw new ArgumentOutOfRangeException(nameof(tolerance));

        for (int index = 0; index < DDDL.Count; index++)
        {
            double ds = ReferenceStations[index + 1] - ReferenceStations[index];
            double jerk = DDDL[index];
            if (Math.Abs(DDL[index + 1] - (DDL[index] + ds * jerk)) > tolerance ||
                Math.Abs(DL[index + 1] - (DL[index] + ds * DDL[index] + 0.5d * ds * ds * jerk)) > tolerance ||
                Math.Abs(L[index + 1] - (L[index] + ds * DL[index] + 0.5d * ds * ds * DDL[index] +
                    ds * ds * ds * jerk / 6d)) > tolerance)
            {
                return false;
            }
        }
        return true;
    }

    private static IReadOnlyList<double> CopyStations(IReadOnlyList<double> values)
    {
        if (values == null || values.Count < 2)
            throw new ArgumentException("At least two reference-S stations are required.", nameof(values));

        var copy = new List<double>(values.Count);
        double previous = double.NegativeInfinity;
        for (int index = 0; index < values.Count; index++)
        {
            double value = values[index];
            if (!IsFinite(value) || value <= previous)
                throw new ArgumentException("Reference-S stations must be finite and strictly increasing.", nameof(values));
            copy.Add(value);
            previous = value;
        }
        return new ReadOnlyCollection<double>(copy);
    }

    private static IReadOnlyList<double> CopyValues(IReadOnlyList<double> values, int expectedCount, string parameterName)
    {
        if (values == null || values.Count != expectedCount)
            throw new ArgumentException("Lateral value count does not match the station layout.", parameterName);

        var copy = new List<double>(values.Count);
        for (int index = 0; index < values.Count; index++)
        {
            if (!IsFinite(values[index]))
                throw new ArgumentOutOfRangeException(parameterName);
            copy.Add(values[index]);
        }
        return new ReadOnlyCollection<double>(copy);
    }

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