using System;
#if UNITY_5_3_OR_NEWER
using Quaternion = UnityEngine.Quaternion;
#endif

namespace LinearAlgebra {

#if UNITY_5_3_OR_NEWER
    public class QuaternionExtensions {
        public static Quaternion Reflect(Quaternion q) {
            return new(-q.x, -q.y, -q.z, q.w);
        }

        public static Matrix2 ToRotationMatrix(Quaternion q) {
            float w = q.x, x = q.y, y = q.z, z = q.w;

            float[,] result = new float[,]
            {
                { 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y) },
                { 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x) },
                { 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) }
            };
            return new Matrix2(result);
        }

        public static Quaternion FromRotationMatrix(Matrix2 m) {
            float trace = m.data[0, 0] + m.data[1, 1] + m.data[2, 2];
            float w, x, y, z;

            if (trace > 0) {
                float s = 0.5f / (float)Math.Sqrt(trace + 1.0f);
                w = 0.25f / s;
                x = (m.data[2, 1] - m.data[1, 2]) * s;
                y = (m.data[0, 2] - m.data[2, 0]) * s;
                z = (m.data[1, 0] - m.data[0, 1]) * s;
            }
            else {
                if (m.data[0, 0] > m.data[1, 1] && m.data[0, 0] > m.data[2, 2]) {
                    float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[0, 0] - m.data[1, 1] - m.data[2, 2]);
                    w = (m.data[2, 1] - m.data[1, 2]) / s;
                    x = 0.25f * s;
                    y = (m.data[0, 1] + m.data[1, 0]) / s;
                    z = (m.data[0, 2] + m.data[2, 0]) / s;
                }
                else if (m.data[1, 1] > m.data[2, 2]) {
                    float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[1, 1] - m.data[0, 0] - m.data[2, 2]);
                    w = (m.data[0, 2] - m.data[2, 0]) / s;
                    x = (m.data[0, 1] + m.data[1, 0]) / s;
                    y = 0.25f * s;
                    z = (m.data[1, 2] + m.data[2, 1]) / s;
                }
                else {
                    float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[2, 2] - m.data[0, 0] - m.data[1, 1]);
                    w = (m.data[1, 0] - m.data[0, 1]) / s;
                    x = (m.data[0, 2] + m.data[2, 0]) / s;
                    y = (m.data[1, 2] + m.data[2, 1]) / s;
                    z = 0.25f * s;
                }
            }

            return new Quaternion(x, y, z, w);
        }
    }
