using System;

namespace LinearAlgebra
{

    /// <summary>
    /// %Angle utilities
    /// </summary>
    public static class Angle
    {
        public const float pi = 3.1415927410125732421875F;
        // public static float Rad2Deg = 360.0f / ((float)Math.PI * 2);
        // public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f;

        public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F;
        public const float Deg2Rad = ((float)Math.PI * 2) / 360.0f; //57.29578F;

        /// <summary>
        /// Clamp the angle between the given min and max values
        /// </summary>
        /// <param name="angle">The angle to clamp</param>
        /// <param name="min">The minimum angle</param>
        /// <param name="max">The maximum angle</param>
        /// <returns>The clamped angle</returns>
        /// Angles are normalized
        public static float Clamp(float angle, float min, float max)
        {
            float normalizedAngle = Normalize(angle);
            return Float.Clamp(normalizedAngle, min, max);
        }

        /// <summary>
        /// Determine the angle difference, result is a normalized angle
        /// </summary>
        /// <param name="a">First first angle</param>
        /// <param name="b">The second angle</param>
        /// <returns>the angle between the two angles</returns>
        /// Angle values should be degrees
        public static float Difference(float a, float b)
        {
            float r = Normalize(b - a);
            return r;
        }

        /// <summary>
        /// Normalize an angle to the range -180 < angle <= 180
        /// </summary>
        /// <param name="angle">The angle to normalize</param>
        /// <returns>The normalized angle in interval (-180..180] </returns>
        /// Angle values should be in degrees
        public static float Normalize(float angle)
        {
            if (float.IsInfinity(angle))
                return angle;

            while (angle <= -180) angle += 360;
            while (angle > 180) angle -= 360;
            return angle;
        }

        /// <summary>
        /// Rotate from one angle to the other with a maximum degrees
        /// </summary>
        /// <param name="fromAngle">Starting angle</param>
        /// <param name="toAngle">Target angle</param>
        /// <param name="maxAngle">Maximum angle to rotate</param>
        /// <returns>The resulting angle</returns>
        /// This function is compatible with radian and degrees angles
        public static float MoveTowards(float fromAngle, float toAngle, float maxAngle)
        {
            float d = toAngle - fromAngle;
            d = Normalize(d);
            d = Math.Sign(d) * Float.Clamp(Math.Abs(d), 0, maxAngle);
            return fromAngle + d;
        }

        /// <summary>
        /// Map interval of angles between vectors [0..Pi] to interval [0..1]
        /// </summary>
        /// <param name="v1">The first vector</param>
        /// <param name="v2">The second vector</param>
        /// <returns>The resulting factor in interval [0..1]</returns>
        /// Vectors a and b must be normalized
        /// \deprecated Please use Vector2.ToFactor instead.
        [Obsolete("Please use Vector2.ToFactor instead.")]
        public static float ToFactor(Vector2 v1, Vector2 v2)
        {
            return (1 - Vector2.Dot(v1, v2)) / 2;
        }

        // Normalize all vector angles to the range -180 < angle < 180
        //public static Vector3 Normalize(Vector3 angles) {
        //    float x = Normalize(angles.x);
        //    float y = Normalize(angles.y);
        //    float z = Normalize(angles.z);
        //    return new Vector3(x, y, z);
        //}

        // Returns the signed angle in degrees between from and to.
        //public static float SignedAngle(Vector3 from, Vector3 to) {
        //    float angle = Vector3.Angle(from, to);
        //    Vector3 cross = Vector3.Cross(from, to);
        //    if (cross.y < 0) angle = -angle;
        //    return angle;
        //}

        // Returns the signed angle in degrees between from and to.
        //public static float SignedAngle(Vector2 from, Vector2 to) {
        //    float sign = Math.Sign(from.y * to.x - from.x * to.y);
        //    return Vector2.Angle(from, to) * sign;
        //}

        //public static Quaternion ToQuaternion(Rotation orientation) {
        //    return new Quaternion(orientation.x, orientation.y, orientation.z, orientation.w);
        //}
    }
}