// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

using System.Collections.Generic;
using System.Linq;
using UnityEngine;

namespace Microsoft.MixedReality.Toolkit.Utilities
{
    /// <summary>
    /// Math Utilities class.
    /// </summary>
    public static class MathUtilities
    {
        /// <summary>
        /// Takes a point in the coordinate space specified by the "from" transform and transforms it to be the correct
        /// point in the coordinate space specified by the "to" transform applies rotation, scale and translation.
        /// </summary>
        /// <returns>Point to.</returns>
        public static Vector3 TransformPointFromTo(Transform from, Transform to, Vector3 fromPoint)
        {
            Vector3 worldPoint = (from == null) ? fromPoint : from.TransformPoint(fromPoint);
            return (to == null) ? worldPoint : to.InverseTransformPoint(worldPoint);
        }

        /// <summary>
        /// Takes a direction in the coordinate space specified by the "from" transform and transforms it to be the correct direction in the coordinate space specified by the "to" transform
        /// applies rotation only, no translation or scale
        /// </summary>
        /// <returns>Direction to.</returns>
        public static Vector3 TransformDirectionFromTo(Transform from, Transform to, Vector3 fromDirection)
        {
            Vector3 worldDirection = (from == null) ? fromDirection : from.TransformDirection(fromDirection);
            return (to == null) ? worldDirection : to.InverseTransformDirection(worldDirection);
        }

        /// <summary>
        /// Takes a vector in the coordinate space specified by the "from" transform and transforms it to be the correct direction in the coordinate space specified by the "to" transform
        /// applies rotation and scale, no translation
        /// </summary>
        public static Vector3 TransformVectorFromTo(Transform from, Transform to, Vector3 vecInFrom)
        {
            Vector3 vecInWorld = (from == null) ? vecInFrom : from.TransformVector(vecInFrom);
            Vector3 vecInTo = (to == null) ? vecInWorld : to.InverseTransformVector(vecInWorld);
            return vecInTo;
        }

        /// <summary>
        /// Retrieve angular measurement describing how large a sphere or circle appears from a given point of view.
        /// Takes an angle (at given point of view) and a distance and returns the actual diameter of the object.
        /// </summary>
        public static float ScaleFromAngularSizeAndDistance(float angle, float distance)
        {
            float scale = 2.0f * distance * Mathf.Tan(angle * Mathf.Deg2Rad * 0.5f);
            return scale;
        }

        [System.Obsolete("Method obsolete. Use ScaleFromAngularSizeAndDistance instead")]
        public static float AngularScaleFromDistance(float angle, float distance)
        {
            return ScaleFromAngularSizeAndDistance(angle, distance);
        }

        /// <summary>
        /// Takes a ray in the coordinate space specified by the "from" transform and transforms it to be the correct ray in the coordinate space specified by the "to" transform
        /// </summary>
        public static Ray TransformRayFromTo(Transform from, Transform to, Ray rayToConvert)
        {
            Ray outputRay = new Ray
            {
                origin = TransformPointFromTo(from, to, rayToConvert.origin),
                direction = TransformDirectionFromTo(from, to, rayToConvert.direction)
            };

            return outputRay;
        }

