using System;
using System.Numerics;

namespace UniGLTF
{
    public static class NumericsExtensions
    {
        const float EPSILON = 1e-5f;

        public static bool NearlyEqual(this Vector3 lhs, Vector3 rhs)
        {
            if (Math.Abs(lhs.X - rhs.X) > EPSILON) return false;
            if (Math.Abs(lhs.Y - rhs.Y) > EPSILON) return false;
            if (Math.Abs(lhs.Z - rhs.Z) > EPSILON) return false;
            return true;
        }

        public static bool NearlyEqual(this Quaternion lhs, Quaternion rhs)
        {
            if (Math.Abs(lhs.X - rhs.X) > EPSILON) return false;
            if (Math.Abs(lhs.Y - rhs.Y) > EPSILON) return false;
            if (Math.Abs(lhs.Z - rhs.Z) > EPSILON) return false;
            if (Math.Abs(lhs.W - rhs.W) > EPSILON) return false;
            return true;
        }

        public const float TO_RAD = (float)(Math.PI / 180.0);


        public static Vector3 ReverseX(this Vector3 src)
        {
            return new Vector3(-src.X, src.Y, src.Z);
        }

        public static Vector3 ReverseZ(this Vector3 src)
        {
            return new Vector3(src.X, src.Y, -src.Z);
        }

        public static (Vector3, float) GetAxisAngle(this Quaternion q)
        {
            var qw = q.W;
            if (qw == 1)
            {
                return (Vector3.UnitX, 0);
            }
            var angle = 2 * Math.Acos(qw);
            var x = q.X / Math.Sqrt(1 - qw * qw);
            var y = q.Y / Math.Sqrt(1 - qw * qw);
            var z = q.Z / Math.Sqrt(1 - qw * qw);
            return (new Vector3((float)x, (float)y, (float)z), (float)angle);
        }

        public static Quaternion ReverseX(this Quaternion src)
        {
            var (axis, angle) = src.GetAxisAngle();
            return Quaternion.CreateFromAxisAngle(axis.ReverseX(), -angle);
        }

        public static Quaternion ReverseZ(this Quaternion src)
        {
            var (axis, angle) = src.GetAxisAngle();
            return Quaternion.CreateFromAxisAngle(axis.ReverseZ(), -angle);
        }

        public static Vector3 ExtractPosition(this Matrix4x4 matrix)
        {
            Vector3 position;
            position.X = matrix.M41;
            position.Y = matrix.M42;
            position.Z = matrix.M43;
            return position;
        }

        public static Quaternion ExtractRotation(this Matrix4x4 matrix)
        {
            return Quaternion.CreateFromRotationMatrix(matrix);
        }

        public static Vector3 ExtractScale(this Matrix4x4 matrix)
        {
            Vector3 scale;
            scale.X = new Vector4(matrix.M11, matrix.M12, matrix.M13, matrix.M14).Length();
            scale.Y = new Vector4(matrix.M21, matrix.M22, matrix.M23, matrix.M24).Length();
            scale.Z = new Vector4(matrix.M31, matrix.M32, matrix.M33, matrix.M34).Length();
            return scale;
        }

        public static Matrix4x4 FromTRS(Vector3 t, Quaternion r, Vector3 s)
        {
            var tt = Matrix4x4.CreateTranslation(t);
            var rr = Matrix4x4.CreateFromQuaternion(r);
            var ss = Matrix4x4.CreateScale(s);
            // return tt * rr * ss;
            return ss * rr * tt;
        }

        public static bool IsOnlyTranslation(this Matrix4x4 m)
        {
            if (m.M11 != 1.0f) return false;
            if (m.M22 != 1.0f) return false;
            if (m.M33 != 1.0f) return false;
            if (m.M12 != 0) return false;
            if (m.M13 != 0) return false;
            if (m.M23 != 0) return false;
            if (m.M21 != 0) return false;
            if (m.M31 != 0) return false;
            if (m.M32 != 0) return false;
            return true;
        }
    }
}