using System;

namespace MultiWheelC.TrajectoryPlanning.Utils;

/// <summary>Deterministic angle helpers used by planning code.</summary>
public static class AngleMath
{
    public static double NormalizeRadians(double radians)
    {
        if (!NumericGuard.IsFinite(radians))
            return double.NaN;

        double normalized = radians % (2d * Math.PI);
        if (normalized >= Math.PI) normalized -= 2d * Math.PI;
        if (normalized < -Math.PI) normalized += 2d * Math.PI;
        return normalized;
    }

    public static double ShortestSignedDifference(double from, double to)
    {
        return NormalizeRadians(to - from);
    }

    public static int ToHeadingIndex(double heading, double resolution, int binCount)
    {
        if (!NumericGuard.IsFinite(heading) || !NumericGuard.IsPositiveFinite(resolution) || binCount <= 0)
            throw new ArgumentOutOfRangeException();

        double normalized = NormalizeRadians(heading);
        int index = (int)Math.Floor((normalized + Math.PI) / resolution);
        index %= binCount;
        return index < 0 ? index + binCount : index;
    }
}