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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Shared affine vehicle-curvature model used by the LS objective and hard QP constraints.</summary>
internal sealed class LateralCurvatureLinearization
{
    private LateralCurvatureLinearization(int[] variableIndices, double[] gradient, double constant)
    {
        VariableIndices = variableIndices;
        Gradient = gradient;
        Constant = constant;
    }

    internal IReadOnlyList<int> VariableIndices { get; }

    internal IReadOnlyList<double> Gradient { get; }

    internal double Constant { get; }

    internal static IReadOnlyList<LateralCurvatureLinearization> Create(LateralPlanningInput input,
        LateralVariableLayout layout, LateralCandidate linearization)
    {
        if (input == null) throw new ArgumentNullException(nameof(input));
        if (layout == null) throw new ArgumentNullException(nameof(layout));
        if (linearization == null) throw new ArgumentNullException(nameof(linearization));
        if (layout.StationCount != input.ReferenceStations.Count ||
            linearization.ReferenceStations.Count != layout.StationCount)
        {
            throw new ArgumentException("Curvature linearization stations must match the lateral layout.", nameof(linearization));
        }

        double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
        var affines = new List<LateralCurvatureLinearization>(layout.StationCount);
        for (int station = 0; station < layout.StationCount; station++)
        {
            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
                input.ReferenceStations[station]);
            double l = linearization.L[station];
            double dl = linearization.DL[station];
            double ddl = linearization.DDL[station];
            double referenceCurvature = reference.GeometricCurvature;
            double referenceCurvatureDerivative = directionSign * reference.VehicleCurvatureDerivative;
            double a = 1d - referenceCurvature * l;
            double denominatorSquared = a * a + dl * dl;
            if (!IsFinite(denominatorSquared) || denominatorSquared <= 0d)
                throw new ArgumentException("Curvature linearization denominator is invalid.", nameof(linearization));

            double denominatorPow3Over2 = denominatorSquared * Math.Sqrt(denominatorSquared);
            double denominatorPow5Over2 = denominatorPow3Over2 * denominatorSquared;
            double numerator = a * a * referenceCurvature + a * ddl +
                referenceCurvatureDerivative * l * dl + 2d * referenceCurvature * dl * dl;
            double geometricCurvature = numerator / denominatorPow3Over2;
            double dNumeratorDLateral = -2d * a * referenceCurvature * referenceCurvature -
                referenceCurvature * ddl + referenceCurvatureDerivative * dl;
            double dNumeratorDSlope = referenceCurvatureDerivative * l + 4d * referenceCurvature * dl;
            double dDenominatorSquaredDLateral = -2d * a * referenceCurvature;
            double dDenominatorSquaredDSlope = 2d * dl;
            double dGeometricDLateral = dNumeratorDLateral / denominatorPow3Over2 -
                1.5d * numerator * dDenominatorSquaredDLateral / denominatorPow5Over2;
            double dGeometricDSlope = dNumeratorDSlope / denominatorPow3Over2 -
                1.5d * numerator * dDenominatorSquaredDSlope / denominatorPow5Over2;
            double dGeometricDSecondDerivative = a / denominatorPow3Over2;
            double vehicleCurvature = directionSign * geometricCurvature;
            double[] gradient =
            {
                directionSign * dGeometricDLateral,
                directionSign * dGeometricDSlope,
                directionSign * dGeometricDSecondDerivative,
            };
            double constant = vehicleCurvature - gradient[0] * l - gradient[1] * dl - gradient[2] * ddl;
            if (!IsFinite(vehicleCurvature) || !IsFinite(constant) || !IsFinite(gradient[0]) ||
                !IsFinite(gradient[1]) || !IsFinite(gradient[2]))
            {
                throw new ArgumentException("Curvature linearization is non-finite.", nameof(linearization));
            }
            affines.Add(new LateralCurvatureLinearization(
                new[] { layout.L(station), layout.DL(station), layout.DDL(station) }, gradient, constant));
        }
        return affines;
    }

    internal static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
    {
        if (vehicle == null) throw new ArgumentNullException(nameof(vehicle));
        double maximum = vehicle.MaximumCurvaturePerMeter ??
            (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d
                ? 1d / vehicle.MinimumTurningRadiusMeters.Value
                : double.NaN);
        if (!IsFinite(maximum) || maximum <= 0d)
            throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
        return maximum;
    }

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