// Copyright (c) Xenko contributors (https://xenko.com) and Silicon Studio Corp. (https://www.siliconstudio.co.jp)
// Distributed under the MIT license. See the LICENSE.md file in the project root for more information.
//
// -----------------------------------------------------------------------------
// Original code from SlimMath project. http://code.google.com/p/slimmath/
// Greetings to SlimDX Group. Original code published with the following license:
// -----------------------------------------------------------------------------
/*
* Copyright (c) 2007-2011 SlimDX Group
* 
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
* 
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/

using System;
using System.Runtime.CompilerServices;
using System.Runtime.Serialization;

namespace math
{
    /// <summary>
    /// Common utility methods for math operations.
    /// </summary>
    public static class MathUtil
    {
        /// <summary>
        /// The value for which all absolute numbers smaller than are considered equal to zero.
        /// </summary>
        public const float ZeroTolerance = 1e-6f; // Value a 8x higher than 1.19209290E-07F

        /// <summary>
        /// The value for which all absolute numbers smaller than are considered equal to zero.
        /// </summary>
        public const double ZeroToleranceDouble = double.Epsilon * 8;

        /// <summary>
        /// A value specifying the approximation of π which is 180 degrees.
        /// </summary>
        public const float Pi = (float)Math.PI;

        /// <summary>
        /// A value specifying the approximation of 2π which is 360 degrees.
        /// </summary>
        public const float TwoPi = (float)(2 * Math.PI);

        /// <summary>
        /// A value specifying the approximation of π/2 which is 90 degrees.
        /// </summary>
        public const float PiOverTwo = (float)(Math.PI / 2);

        /// <summary>
        /// A value specifying the approximation of π/4 which is 45 degrees.
        /// </summary>
        public const float PiOverFour = (float)(Math.PI / 4);

        /// <summary>
        /// Checks if a and b are almost equals, taking into account the magnitude of floating point numbers (unlike <see cref="WithinEpsilon"/> method). See Remarks.
        /// See remarks.
        /// </summary>
        /// <param name="a">The left value to compare.</param>
        /// <param name="b">The right value to compare.</param>
        /// <returns><c>true</c> if a almost equal to b, <c>false</c> otherwise</returns>
        /// <remarks>
        /// The code is using the technique described by Bruce Dawson in 
        /// <a href="http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/">Comparing Floating point numbers 2012 edition</a>. 
        /// </remarks>
        public static unsafe bool NearEqual(float a, float b)
        {
            // Check if the numbers are really close -- needed
            // when comparing numbers near zero.
            if (IsZero(a - b))
                return true;

            // Original from Bruce Dawson: http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
            int aInt = *(int*)&a;
            int bInt = *(int*)&b;

            // Different signs means they do not match.
            if ((aInt < 0) != (bInt < 0))
                return false;

            // Find the difference in ULPs.
            int ulp = Math.Abs(aInt - bInt);

            // Choose of maxUlp = 4
            // according to http://code.google.com/p/googletest/source/browse/trunk/include/gtest/internal/gtest-internal.h
            const int maxUlp = 4;
            return (ulp <= maxUlp);
        }

        /// <summary>
        /// Determines whether the specified value is close to zero (0.0f).
        /// </summary>
        /// <param name="a">The floating value.</param>
        /// <returns><c>true</c> if the specified value is close to zero (0.0f); otherwise, <c>false</c>.</returns>
        public static bool IsZero(float a)
        {
            return Math.Abs(a) < ZeroTolerance;
        }

        /// <summary>
        /// Determines whether the specified value is close to zero (0.0f).
        /// </summary>
        /// <param name="a">The floating value.</param>
        /// <returns><c>true</c> if the specified value is close to zero (0.0f); otherwise, <c>false</c>.</returns>
        public static bool IsZero(double a)
        {
            return Math.Abs(a) < ZeroToleranceDouble;
        }

        /// <summary>
        /// Determines whether the specified value is close to one (1.0f).
        /// </summary>
        /// <param name="a">The floating value.</param>
        /// <returns><c>true</c> if the specified value is close to one (1.0f); otherwise, <c>false</c>.</returns>
        public static bool IsOne(float a)
        {
            return IsZero(a - 1.0f);
        }

        /// <summary>
        /// Checks if a - b are almost equals within a float epsilon.
        /// </summary>
        /// <param name="a">The left value to compare.</param>
        /// <param name="b">The right value to compare.</param>
        /// <param name="epsilon">Epsilon value</param>
        /// <returns><c>true</c> if a almost equal to b within a float epsilon, <c>false</c> otherwise</returns>
        public static bool WithinEpsilon(float a, float b, float epsilon)
        {
            float num = a - b;
            return ((-epsilon <= num) && (num <= epsilon));
        }

        /// <summary>
        /// Creates a one-dimensional array of the specified <typeparamref name="T"/> and <paramref name="length"/> filled with the specified <paramref name="value"/>.
        /// </summary>
        /// <typeparam name="T">The Type of the array to create.</typeparam>
        /// <param name="value">The value to fill the array with.</param>
        /// <param name="length">The size of the array to create.</param>
        /// <returns>A new one-dimensional array of the specified type with the specified length and filled with the specified value.</returns>
        public static T[] Array<T>(T value, int length)
        {
            var result = new T[length];
            for (var i = 0; i < length; i++)
                result[i] = value;

            return result;
        }

        /// <summary>
        /// Converts revolutions to degrees.
        /// </summary>
        /// <param name="revolution">The value to convert.</param>
        /// <returns>The converted value.</returns>
        public static float RevolutionsToDegrees(float revolution)
        {
            return revolution * 360.0f;
        }

        /// <summary>
        /// Converts revolutions to radians.
        /// </summary>
        /// <param name="revolution">The value to convert.</param>
        /// <returns>The converted value.</returns>
        public static float RevolutionsToRadians(float revolution)
        {
            return revolution * TwoPi;
        }

        /// <summary>
        /// Converts revolutions to gradians.
        /// </summary>
        /// <param name="revolution">The value to convert.</param>
        /// <returns>The converted value.</returns>
        public static float RevolutionsToGradians(float revolution)
        {
            return revolution * 400.0f;
        }

        /// <summary>
        /// Converts degrees to revolutions.
        /// </summary>
        /// <param name="degree">The value to convert.</param>
        /// <returns>The converted value.</returns>
        public static float DegreesToRevolutions(float degree)
        {
            return degree / 360.0f;
        }

        /// <summary>
        /// Converts degrees to radians.
        /// </summary>
        /// <param name="degree">The value to convert.</param>
        /// <returns>The converted value.</returns>
        public static float DegreesToRadians(float degree)
        {
            return degree * (Pi / 180.0f);
        }

        /// <summary>
        /// Converts radians to revolutions.
        /// </summary>
        /// <param name="radian">The value to convert.</param>
        /// <returns>The converted value.</returns>
        public static float RadiansToRevolutions(float radian)
        {
            return radian / TwoPi;
        }

        /// <summary>
        /// Converts radians to gradians.
        /// </summary>
        /// <param name="radian">The value to convert.</param>
        /// <returns>The converted value.</returns>
        public static float RadiansToGradians(float radian)
        {
            return radian * (200.0f / Pi);
        }

        /// <summary>
        /// Converts gradians to revolutions.
        /// </summary>
        /// <param name="gradian">The value to convert.</param>
        /// <returns>The converted value.</returns>
        public static float GradiansToRevolutions(float gradian)
        {
            return gradian / 400.0f;
        }

        /// <summary>
        /// Converts gradians to degrees.
        /// </summary>
        /// <param name="gradian">The value to convert.</param>
        /// <returns>The converted value.</returns>
        public static float GradiansToDegrees(float gradian)
        {
            return gradian * (9.0f / 10.0f);
        }

        /// <summary>
        /// Converts gradians to radians.
        /// </summary>
        /// <param name="gradian">The value to convert.</param>
        /// <returns>The converted value.</returns>
        public static float GradiansToRadians(float gradian)
        {
            return gradian * (Pi / 200.0f);
        }

        /// <summary>
        /// Converts radians to degrees.
        /// </summary>
        /// <param name="radian">The value to convert.</param>
        /// <returns>The converted value.</returns>
        public static float RadiansToDegrees(float radian)
        {
            return radian * (180.0f / Pi);
        }

        /// <summary>
        /// Clamps the specified value.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The min.</param>
        /// <param name="max">The max.</param>
        /// <returns>The result of clamping a value between min and max</returns>
        public static float Clamp(float value, float min, float max)
        {
            return value < min ? min : value > max ? max : value;
        }

        /// <summary>
        /// Clamps the specified value.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The min.</param>
        /// <param name="max">The max.</param>
        /// <returns>The result of clamping a value between min and max</returns>
        public static double Clamp(double value, double min, double max)
        {
            return value < min ? min : value > max ? max : value;
        }

        /// <summary>
        /// Clamps the specified value.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The min.</param>
        /// <param name="max">The max.</param>
        /// <returns>The result of clamping a value between min and max</returns>
        public static int Clamp(int value, int min, int max)
        {
            return value < min ? min : value > max ? max : value;
        }

        /// <summary>
        /// Inverse-interpolates a value linearly.
        /// </summary>
        /// <param name="min">Minimum value that takes place in inverse-interpolation.</param>
        /// <param name="max">Maximum value that takes place in inverse-interpolation.</param>
        /// <param name="value">Value to get inverse interpolation.</param>
        /// <returns>Returns an inverse-linearly interpolated coeficient.</returns>
        public static float InverseLerp(float min, float max, float value)
        {
            if (IsZero(Math.Abs(max - min)))
                return float.NaN;
            return (value - min) / (max - min);
        }

        /// <summary>
        /// Inverse-interpolates a value linearly.
        /// </summary>
        /// <param name="min">Minimum value that takes place in inverse-interpolation.</param>
        /// <param name="max">Maximum value that takes place in inverse-interpolation.</param>
        /// <param name="value">Value to get inverse interpolation.</param>
        /// <returns>Returns an inverse-linearly interpolated coeficient.</returns>
        public static double InverseLerp(double min, double max, double value)
        {
            if (IsZero(Math.Abs(max - min)))
                return double.NaN;
            return (value - min) / (max - min);
        }

        /// <summary>
        /// Interpolates between two values using a linear function by a given amount.
        /// </summary>
        /// <remarks>
        /// See http://www.encyclopediaofmath.org/index.php/Linear_interpolation and
        /// http://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
        /// </remarks>
        /// <param name="from">Value to interpolate from.</param>
        /// <param name="to">Value to interpolate to.</param>
        /// <param name="amount">Interpolation amount.</param>
        /// <returns>The result of linear interpolation of values based on the amount.</returns>
        public static double Lerp(double from, double to, double amount)
        {
            return (1 - amount) * from + amount * to;
        }

        /// <summary>
        /// Interpolates between two values using a linear function by a given amount.
        /// </summary>
        /// <remarks>
        /// See http://www.encyclopediaofmath.org/index.php/Linear_interpolation and
        /// http://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
        /// </remarks>
        /// <param name="from">Value to interpolate from.</param>
        /// <param name="to">Value to interpolate to.</param>
        /// <param name="amount">Interpolation amount.</param>
        /// <returns>The result of linear interpolation of values based on the amount.</returns>
        public static float Lerp(float from, float to, float amount)
        {
            return (1 - amount) * from + amount * to;
        }

        /// <summary>
        /// Interpolates between two values using a linear function by a given amount.
        /// </summary>
        /// <remarks>
        /// See http://www.encyclopediaofmath.org/index.php/Linear_interpolation and
        /// http://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/
        /// </remarks>
        /// <param name="from">Value to interpolate from.</param>
        /// <param name="to">Value to interpolate to.</param>
        /// <param name="amount">Interpolation amount.</param>
        /// <returns>The result of linear interpolation of values based on the amount.</returns>
        public static byte Lerp(byte from, byte to, float amount)
        {
            return (byte)Lerp((float)from, (float)to, amount);
        }

        /// <summary>
        /// Performs smooth (cubic Hermite) interpolation between 0 and 1.
        /// </summary>
        /// <remarks>
        /// See https://en.wikipedia.org/wiki/Smoothstep
        /// </remarks>
        /// <param name="amount">Value between 0 and 1 indicating interpolation amount.</param>
        public static float SmoothStep(float amount)
        {
            return (amount <= 0) ? 0
                : (amount >= 1) ? 1
                : amount * amount * (3 - (2 * amount));
        }

        /// <summary>
        /// Performs a smooth(er) interpolation between 0 and 1 with 1st and 2nd order derivatives of zero at endpoints.
        /// </summary>
        /// <remarks>
        /// See https://en.wikipedia.org/wiki/Smoothstep
        /// </remarks>
        /// <param name="amount">Value between 0 and 1 indicating interpolation amount.</param>
        public static float SmootherStep(float amount)
        {
            return (amount <= 0) ? 0
                : (amount >= 1) ? 1
                : amount * amount * amount * (amount * ((amount * 6) - 15) + 10);
        }

        /// <summary>
        /// Determines whether the value is inside the given range (inclusively).
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The minimum value of the range.</param>
        /// <param name="max">The maximum value of the range.</param>
        /// <returns><c>true</c> if value is inside the specified range; otherwise, <c>false</c>.</returns>
        public static bool IsInRange(float value, float min, float max)
        {
            return min <= value && value <= max;
        }

        /// <summary>
        /// Determines whether the value is inside the given range (inclusively).
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="min">The minimum value of the range.</param>
        /// <param name="max">The maximum value of the range.</param>
        /// <returns><c>true</c> if value is inside the specified range; otherwise, <c>false</c>.</returns>
        public static bool IsInRange(int value, int min, int max)
        {
            return min <= value && value <= max;
        }

        /// <summary>
        /// Determines whether the specified x is pow2.
        /// </summary>
        /// <param name="x">The x.</param>
        /// <returns><c>true</c> if the specified x is pow2; otherwise, <c>false</c>.</returns>
        public static bool IsPow2(int x)
        {
            return ((x != 0) && (x & (x - 1)) == 0);
        }

        /// <summary>
        /// Converts a float value from sRGB to linear.
        /// </summary>
        /// <param name="sRgbValue">The sRGB value.</param>
        /// <returns>A linear value.</returns>
        public static float SRgbToLinear(float sRgbValue)
        {
            if (sRgbValue < 0.04045f) return sRgbValue / 12.92f;
            return (float)Math.Pow((sRgbValue + 0.055) / 1.055, 2.4);
        }

        /// <summary>
        /// Converts a float value from linear to sRGB.
        /// </summary>
        /// <param name="linearValue">The linear value.</param>
        /// <returns>The encoded sRGB value.</returns>
        public static float LinearToSRgb(float linearValue)
        {
            if (linearValue < 0.0031308f) return linearValue * 12.92f;
            return (float)(1.055 * Math.Pow(linearValue, 1 / 2.4) - 0.055);
        }

        /// <summary>
        /// Calculate the logarithm 2 of a floating point.
        /// </summary>
        /// <param name="x">The input float</param>
        /// <returns><value>Log2(x)</value></returns>
        public static float Log2(float x)
        {
            return (float)Math.Log(x) / 0.6931471805599453f;
        }

        /// <summary>
        /// Calculate the logarithm 2 of an integer.
        /// </summary>
        /// <param name="i">The input integer</param>
        /// <returns><value>the log2(i) rounded to lower integer</value></returns>
        public static int Log2(int i)
        {
            var r = 0;

            while ((i >>= 1) != 0)
                ++r;

            return r;
        }

        /// <summary>
        /// Get the next power of two of an integer.
        /// </summary>
        /// <param name="x">The size.</param>
        /// <returns>System.Int32.</returns>
        /// <remarks>https://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2</remarks>
        public static int NextPowerOfTwo(int x)
        {
            if (x < 0)
                return 0;

            x--;
            x |= x >> 1;
            x |= x >> 2;
            x |= x >> 4;
            x |= x >> 8;
            x |= x >> 16;
            return x + 1;
        }

        /// <summary>
        /// Get the next power of two for a size.
        /// </summary>
        /// <param name="size">The size.</param>
        /// <returns>System.Int32.</returns>
        public static float NextPowerOfTwo(float size)
        {
            return (float)Math.Pow(2, Math.Ceiling(Math.Log(size, 2)));
        }

        /// <summary>
        /// Get the previous power of two of the provided integer.
        /// </summary>
        /// <param name="size">The value</param>
        public static int PreviousPowerOfTwo(int size)
        {
            return 1 << (int)Math.Floor(Math.Log(size, 2));
        }

        /// <summary>
        /// Get the previous power of two of the provided float.
        /// </summary>
        /// <param name="size">The value</param>
        public static float PreviousPowerOfTwo(float size)
        {
            return (float)Math.Pow(2, Math.Floor(Math.Log(size, 2)));
        }

        /// <summary>
        /// Alignes value up to match desire alignment.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="alignment">The alignment.</param>
        /// <returns>Aligned value (multiple of alignment).</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static int AlignUp(int value, int alignment)
        {
            int mask = alignment - 1;
            return (value + mask) & ~mask;
        }

        /// <summary>
        /// Alignes value down to match desire alignment.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="alignment">The alignment.</param>
        /// <returns>Aligned value (multiple of alignment).</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static int AlignDown(int value, int alignment)
        {
            int mask = alignment - 1;
            return value & ~mask;
        }

        /// <summary>
        /// Determines whether the specified value is aligned.
        /// </summary>
        /// <param name="value">The value.</param>
        /// <param name="alignment">The alignment.</param>
        /// <returns><c>true</c> if the specified value is aligned; otherwise, <c>false</c>.</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool IsAligned(int value, int alignment)
        {
            return (value & (alignment - 1)) == 0;
        }

        /// <summary>
        /// Snaps a value to the nearest interval.
        /// </summary>
        /// <param name="value">The value to snap.</param>
        /// <param name="gap">The interval gap.</param>
        /// <returns>The nearest interval to the provided value.</returns>
        public static float Snap(float value, float gap)
        {
            if (gap == 0)
                return value;
            return (float)Math.Round((value / gap), MidpointRounding.AwayFromZero) * gap;
        }

        /// <summary>
        /// Snaps a value to the nearest interval.
        /// </summary>
        /// <param name="value">The value to snap.</param>
        /// <param name="gap">The interval gap.</param>
        /// <returns>The nearest interval to the provided value.</returns>
        public static double Snap(double value, double gap)
        {
            if (gap == 0)
                return value;
            return Math.Round((value / gap), MidpointRounding.AwayFromZero) * gap;
        }

        /// <summary>
        /// Snaps all vector components to the nearest interval.
        /// </summary>
        /// <param name="value">The vector to snap.</param>
        /// <param name="gap">The interval gap.</param>
        /// <returns>A vector which components are snapped to the nearest interval.</returns>
        public static Vec2 Snap(Vec2 value, float gap)
        {
            if (gap == 0)
                return value;
            return new Vec2(
                (float)Math.Round((value.X / gap), MidpointRounding.AwayFromZero) * gap,
                (float)Math.Round((value.Y / gap), MidpointRounding.AwayFromZero) * gap);
        }

        /// <summary>
        /// Snaps all vector components to the nearest interval.
        /// </summary>
        /// <param name="value">The vector to snap.</param>
        /// <param name="gap">The interval gap.</param>
        /// <returns>A vector which components are snapped to the nearest interval.</returns>
        public static Vec3 Snap(Vec3 value, float gap)
        {
            if (gap == 0)
                return value;
            return new Vec3(
                (float)Math.Round((value.X / gap), MidpointRounding.AwayFromZero) * gap,
                (float)Math.Round((value.Y / gap), MidpointRounding.AwayFromZero) * gap,
                (float)Math.Round((value.Z / gap), MidpointRounding.AwayFromZero) * gap);
        }

        /// <summary>
        /// Snaps all vector components to the nearest interval.
        /// </summary>
        /// <param name="value">The vector to snap.</param>
        /// <param name="gap">The interval gap.</param>
        /// <returns>A vector which components are snapped to the nearest interval.</returns>
        public static Vec4 Snap(Vec4 value, float gap)
        {
            if (gap == 0)
                return value;
            return new Vec4(
                (float)Math.Round((value.X / gap), MidpointRounding.AwayFromZero) * gap,
                (float)Math.Round((value.Y / gap), MidpointRounding.AwayFromZero) * gap,
                (float)Math.Round((value.Z / gap), MidpointRounding.AwayFromZero) * gap,
                (float)Math.Round((value.W / gap), MidpointRounding.AwayFromZero) * gap);
        }
    }
}