        /// <summary>
        /// Creates a quaternion containing the rotation from the input matrix.
        /// </summary>
        /// <param name="m">Input matrix to convert to quaternion</param>
        public static Quaternion QuaternionFromMatrix(Matrix4x4 m)
        {
            // TODO: test and replace with this simpler, more unity-friendly code
            //       Quaternion q = Quaternion.LookRotation(m.GetColumn(2),m.GetColumn(1));

            Quaternion q = new Quaternion
            {
                w = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] + m[1, 1] + m[2, 2])) / 2,
                x = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] - m[1, 1] - m[2, 2])) / 2,
                y = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] + m[1, 1] - m[2, 2])) / 2,
                z = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] - m[1, 1] + m[2, 2])) / 2
            };
            q.x *= Mathf.Sign(q.x * (m[2, 1] - m[1, 2]));
            q.y *= Mathf.Sign(q.y * (m[0, 2] - m[2, 0]));
            q.z *= Mathf.Sign(q.z * (m[1, 0] - m[0, 1]));
            return q;
        }

        /// <summary>
        /// Extract the translation and rotation components of a Unity matrix
        /// </summary>
        public static void ToTranslationRotation(Matrix4x4 unityMtx, out Vector3 translation, out Quaternion rotation)
        {
            Vector3 upwards = new Vector3(unityMtx.m01, unityMtx.m11, unityMtx.m21);
            Vector3 forward = new Vector3(unityMtx.m02, unityMtx.m12, unityMtx.m22);
            translation = new Vector3(unityMtx.m03, unityMtx.m13, unityMtx.m23);
            rotation = Quaternion.LookRotation(forward, upwards);
        }

        /// <summary>
        /// Project vector onto XZ plane
        /// </summary>
        /// <returns>result of projecting v onto XZ plane</returns>
        public static Vector3 XZProject(Vector3 v)
        {
            return new Vector3(v.x, 0.0f, v.z);
        }

        /// <summary>
        /// Project vector onto YZ plane
        /// </summary>
        /// <returns>result of projecting v onto YZ plane</returns>
        public static Vector3 YZProject(Vector3 v)
        {
            return new Vector3(0.0f, v.y, v.z);
        }

        /// <summary>
        /// Project vector onto XY plane
        /// </summary>
        /// <returns>result of projecting v onto XY plane</returns>
        public static Vector3 XYProject(Vector3 v)
        {
            return new Vector3(v.x, v.y, 0.0f);
        }

        /// <summary>
        /// Returns the distance between a point and an infinite line defined by two points; linePointA and linePointB
        /// </summary>
        public static float DistanceOfPointToLine(Vector3 point, Vector3 linePointA, Vector3 linePointB)
        {
            Vector3 closestPoint = ClosestPointOnLineToPoint(point, linePointA, linePointB);
            return (point - closestPoint).magnitude;
        }

        public static Vector3 ClosestPointOnLineToPoint(Vector3 point, Vector3 linePointA, Vector3 linePointB)
        {
            Vector3 v = linePointB - linePointA;
            Vector3 w = point - linePointA;

            float c1 = Vector3.Dot(w, v);
            float c2 = Vector3.Dot(v, v);
            float b = c1 / c2;

            Vector3 pointB = linePointA + (v * b);

            return pointB;
        }

        public static float DistanceOfPointToLineSegment(Vector3 point, Vector3 lineStart, Vector3 lineEnd)
        {
            Vector3 closestPoint = ClosestPointOnLineSegmentToPoint(point, lineStart, lineEnd);
            return (point - closestPoint).magnitude;
        }

        public static Vector3 ClosestPointOnLineSegmentToPoint(Vector3 point, Vector3 lineStart, Vector3 lineEnd)
        {
            Vector3 v = lineEnd - lineStart;
            Vector3 w = point - lineStart;

            float c1 = Vector3.Dot(w, v);
            if (c1 <= 0)
            {
                return lineStart;
            }

            float c2 = Vector3.Dot(v, v);
            if (c2 <= c1)
            {
                return lineEnd;
            }

            float b = c1 / c2;

            Vector3 pointB = lineStart + (v * b);

            return pointB;
        }

        public static bool TestPlanesAABB(Plane[] planes, int planeMask, Bounds bounds, out bool entirelyInside)
        {
            int planeIndex = 0;
            int entirelyInsideCount = 0;
            Vector3 boundsCenter = bounds.center;  // center of bounds
            Vector3 boundsExtent = bounds.extents; // half diagonal
                                                   // do intersection test for each active frame
            int mask = 1;

            // while active frames
            while (mask <= planeMask)
            {
                // if active
                if ((uint)(planeMask & mask) != 0)
                {
                    Plane p = planes[planeIndex];
                    Vector3 n = p.normal;
                    n.x = Mathf.Abs(n.x);
                    n.y = Mathf.Abs(n.y);
                    n.z = Mathf.Abs(n.z);

                    float distance = p.GetDistanceToPoint(boundsCenter);
                    float radius = Vector3.Dot(boundsExtent, n);

                    if (distance + radius < 0)
                    {
                        // behind clip plane
                        entirelyInside = false;
                        return false;
                    }

                    if (distance > radius)
                    {
                        entirelyInsideCount++;
                    }
                }

                mask += mask;
                planeIndex++;
            }

            entirelyInside = entirelyInsideCount == planes.Length;
            return true;
        }

        /// <summary>
        /// Tests component-wise if a Vector2 is in a given range
        /// </summary>
        /// <param name="vec">The vector to test</param>
        /// <param name="lower">The lower bounds</param>
        /// <param name="upper">The upper bounds</param>
        /// <returns>true if in range, otherwise false</returns>
        public static bool InRange(Vector2 vec, Vector2 lower, Vector2 upper)
        {
            return vec.x >= lower.x && vec.x <= upper.x && vec.y >= lower.y && vec.y <= upper.y;
        }

        /// <summary>
        /// Tests component-wise if a Vector3 is in a given range
        /// </summary>
        /// <param name="vec">The vector to test</param>
        /// <param name="lower">The lower bounds</param>
        /// <param name="upper">The upper bounds</param>
        /// <returns>true if in range, otherwise false</returns>
        public static bool InRange(Vector3 vec, Vector3 lower, Vector3 upper)
        {
            return vec.x >= lower.x && vec.x <= upper.x && vec.y >= lower.y && vec.y <= upper.y && vec.z >= lower.z && vec.z <= upper.z;
        }

        /// <summary>
        /// Element-wise addition of two Matrix4x4s - extension method
        /// </summary>
        /// <param name="a">matrix</param>
        /// <param name="b">matrix</param>
        /// <returns>element-wise (a+b)</returns>
        public static Matrix4x4 Add(Matrix4x4 a, Matrix4x4 b)
        {
            Matrix4x4 result = new Matrix4x4();
            result.SetColumn(0, a.GetColumn(0) + b.GetColumn(0));
            result.SetColumn(1, a.GetColumn(1) + b.GetColumn(1));
            result.SetColumn(2, a.GetColumn(2) + b.GetColumn(2));
            result.SetColumn(3, a.GetColumn(3) + b.GetColumn(3));
            return result;
        }

        /// <summary>
        /// Element-wise subtraction of two Matrix4x4s - extension method
        /// </summary>
        /// <param name="a">matrix</param>
        /// <param name="b">matrix</param>
        /// <returns>element-wise (a-b)</returns>
        public static Matrix4x4 Subtract(Matrix4x4 a, Matrix4x4 b)
        {
            Matrix4x4 result = new Matrix4x4();
            result.SetColumn(0, a.GetColumn(0) - b.GetColumn(0));
            result.SetColumn(1, a.GetColumn(1) - b.GetColumn(1));
            result.SetColumn(2, a.GetColumn(2) - b.GetColumn(2));
            result.SetColumn(3, a.GetColumn(3) - b.GetColumn(3));
            return result;
        }

        /// <summary>
        /// find unsigned distance of 3D point to an infinite line
        /// </summary>
        /// <param name="ray">ray that specifies an infinite line</param>
        /// <param name="point">3D point</param>
        /// <returns>unsigned perpendicular distance from point to line</returns>
        public static float DistanceOfPointToLine(Ray ray, Vector3 point)
        {
            return Vector3.Cross(ray.direction, point - ray.origin).magnitude;
        }

        /// <summary>
        /// Find 3D point that minimizes distance to 2 lines, midpoint of the shortest perpendicular line segment between them
        /// </summary>
        /// <param name="p">ray that specifies a line</param>
        /// <param name="q">ray that specifies a line</param>
        /// <returns>point nearest to the lines</returns>
        public static Vector3 NearestPointToLines(Ray p, Ray q)
        {
            float a = Vector3.Dot(p.direction, p.direction);
            float b = Vector3.Dot(p.direction, q.direction);
            float c = Vector3.Dot(q.direction, q.direction);
            Vector3 w0 = p.origin - q.origin;
            float den = a * c - b * b;
            float epsilon = 0.00001f;
            if (den < epsilon)
            {
                // parallel, so just average origins
                return 0.5f * (p.origin + q.origin);
            }
            float d = Vector3.Dot(p.direction, w0);
            float e = Vector3.Dot(q.direction, w0);
            float sc = (b * e - c * d) / den;
            float tc = (a * e - b * d) / den;
            Vector3 point = 0.5f * (p.origin + sc * p.direction + q.origin + tc * q.direction);
            return point;
        }

        /// <summary>
        /// Find 3D point that minimizes distance to a set of 2 or more lines, ignoring outliers
        /// </summary>
        /// <param name="rays">list of rays, each specifying a line, must have at least 1</param>
        /// <param name="ransac_iterations">number of iterations:  log(1-p)/log(1-(1-E)^s)
        ///      where p is probability of at least one sample containing s points is all inliers
        ///      E is proportion of outliers (1-ransac_ratio)
        ///      e.g. p=0.999, ransac_ratio=0.54, s=2 ==>  log(0.001)/(log(1-0.54^2) = 20
        /// </param>
        /// <param name="ransac_threshold">minimum distance from point to line for a line to be considered an inlier</param>
        /// <param name="numActualInliers">return number of inliers: lines that are within ransac_threshold of nearest point</param>
        /// <returns>point nearest to the set of lines, ignoring outliers</returns>
        public static Vector3 NearestPointToLinesRANSAC(List<Ray> rays, int ransac_iterations, float ransac_threshold, out int numActualInliers)
        {
            // start with something, just in case no inliers - this works for case of 1 or 2 rays
            Vector3 nearestPoint = NearestPointToLines(rays[0], rays[rays.Count - 1]);
            numActualInliers = 0;
            if (rays.Count > 2)
            {
                for (int it = 0; it < ransac_iterations; it++)
                {
                    Vector3 testPoint = NearestPointToLines(rays[Random.Range(0, rays.Count)], rays[Random.Range(0, rays.Count)]);

                    // count inliers
                    int numInliersForIteration = 0;
                    for (int ind = 0; ind < rays.Count; ++ind)
                    {
                        if (DistanceOfPointToLine(rays[ind], testPoint) < ransac_threshold)
                            ++numInliersForIteration;
                    }

                    // remember best
                    if (numInliersForIteration > numActualInliers)
                    {
                        numActualInliers = numInliersForIteration;
                        nearestPoint = testPoint;
                    }
                }
            }

            // now find and count actual inliers and do least-squares to find best fit
            IEnumerable<Ray> inlierList = rays.Where(r => DistanceOfPointToLine(r, nearestPoint) < ransac_threshold);
            numActualInliers = inlierList.Count();
            if (numActualInliers >= 2)
            {
                nearestPoint = NearestPointToLinesLeastSquares(inlierList);
            }
            return nearestPoint;
        }

        /// <summary>
        /// Find 3D point that minimizes distance to a set of 2 or more lines
        /// </summary>
        /// <param name="rays">each ray specifies an infinite line</param>
        /// <returns>point nearest to the set of lines</returns>
        public static Vector3 NearestPointToLinesLeastSquares(IEnumerable<Ray> rays)
        {
            // finding the point nearest to the set of lines specified by rays
            // Use the following formula, where u_i are normalized direction
            // vectors along each ray and p_i is a point along each ray.

            //                      -1
            //  / =====             \   =====
            //  | \     /        T\ |   \     /        T\
            //  |  >    |I - u  u | |    >    |I - u  u | p
            //  | /     \     i  i/ |   /     \     i  i/  i
            //  | =====             |   =====
            //  \   i               /     i

            Matrix4x4 sumOfProduct = Matrix4x4.zero;
            Vector4 sumOfProductTimesDirection = Vector4.zero;

            foreach (Ray r in rays)
            {
                Vector4 point = r.origin;
                Matrix4x4 directionColumnMatrix = new Matrix4x4();
                Vector3 rNormal = r.direction.normalized;
                directionColumnMatrix.SetColumn(0, rNormal);
                Matrix4x4 directionRowMatrix = directionColumnMatrix.transpose;
                Matrix4x4 product = directionColumnMatrix * directionRowMatrix;
                Matrix4x4 identityMinusDirectionProduct = Subtract(Matrix4x4.identity, product);
                sumOfProduct = Add(sumOfProduct, identityMinusDirectionProduct);
                Vector4 vectorProduct = identityMinusDirectionProduct * point;
                sumOfProductTimesDirection += vectorProduct;
            }

            Matrix4x4 sumOfProductInverse = sumOfProduct.inverse;
            Vector3 nearestPoint = sumOfProductInverse * sumOfProductTimesDirection;
            return nearestPoint;
        }

        /// <summary>
        /// Convert degrees to radians.
        /// </summary>
        /// <param name="degrees">Angle, in degrees.</param>
        /// <returns>Angle, in radians.</returns>
        public static float DegreesToRadians(double degrees)
        {
            return (float)(degrees * Mathf.Deg2Rad);
        }

        /// <summary>
        /// Convert radians to degrees.
        /// </summary>
        /// <param name="radians">Angle, in radians.</param>
        /// <returns>Angle, in degrees.</returns>
        public static float RadiansToDegrees(float radians)
        {
            return (radians * Mathf.Rad2Deg);
        }

        /// <summary>
        /// Calculates the angle (at pointA) between two, two-dimensional points.
        /// </summary>
        /// <param name="pointA">The first point.</param>
        /// <param name="pointB">The second point.</param>
        /// <returns>
        /// The angle between the two points.
        /// </returns>
        public static float GetAngleBetween(Vector2 pointA, Vector2 pointB)
        {
            Vector2 diff = pointA - pointB;
            return MathUtilities.RadiansToDegrees(Mathf.Atan2(diff.y, diff.x));
        }

        /// <summary>
        /// Clamps via a lerp for a "soft" clamp effect
        /// </summary>
        /// <param name="pos">number to clamp</param>
        /// <param name="min">if pos is less than min, then lerp clamps to this value</param>
        /// <param name="max">if pos is more than max, lerp clamps to this value</param>
        /// <param name="clampFactor"> Range from 0.0f to 1.0f of how close to snap to min and max </param>
        /// <returns>A soft clamped value</returns>
        public static float CLampLerp(float pos, float min, float max, float clampFactor)
        {
            clampFactor = Mathf.Clamp(clampFactor, 0.0f, 1.0f);
            if (pos < min)
            {
                return Mathf.Lerp(pos, min, clampFactor);
            }
            else if (pos > max)
            {
                return Mathf.Lerp(pos, max, clampFactor);
            }

            return pos;
        }

        /// <summary>
        /// Calculates the direction vector from a rotation.
        /// </summary>
        /// <param name="rotation">Quaternion representing the rotation of the object.</param>
        /// <returns>
        /// Normalized Vector3 representing the direction vector.
        /// </returns>
        public static Vector3 GetDirection(Quaternion rotation)
        {
            return (rotation * Vector3.forward).normalized;
        }

        /// <summary>
        /// Returns if a point lies within a frame of reference view as defined by arguments
        /// </summary>
        /// <remarks>
        /// Field of view parameters are in degrees and plane distances are in meters 
        /// </remarks>
        public static bool IsInFOV(Vector3 testPosition, Transform frameOfReference,
            float verticalFOV, float horizontalFOV,
            float minPlaneDistance, float maxPlaneDistance)
        {
            Vector3 deltaPos = testPosition - frameOfReference.position;
            Vector3 referenceDeltaPos = TransformDirectionFromTo(null, frameOfReference, deltaPos);

            if (referenceDeltaPos.z < minPlaneDistance || referenceDeltaPos.z > maxPlaneDistance)
            {
                return false;
            }

            float verticalFovHalf = verticalFOV * 0.5f;
            float horizontalFovHalf = horizontalFOV * 0.5f;

            referenceDeltaPos = referenceDeltaPos.normalized;
            float yaw = Mathf.Asin(referenceDeltaPos.x) * Mathf.Rad2Deg;
            float pitch = Mathf.Asin(referenceDeltaPos.y) * Mathf.Rad2Deg;

            return Mathf.Abs(yaw) < horizontalFovHalf && Mathf.Abs(pitch) < verticalFovHalf;
        }

        /// <summary>
        /// Returns true if a point lies inside the cone described with given parameters, false otherwise.
        /// The cone is inscribed to a radius equal to the vertical height of the provided FOV.
        /// The test also ensures the distance from the point to the cone lies within the given range.
        /// </summary>
        /// <param name="cone">The transform that defines the orientation and position of the cone</param>
        /// <param name="point">The point to test if it lies within the cone FOV</param>
        /// <param name="fieldOfView">Field of view for the cone which calculates its radius</param>
        /// <param name="minDist">Point must be at least this far away (along direction forward) from the cone </param>
        /// <param name="maxDist">Point must be at most this far away (along direction forward) from the cone. </param>
        /// <remarks>
        /// Field of view parameter is in degrees and distances are in meters.
        /// </remarks>
        public static bool IsInFOVCone(Transform cone,
            Vector3 point,
            float fieldOfView,
            float minDist = 0.05f,
            float maxDist = 100f)
        {
            var dirToPoint = point - cone.position;

            var pointDist = Vector3.Dot(cone.forward, dirToPoint);
            if (pointDist < minDist || pointDist > maxDist)
            {
                return false;
            }

            var degrees = Mathf.Acos(pointDist / dirToPoint.magnitude) * Mathf.Rad2Deg;
            return degrees < fieldOfView * 0.5f;
        }

    }
}