using System;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal sealed class LateralPathInterpolator
{
    private const double BoundaryTolerance = 1e-10d;
    private readonly LateralPath path;

    public LateralPathInterpolator(LateralPath path)
    {
        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
            throw new ArgumentException("Trajectory assembly requires an independently validated lateral path.", nameof(path));

        double previousPathS = double.NegativeInfinity;
        for (int index = 0; index < path.Points.Count; index++)
        {
            LateralPathPoint point = path.Points[index];
            if (point == null || point.PathS <= previousPathS)
                throw new ArgumentException("Lateral path points must have strictly increasing PathS.", nameof(path));
            previousPathS = point.PathS;
        }

        this.path = path;
    }

    public InterpolatedLateralPathPoint Interpolate(double pathS)
    {
        if (!IsFinite(pathS))
            throw new ArgumentOutOfRangeException(nameof(pathS));

        LateralPathPoint first = path.Points[0];
        LateralPathPoint last = path.Points[path.Points.Count - 1];
        if (pathS < first.PathS - BoundaryTolerance || pathS > last.PathS + BoundaryTolerance)
            throw new ArgumentOutOfRangeException(nameof(pathS));
        if (pathS <= first.PathS + BoundaryTolerance)
            return From(first);
        if (pathS >= last.PathS - BoundaryTolerance)
            return From(last);

        for (int index = 1; index < path.Points.Count; index++)
        {
            LateralPathPoint right = path.Points[index];
            if (pathS <= right.PathS)
            {
                LateralPathPoint left = path.Points[index - 1];
                double ratio = (pathS - left.PathS) / (right.PathS - left.PathS);
                return new InterpolatedLateralPathPoint(
                    Linear(left.X, right.X, ratio),
                    Linear(left.Y, right.Y, ratio),
                    NormalizeYaw(left.VehicleYaw + ratio * NormalizeYaw(right.VehicleYaw - left.VehicleYaw)),
                    Linear(left.VehicleCurvature, right.VehicleCurvature, ratio));
            }
        }

        throw new InvalidOperationException("A PathS value inside the lateral path was not bracketed.");
    }

    private static InterpolatedLateralPathPoint From(LateralPathPoint point)
    {
        return new InterpolatedLateralPathPoint(point.X, point.Y, NormalizeYaw(point.VehicleYaw), point.VehicleCurvature);
    }

    private static double Linear(double left, double right, double ratio)
    {
        return left + ratio * (right - left);
    }

    internal static double NormalizeYaw(double yaw)
    {
        double normalized = yaw % (2d * Math.PI);
        if (normalized >= Math.PI)
            normalized -= 2d * Math.PI;
        if (normalized < -Math.PI)
            normalized += 2d * Math.PI;
        return normalized;
    }

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

internal sealed class InterpolatedLateralPathPoint
{
    public InterpolatedLateralPathPoint(double x, double y, double yaw, double vehicleCurvature)
    {
        X = x;
        Y = y;
        Yaw = yaw;
        VehicleCurvature = vehicleCurvature;
    }

    public double X { get; }
    public double Y { get; }
    public double Yaw { get; }
    public double VehicleCurvature { get; }
}