#else
    public struct Quaternion {
        public float x;
        public float y;
        public float z;
        public float w;

        /// <summary>
        /// create a new quaternion with the given values
        /// </summary>
        /// <param name="x">x component</param>
        /// <param name="y">y component</param>
        /// <param name="z">z component</param>
        /// <param name="w">w component</param>
        public Quaternion(float x, float y, float z, float w) {
            this.x = x;
            this.y = y;
            this.z = z;
            this.w = w;
        }

        /// <summary>
        /// An identity quaternion
        /// </summary>
        public static readonly Quaternion identity = new(0, 0, 0, 1);

        private readonly Vector3Float xyz => new(x, y, z);

        private readonly float magnitude => MathF.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w);

        private readonly float sqrMagnitude => x * x + y * y + z * z + w * w;

        /// <summary>
        /// Convert to unit quaternion
        /// </summary>
        /// This will preserve the orientation,
        /// but ensures that it is a unit quaternion.
        public readonly Quaternion normalized {
            get {
                float length = this.magnitude;
                Quaternion q = new(this.x / length, this.y / length, this.z / length, this.w / length);
                return q;
            }
        }

        /// <summary>
        /// Convert to unity quaternion
        /// </summary>
        /// <param name="q">The quaternion to convert</param>
        /// <returns>A unit quaternion</returns>
        /// This will preserve the orientation,
        /// but ensures that it is a unit quaternion.
        public static Quaternion Normalize(Quaternion q) {
            return q.normalized;
        }

        /// <summary>
        /// Convert to euler angles
        /// </summary>
        /// <param name="q">The quaternion to convert</param>
        /// <returns>A vector containing euler angles</returns>
        /// The euler angles performed in the order: Z, X, Y
        public static Vector3Float ToAngles(Quaternion q) {
            // Extract Euler angles in Unity order (X = pitch, Y = yaw, Z = roll), returned in degrees as (x,pitch),(y,yaw),(z,roll)
            // Handle singularities/gimbal lock
            float test = 2f * (q.w * q.x - q.y * q.z);
            // clamp
            if (test >= 1f) test = 1f;
            if (test <= -1f) test = -1f;

            float pitch = MathF.Asin(test); // X

            float roll = MathF.Atan2(2f * (q.w * q.z + q.x * q.y),
                                     1f - 2f * (q.x * q.x + q.z * q.z)); // Z

            float yaw = MathF.Atan2(2f * (q.w * q.y + q.x * q.z),
                                    1f - 2f * (q.y * q.y + q.x * q.x)); // Y

            const float rad2deg = 180f / MathF.PI;
            return new Vector3Float(pitch * rad2deg, yaw * rad2deg, roll * rad2deg);
            // float test = q.x * q.y + q.z * q.w;
            // if (test > 0.499f)  // singularity at north pole
            //     return new Vector3Float(0, 2 * MathF.Atan2(q.x, q.w) * AngleFloat.Rad2Deg, 90);

            // else if (test < -0.499f)  // singularity at south pole
            //     return new Vector3Float(0, -2 * MathF.Atan2(q.x, q.w) * AngleFloat.Rad2Deg, -90);

            // else {
            //     float sqx = q.x * q.x;
            //     float sqy = q.y * q.y;
            //     float sqz = q.z * q.z;

            //     return new Vector3Float(
            //         MathF.Atan2(2 * q.x * q.w - 2 * q.y * q.z, 1 - 2 * sqx - 2 * sqz) *
            //             AngleFloat.Rad2Deg,
            //         MathF.Atan2(2 * q.y * q.w - 2 * q.x * q.z, 1 - 2 * sqy - 2 * sqz) *
            //             AngleFloat.Rad2Deg,
            //         MathF.Asin(2 * test) * AngleFloat.Rad2Deg);
            // }
        }

        /// <summary>
        /// Create a rotation from euler angles
        /// </summary>
        /// <param name="x">The angle around the right axis</param>
        /// <param name="y">The angle around the upward axis</param>
        /// <param name="z">The angle around the forward axis</param>
        /// <returns>The resulting quaternion</returns>
        /// Rotation are appied in the order Z, X, Y.
        public static Quaternion Euler(float x, float y, float z) {
            return Quaternion.Euler(new Vector3Float(x, y, z));
        }
        /// <summary>
        /// Create a rotation from a vector containing euler angles
        /// </summary>
        /// <param name="eulerAngles">Vector with the euler angles</param>
        /// <returns>The resulting quaternion</returns>
        /// Rotation are appied in the order Z, X, Y.
        public static Quaternion Euler(Vector3Float angles) {
            Vector3Float euler = angles * AngleFloat.Deg2Rad;
            float cx = MathF.Cos(euler.horizontal * 0.5f);
            float sx = MathF.Sin(euler.horizontal * 0.5f);
            float cy = MathF.Cos(euler.vertical * 0.5f);
            float sy = MathF.Sin(euler.vertical * 0.5f);
            float cz = MathF.Cos(euler.depth * 0.5f);
            float sz = MathF.Sin(euler.depth * 0.5f);

            // Unity uses intrinsic Z, then X, then Y -> q = Qy * Qx * Qz
            Quaternion q;
            q.w = cy * cx * cz + sy * sx * sz;
            q.x = cy * sx * cz + sy * cx * sz;
            q.y = sy * cx * cz - cy * sx * sz;
            q.z = cy * cx * sz - sy * sx * cz;
            return q;
        }

        /// <summary>
        /// Multiply two quaternions
        /// </summary>
        /// <param name="q1"></param>
        /// <param name="q2"></param>
        /// <returns>The resulting rotation</returns>
        public static Quaternion operator *(Quaternion q1, Quaternion q2) {
            return new Quaternion(
                q1.x * q2.w + q1.y * q2.z - q1.z * q2.y + q1.w * q2.x,
                -q1.x * q2.z + q1.y * q2.w + q1.z * q2.x + q1.w * q2.y,
                q1.x * q2.y - q1.y * q2.x + q1.z * q2.w + q1.w * q2.z,
                -q1.x * q2.x - q1.y * q2.y - q1.z * q2.z + q1.w * q2.w);
        }

        /// <summary>
        /// Rotate a vector using this quaterion
        /// </summary>
        /// <param name="q">The rotation</param>
        /// <param name="v">The vector to rotate</param>
        /// <returns>The rotated vector</returns>
        public static Vector3Float operator *(Quaternion q, Vector3Float v) {
            float num = q.x * 2;
            float num2 = q.y * 2;
            float num3 = q.z * 2;
            float num4 = q.x * num;
            float num5 = q.y * num2;
            float num6 = q.z * num3;
            float num7 = q.x * num2;
            float num8 = q.x * num3;
            float num9 = q.y * num3;
            float num10 = q.w * num;
            float num11 = q.w * num2;
            float num12 = q.w * num3;

            float px = v.horizontal;
            float py = v.vertical;
            float pz = v.depth;
            float rx =
                (1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz;
            float ry =
                (num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz;
            float rz =
                (num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz;
            Vector3Float result = new(rx, ry, rz);
            return result;
        }

        /// <summary>
        /// The inverse of quaterion
        /// </summary>
        /// <param name="quaternion">The quaternion for which the inverse is
        /// needed</param> <returns>The inverted quaternion</returns>
        public static Quaternion Inverse(Quaternion q) {
            float n = MathF.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
            return new Quaternion(-q.x / n, -q.y / n, -q.z / n, q.w / n);
        }

        /// <summary>
        /// A rotation which looks in the given direction
        /// </summary>
        /// <param name="forward">The look direction</param>
        /// <param name="upwards">The up direction</param>
        /// <returns>The look rotation</returns>
        public static Quaternion LookRotation(Vector3Float forward, Vector3Float up) {
            Vector3Float nForward = forward.normalized;
            Vector3Float nRight = Vector3Float.Normalize(Vector3Float.Cross(up, nForward));
            Vector3Float nUp = Vector3Float.Cross(nForward, nRight);
            float m00 = nRight.horizontal;      // x;
            float m01 = nRight.vertical;         // y;
            float m02 = nRight.depth;    // z;
            float m10 = nUp.horizontal;         // x;
            float m11 = nUp.vertical;            // y;
            float m12 = nUp.depth;       // z;
            float m20 = nForward.horizontal;    // x;
            float m21 = nForward.vertical;       // y;
            float m22 = nForward.depth;  // z;

            float num8 = (m00 + m11) + m22;
            float x, y, z, w;
            if (num8 > 0) {
                float num = MathF.Sqrt(num8 + 1);
                w = num * 0.5f;
                num = 0.5f / num;
                x = (m12 - m21) * num;
                y = (m20 - m02) * num;
                z = (m01 - m10) * num;
                return new Quaternion(x, y, z, w);
            }
            if ((m00 >= m11) && (m00 >= m22)) {
                float num7 = MathF.Sqrt(((1 + m00) - m11) - m22);
                float num4 = 0.5F / num7;
                x = 0.5f * num7;
                y = (m01 + m10) * num4;
                z = (m02 + m20) * num4;
                w = (m12 - m21) * num4;
                return new Quaternion(x, y, z, w);
            }
            if (m11 > m22) {
                float num6 = MathF.Sqrt(((1 + m11) - m00) - m22);
                float num3 = 0.5F / num6;
                x = (m10 + m01) * num3;
                y = 0.5F * num6;
                z = (m21 + m12) * num3;
                w = (m20 - m02) * num3;
                return new Quaternion(x, y, z, w);
            }
            float num5 = MathF.Sqrt(((1 + m22) - m00) - m11);
            float num2 = 0.5F / num5;
            x = (m20 + m02) * num2;
            y = (m21 + m12) * num2;
            z = 0.5F * num5;
            w = (m01 - m10) * num2;
            return new Quaternion(x, y, z, w);
        }

        /// <summary>
        /// Creates a quaternion with the given forward direction with up =
        /// Vector3::up
        /// </summary>
        /// <param name="forward">The look direction</param>
        /// <returns>The rotation for this direction</returns>
        /// For the rotation, Vector::up is used for the up direction.
        /// Note: if the forward direction == Vector3::up, the result is
        /// Quaternion::identity
        public static Quaternion LookRotation(Vector3Float forward) {
            Vector3Float up = new(0, 1, 0);
            return LookRotation(forward, up);
        }

        /// <summary>
        /// Calculat the rotation from on vector to another
        /// </summary>
        /// <param name="fromDirection">The from direction</param>
        /// <param name="toDirection">The to direction</param>
        /// <returns>The rotation from the first to the second vector</returns>
        public static Quaternion FromToRotation(Vector3Float fromDirection, Vector3Float toDirection) {
            Vector3Float axis = Vector3Float.Cross(fromDirection, toDirection);
            axis = axis.normalized;
            AngleFloat angle = Vector3Float.SignedAngle(fromDirection, toDirection, axis);
            Quaternion rotation = AngleAxis(angle, axis);
            return rotation;

        }

        /// <summary>
        /// Rotate form one orientation to anther with a maximum amount of degrees
        /// </summary>
        /// <param name="from">The from rotation</param>
        /// <param name="to">The destination rotation</param>
        /// <param name="maxDegreesDelta">The maximum amount of degrees to
        /// rotate</param> <returns>The possibly limited rotation</returns>
        public static Quaternion RotateTowards(Quaternion from, Quaternion to,
                                  float maxDegreesDelta) {
            float num = Quaternion.UnsignedAngle(from, to);
            if (num == 0) {
                return to;
            }
            float t = MathF.Min(1, maxDegreesDelta / num);
            return SlerpUnclamped(from, to, t);

        }

        /// <summary>
        /// Convert an angle/axis representation to a quaternion
        /// </summary>
        /// <param name="angle">The angle</param>
        /// <param name="axis">The axis</param>
        /// <returns>The resulting quaternion</returns>
        public static Quaternion AngleAxis(AngleFloat angle, Vector3Float axis) {
            if (axis.sqrMagnitude == 0.0f)
                return Quaternion.identity;

            float radians = angle.inRadians;
            radians *= 0.5f;

            Vector3Float axis2 = axis * MathF.Sin(radians);
            float x = axis2.horizontal;    // x;
            float y = axis2.vertical;       // y;
            float z = axis2.depth;  // z;
            float w = MathF.Cos(radians);

            return new Quaternion(x, y, z, w).normalized;
        }
        /// <summary>
        /// Convert this quaternion to angle/axis representation
        /// </summary>
        /// <param name="angle">A pointer to the angle for the result</param>
        /// <param name="axis">A pointer to the axis for the result</param>
        public readonly void ToAngleAxis(out AngleFloat angle, out Vector3Float axis) {
            Quaternion q1 = (MathF.Abs(this.w) > 1.0f) ? this.normalized : this;
            angle = AngleFloat.Radians(2.0f * MathF.Acos(q1.w));  // angle
            float den = MathF.Sqrt(1.0F - q1.w * q1.w);
            if (den > 0.0001f) {
                axis = Vector3Float.Normalize(q1.xyz / den);
            }
            else {
                // This occurs when the angle is zero.
                // Not a problem: just set an arbitrary normalized axis.
                axis = Vector3Float.right;
            }
        }

        /// <summary>
        /// Get the angle between two orientations
        /// </summary>
        /// <param name="orientation1">The first orientation</param>
        /// <param name="orientation2">The second orientation</param>
        /// <returns>The smallest angle in degrees between the two
        /// orientations</returns>
        public static float UnsignedAngle(Quaternion q1, Quaternion q2) {
            // float f = Dot(q1, q2);
            // return MathF.Acos(MathF.Min(MathF.Abs(f), 1)) * 2 * AngleFloat.Rad2Deg;

            float dot = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
            dot = MathF.Min(MathF.Max(dot, -1f), 1f);
            return 2f * MathF.Acos(MathF.Abs(dot)) * (180f / MathF.PI);
        }

        /// <summary>
        /// Sherical lerp between two rotations
        /// </summary>
        /// <param name="rotation1">The first rotation</param>
        /// <param name="rotation2">The second rotation</param>
        /// <param name="factor">The factor between 0 and 1.</param>
        /// <returns>The resulting rotation</returns>
        /// A factor 0 returns rotation1, factor1 returns rotation2.
        public static Quaternion Slerp(Quaternion a,
                          Quaternion b, float t) {
            if (t > 1)
                t = 1;
            if (t < 0)
                t = 0;
            return SlerpUnclamped(a, b, t);
        }

        /// <summary>
        /// Unclamped sherical lerp between two rotations
        /// </summary>
        /// <param name="rotation1">The first rotation</param>
        /// <param name="rotation2">The second rotation</param>
        /// <param name="factor">The factor</param>
        /// <returns>The resulting rotation</returns>
        /// A factor 0 returns rotation1, factor1 returns rotation2.
        /// Values outside the 0..1 range will result in extrapolated rotations
        public static Quaternion SlerpUnclamped(Quaternion a,
                                   Quaternion b, float t) {
            // if either input is zero, return the other.
            if (a.sqrMagnitude == 0.0f) {
                if (b.sqrMagnitude == 0.0f) {
                    return identity;
                }
                return b;
            }
            else if (b.sqrMagnitude == 0.0f) {
                return a;
            }

            Vector3Float axyz = a.xyz;
            Vector3Float bxyz = b.xyz;
            float cosHalfAngle = a.w * b.w + Vector3Float.Dot(axyz, bxyz);

            Quaternion b2 = b;
            if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) {
                // angle = 0.0f, so just return one input.
                return a;
            }
            else if (cosHalfAngle < 0.0f) {
                b2.x = -b.x;
                b2.y = -b.y;
                b2.z = -b.z;
                b2.w = -b.w;
                cosHalfAngle = -cosHalfAngle;
            }

            float blendA;
            float blendB;
            if (cosHalfAngle < 0.99f) {
                // do proper slerp for big angles
                float halfAngle = MathF.Acos(cosHalfAngle);
                float sinHalfAngle = MathF.Sin(halfAngle);
                float oneOverSinHalfAngle = 1.0F / sinHalfAngle;
                blendA = MathF.Sin(halfAngle * (1.0F - t)) * oneOverSinHalfAngle;
                blendB = MathF.Sin(halfAngle * t) * oneOverSinHalfAngle;
            }
            else {
                // do lerp if angle is really small.
                blendA = 1.0f - t;
                blendB = t;
            }
            Vector3Float v = axyz * blendA + b2.xyz * blendB;
            Quaternion result =
               new(v.horizontal, v.vertical, v.depth, blendA * a.w + blendB * b2.w);
            if (result.sqrMagnitude > 0.0f)
                return result.normalized;
            else
                return Quaternion.identity;
        }

        /// <summary>
        /// Convert this quaternion to angle/axis representation
        /// </summary>
        /// <param name="angle">A pointer to the angle for the result</param>
        /// <param name="axis">A pointer to the axis for the result</param>
        public readonly void ToAngleAxis(out float angle, out Vector3Float axis) {
            ToAxisAngleRad(this, out axis, out angle);
            angle *= AngleFloat.Rad2Deg;
        }
        private static void ToAxisAngleRad(Quaternion q,
                                out Vector3Float axis,
                                out float angle) {
            Quaternion q1 = (MathF.Abs(q.w) > 1.0f) ? Quaternion.Normalize(q) : q;
            angle = 2.0f * MathF.Acos(q1.w);  // angle
            float den = MathF.Sqrt(1.0F - q1.w * q1.w);
            if (den > 0.0001f) {
                axis = (q1.xyz / den).normalized;
            }
            else {
                // This occurs when the angle is zero.
                // Not a problem: just set an arbitrary normalized axis.
                axis = new Vector3Float(1, 0, 0);
            }
        }

        /// <summary>
        /// Returns the angle of around the give axis for a rotation
        /// </summary>
        /// <param name="axis">The axis around which the angle should be
        /// computed</param> <param name="rotation">The source rotation</param>
        /// <returns>The signed angle around the axis</returns>
        public static float GetAngleAround(Vector3Float axis, Quaternion rotation) {
            Quaternion secondaryRotation = GetRotationAround(axis, rotation);
            secondaryRotation.ToAngleAxis(out float rotationAngle, out Vector3Float rotationAxis);

            // Do the axis point in opposite directions?
            if (Vector3Float.Dot(axis, rotationAxis) < 0)
                rotationAngle = -rotationAngle;

            return rotationAngle;
        }

        /// <summary>
        /// Returns the rotation limited around the given axis
        /// </summary>
        /// <param name="axis">The axis which which the rotation should be
        /// limited</param> <param name="rotation">The source rotation</param>
        /// <returns>The rotation around the given axis</returns>
        public static Quaternion GetRotationAround(Vector3Float axis, Quaternion rotation) {
            Vector3Float ra = new(rotation.x, rotation.y, rotation.z);  // rotation axis
            Vector3Float p = Vector3Float.Project(
                ra, axis);  // return projection ra on to axis  (parallel component)
            Quaternion twist = new(p.horizontal, p.vertical, p.depth, rotation.w);
            twist = Normalize(twist);
            return twist;

        }

        /// <summary>
        /// Swing-twist decomposition of a rotation
        /// </summary>
        /// <param name="axis">The base direction for the decomposition</param>
        /// <param name="q">The source rotation</param>
        /// <param name="swing">A pointer to the quaternion for the swing
        /// result</param> <param name="twist">A pointer to the quaternion for the
        /// twist result</param>
        static void GetSwingTwist(Vector3Float axis, Quaternion q,
                                  out Quaternion swing, out Quaternion twist) {
            twist = GetRotationAround(axis, q);
            swing = q * Inverse(twist);
        }

        /// <summary>
        /// Calculate the dot product of two quaternions
        /// </summary>
        /// <param name="rotation1">The first rotation</param>
        /// <param name="rotation2">The second rotation</param>
        /// <returns></returns>
        public static float Dot(Quaternion q1, Quaternion q2) {
            return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
        }
    }
#endif

}