using System;
using System.Collections.Generic;
using System.Numerics;
using System.Text;

using NUnit.Framework;

namespace SharpGLTF
{
    // [System.Diagnostics.DebuggerStepThrough]
    public static class NumericsAssert
    {
        public static double UnitError(this Vector3 v) { return v.LengthError(1); }
        
        public static double LengthError(this Vector3 v, double expectedLength)
        {
            return Math.Abs(Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z) - expectedLength);
        }

        public static void IsFinite(Single value, string message = null)
        {
            // Assert.IsTrue(float.IsFinite(value), message);

            Assert.That(!float.IsNaN(value) && !float.IsInfinity(value), Is.True, message);
        }

        public static void IsFinite(Double value, string message = null)
        {
            // Assert.IsTrue(double.IsFinite(value), message);

            Assert.That(!Double.IsNaN(value) && !Double.IsInfinity(value), Is.True, message);
        }

        public static void IsFinite(Vector2 vector)
        {
            IsFinite(vector.X, "X");
            IsFinite(vector.Y, "Y");
        }

        public static void IsFinite(Vector3 vector)
        {
            IsFinite(vector.X, "X");
            IsFinite(vector.Y, "Y");
            IsFinite(vector.Z, "Z");
        }

        public static void IsFinite(Vector4 vector)
        {
            IsFinite(vector.X, "X");
            IsFinite(vector.Y, "Y");
            IsFinite(vector.Z, "Z");
            IsFinite(vector.W, "W");
        }

        public static void IsFinite(Quaternion quaternion)
        {
            IsFinite(quaternion.X, "X");
            IsFinite(quaternion.Y, "Y");
            IsFinite(quaternion.Z, "Z");
            IsFinite(quaternion.W, "W");
        }

        public static void IsFinite(Plane plane)
        {
            IsFinite(plane.Normal.X, "Normal.X");
            IsFinite(plane.Normal.Y, "Normal.Y");
            IsFinite(plane.Normal.Z, "Normal.Z");
            IsFinite(plane.D, "D");
        }

        public static void IsFinite(Matrix3x2 matrix)
        {
            IsFinite(matrix.M11, "M11");
            IsFinite(matrix.M12, "M12");            

            IsFinite(matrix.M21, "M21");
            IsFinite(matrix.M22, "M22");
            
            IsFinite(matrix.M31, "M31");
            IsFinite(matrix.M32, "M32");
        }

        public static void IsFinite(Matrix4x4 matrix)
        {
            IsFinite(matrix.M11, "M11");
            IsFinite(matrix.M12, "M12");
            IsFinite(matrix.M13, "M13");
            IsFinite(matrix.M14, "M14");

            IsFinite(matrix.M21, "M21");
            IsFinite(matrix.M22, "M22");
            IsFinite(matrix.M23, "M23");
            IsFinite(matrix.M24, "M24");

            IsFinite(matrix.M31, "M31");
            IsFinite(matrix.M32, "M32");
            IsFinite(matrix.M33, "M33");
            IsFinite(matrix.M34, "M34");

            IsFinite(matrix.M41, "M41");
            IsFinite(matrix.M42, "M42");
            IsFinite(matrix.M43, "M43");
            IsFinite(matrix.M44, "M44");
        }

        public static void AreEqual(BigInteger expected, BigInteger actual, double tolerance = 0)
        {
            Assert.That((double)BigInteger.Abs(actual - expected), Is.EqualTo(0).Within(tolerance));
        }
        
        public static void AreEqual(Vector2 expected, Vector2 actual, double tolerance = 0)
        {
            Assert.Multiple(() =>
            {
                Assert.That(actual.X, Is.EqualTo(expected.X).Within(tolerance), "X");
                Assert.That(actual.Y, Is.EqualTo(expected.Y).Within(tolerance), "Y");
            });
        }

        public static float AreEqual(Vector3 expected, Vector3 actual, double tolerance = 0)
        {
            Assert.Multiple(() =>
            {
                Assert.That(actual.X, Is.EqualTo(expected.X).Within(tolerance), "X");
                Assert.That(actual.Y, Is.EqualTo(expected.Y).Within(tolerance), "Y");
                Assert.That(actual.Z, Is.EqualTo(expected.Z).Within(tolerance), "Z");
            });

            // get tolerance
            var tx = Math.Abs(expected.X - actual.X);
            var ty = Math.Abs(expected.Y - actual.Y);
            var tz = Math.Abs(expected.Z - actual.Z);
            return Math.Max(tx, Math.Max(ty, tz));
        }

        public static void AreEqual(Vector4 expected, Vector4 actual, double tolerance = 0)
        {
            Assert.Multiple(() =>
            {
                Assert.That(actual.X, Is.EqualTo(expected.X).Within(tolerance), "X");
                Assert.That(actual.Y, Is.EqualTo(expected.Y).Within(tolerance), "Y");
                Assert.That(actual.Z, Is.EqualTo(expected.Z).Within(tolerance), "Z");
                Assert.That(actual.W, Is.EqualTo(expected.W).Within(tolerance), "W");
            });
        }

        public static void AreEqual(Quaternion expected, Quaternion actual, double tolerance = 0)
        {
            Assert.Multiple(() =>
            {
                Assert.That(actual.X, Is.EqualTo(expected.X).Within(tolerance), "X");
                Assert.That(actual.Y, Is.EqualTo(expected.Y).Within(tolerance), "Y");
                Assert.That(actual.Z, Is.EqualTo(expected.Z).Within(tolerance), "Z");
                Assert.That(actual.W, Is.EqualTo(expected.W).Within(tolerance), "W");
            });
        }

        public static void AreEqual(Matrix4x4 expected, Matrix4x4 actual, double tolerance = 0)
        {
            Assert.Multiple(() =>
            {
                Assert.That(actual.M11, Is.EqualTo(expected.M11).Within(tolerance), "M11");
                Assert.That(actual.M12, Is.EqualTo(expected.M12).Within(tolerance), "M12");
                Assert.That(actual.M13, Is.EqualTo(expected.M13).Within(tolerance), "M13");
                Assert.That(actual.M14, Is.EqualTo(expected.M14).Within(tolerance), "M14");

                Assert.That(actual.M21, Is.EqualTo(expected.M21).Within(tolerance), "M21");
                Assert.That(actual.M22, Is.EqualTo(expected.M22).Within(tolerance), "M22");
                Assert.That(actual.M23, Is.EqualTo(expected.M23).Within(tolerance), "M23");
                Assert.That(actual.M24, Is.EqualTo(expected.M24).Within(tolerance), "M24");

                Assert.That(actual.M31, Is.EqualTo(expected.M31).Within(tolerance), "M31");
                Assert.That(actual.M32, Is.EqualTo(expected.M32).Within(tolerance), "M32");
                Assert.That(actual.M33, Is.EqualTo(expected.M33).Within(tolerance), "M33");
                Assert.That(actual.M34, Is.EqualTo(expected.M34).Within(tolerance), "M34");

                Assert.That(actual.M41, Is.EqualTo(expected.M41).Within(tolerance), "M41");
                Assert.That(actual.M42, Is.EqualTo(expected.M42).Within(tolerance), "M42");
                Assert.That(actual.M43, Is.EqualTo(expected.M43).Within(tolerance), "M43");
                Assert.That(actual.M44, Is.EqualTo(expected.M44).Within(tolerance), "M44");
            });
        }

        public static float AreGeometryicallyEquivalent(Matrix4x4 expected, Matrix4x4 actual, double tolerance = 0)
        {
            var expectedX = Vector3.Transform(Vector3.UnitX, expected);
            var expectedY = Vector3.Transform(Vector3.UnitY, expected);
            var expectedZ = Vector3.Transform(Vector3.UnitZ, expected);

            var actualX = Vector3.Transform(Vector3.UnitX, actual);
            var actualY = Vector3.Transform(Vector3.UnitY, actual);
            var actualZ = Vector3.Transform(Vector3.UnitZ, actual);

            var tx = AreEqual(expectedX, actualX, tolerance);
            var ty = AreEqual(expectedY, actualY, tolerance);
            var tz = AreEqual(expectedZ, actualZ, tolerance);
            return Math.Max(tx, Math.Max(ty, tz));
        }

        public static void IsInvertible(Matrix3x2 matrix)
        {
            IsFinite(matrix);
            Assert.That(Matrix3x2.Invert(matrix, out Matrix3x2 inverted), Is.True);
        }

        public static void IsInvertible(Matrix4x4 matrix)
        {
            IsFinite(matrix);
            Assert.That(Matrix4x4.Invert(matrix, out Matrix4x4 inverted), Is.True);
        }

        public static void IsOrthogonal3x3(Matrix4x4 matrix, double tolerance = 0)
        {
            IsFinite(matrix);

            Assert.Multiple(() =>
            {
                Assert.That(matrix.M41, Is.EqualTo(0));
                Assert.That(matrix.M42, Is.EqualTo(0));
                Assert.That(matrix.M43, Is.EqualTo(0));
                Assert.That(matrix.M44, Is.EqualTo(1));
            });

            var cx = new Vector3(matrix.M11, matrix.M21, matrix.M31);
            var cy = new Vector3(matrix.M12, matrix.M22, matrix.M32);
            var cz = new Vector3(matrix.M13, matrix.M23, matrix.M33);

            Assert.Multiple(() =>
            {
                Assert.That(Vector3.Dot(cx, cy), Is.EqualTo(0).Within(tolerance));
                Assert.That(Vector3.Dot(cx, cz), Is.EqualTo(0).Within(tolerance));
                Assert.That(Vector3.Dot(cy, cz), Is.EqualTo(0).Within(tolerance));
            });
        }

        public static void Length(Vector2 actual, double length, double tolerance = 0)
        {
            IsFinite(actual);

            length = Math.Abs(actual.Length() - length);

            Assert.That(length, Is.EqualTo(0).Within(tolerance));
        }

        public static void Length(Vector3 actual, double length, double tolerance = 0)
        {
            IsFinite(actual);

            length = Math.Abs(actual.Length() - length);

            Assert.That(length, Is.EqualTo(0).Within(tolerance));
        }

        public static void Length(Vector4 actual, double length, double tolerance = 0)
        {
            IsFinite(actual);

            length = Math.Abs(actual.Length() - length);

            Assert.That(length, Is.EqualTo(0).Within(tolerance));
        }

        public static void Length(Quaternion actual, double length, double tolerance = 0)
        {
            IsFinite(actual);

            length = Math.Abs(actual.Length() - length);

            Assert.That(length, Is.EqualTo(0).Within(tolerance));
        }

        public static void IsNormalized(Vector2 actual, double tolerance = 0)
        {
            Length(actual, 1, tolerance);
        }

        public static void IsNormalized(Vector3 actual, double tolerance = 0)
        {
            Length(actual, 1, tolerance);
        }

        public static void IsNormalized(Vector4 actual, double tolerance = 0)
        {
            Length(actual, 1, tolerance);
        }

        public static void IsNormalized(Quaternion actual, double tolerance = 0)
        {
            Length(actual, 1, tolerance);
        }
        
        public static void InRange(BigInteger value, BigInteger min, BigInteger max)
        {
            GreaterOrEqual(value, min);
            LessOrEqual(value, max);
        }

        public static void InRange(Vector2 value, Vector2 min, Vector2 max)
        {
            GreaterOrEqual(value, min);
            LessOrEqual(value, max);
        }

        public static void InRange(Vector3 value, Vector3 min, Vector3 max)
        {
            GreaterOrEqual(value, min);
            LessOrEqual(value, max);
        }

        public static void InRange(Vector4 value, Vector4 min, Vector4 max)
        {
            GreaterOrEqual(value, min);
            LessOrEqual(value, max);
        }

        public static void Less(BigInteger arg1, BigInteger arg2)
        {
            Assert.That(arg1.CompareTo(arg2), Is.LessThan(0));
        }

        public static void Less(Vector2 arg1, Vector2 arg2)
        {
            Assert.Multiple(() =>
            {
                Assert.That(arg1.X, Is.LessThan(arg2.X), "X");
                Assert.That(arg1.Y, Is.LessThan(arg2.Y), "Y");
            });
        }

        public static void Less(Vector3 arg1, Vector3 arg2)
        {
            Assert.Multiple(() =>
            {
                Assert.That(arg1.X, Is.LessThan(arg2.X), "X");
                Assert.That(arg1.Y, Is.LessThan(arg2.Y), "Y");
                Assert.That(arg1.Z, Is.LessThan(arg2.Z), "Z");
            });
        }

        public static void Less(Vector4 arg1, Vector4 arg2)
        {
            Assert.Multiple(() =>
            {
                Assert.That(arg1.X, Is.LessThan(arg2.X), "X");
                Assert.That(arg1.Y, Is.LessThan(arg2.Y), "Y");
                Assert.That(arg1.Z, Is.LessThan(arg2.Z), "Z");
                Assert.That(arg1.W, Is.LessThan(arg2.W), "W");
            });
        }

        public static void LessOrEqual(BigInteger arg1, BigInteger arg2)
        {
            Assert.That(arg1.CompareTo(arg2), Is.LessThanOrEqualTo(0));
        }

        public static void LessOrEqual(Vector2 arg1, Vector2 arg2)
        {
            Assert.Multiple(() =>
            {
                Assert.That(arg1.X, Is.LessThanOrEqualTo(arg2.X), "X");
                Assert.That(arg1.Y, Is.LessThanOrEqualTo(arg2.Y), "Y");
            });
        }

        public static void LessOrEqual(Vector3 arg1, Vector3 arg2)
        {
            Assert.Multiple(() =>
            {
                Assert.That(arg1.X, Is.LessThanOrEqualTo(arg2.X), "X");
                Assert.That(arg1.Y, Is.LessThanOrEqualTo(arg2.Y), "Y");
                Assert.That(arg1.Z, Is.LessThanOrEqualTo(arg2.Z), "Z");
            });
        }

        public static void LessOrEqual(Vector4 arg1, Vector4 arg2)
        {
            Assert.Multiple(() =>
            {
                Assert.That(arg1.X, Is.LessThanOrEqualTo(arg2.X), "X");
                Assert.That(arg1.Y, Is.LessThanOrEqualTo(arg2.Y), "Y");
                Assert.That(arg1.Z, Is.LessThanOrEqualTo(arg2.Z), "Z");
                Assert.That(arg1.W, Is.LessThanOrEqualTo(arg2.W), "W");
            });
        }

        public static void Greater(BigInteger arg1, BigInteger arg2)
        {
            Assert.That(arg1.CompareTo(arg2), Is.GreaterThan(0));
        }

        public static void Greater(Vector2 arg1, Vector2 arg2)
        {
            Assert.That(arg1.X, Is.GreaterThan(arg2.X), "X");
            Assert.That(arg1.Y, Is.GreaterThan(arg2.Y), "Y");
        }

        public static void Greater(Vector3 arg1, Vector3 arg2)
        {
            Assert.Multiple(() =>
            {
                Assert.That(arg1.X, Is.GreaterThan(arg2.X), "X");
                Assert.That(arg1.Y, Is.GreaterThan(arg2.Y), "Y");
                Assert.That(arg1.Z, Is.GreaterThan(arg2.Z), "Z");
            });
        }

        public static void Greater(Vector4 arg1, Vector4 arg2)
        {
            Assert.Multiple(() =>
            {
                Assert.That(arg1.X, Is.GreaterThan(arg2.X), "X");
                Assert.That(arg1.Y, Is.GreaterThan(arg2.Y), "Y");
                Assert.That(arg1.Z, Is.GreaterThan(arg2.Z), "Z");
                Assert.That(arg1.W, Is.GreaterThan(arg2.W), "W");
            });
        }

        public static void GreaterOrEqual(BigInteger arg1, BigInteger arg2)
        {
            Assert.That(arg1.CompareTo(arg2), Is.GreaterThanOrEqualTo(0));
        }

        public static void GreaterOrEqual(Vector2 arg1, Vector2 arg2)
        {
            Assert.Multiple(() =>
            {
                Assert.That(arg1.X, Is.GreaterThanOrEqualTo(arg2.X), "X");
                Assert.That(arg1.Y, Is.GreaterThanOrEqualTo(arg2.Y), "Y");
            });
        }

        public static void GreaterOrEqual(Vector3 arg1, Vector3 arg2)
        {
            Assert.Multiple(() =>
            {
                Assert.That(arg1.X, Is.GreaterThanOrEqualTo(arg2.X), "X");
                Assert.That(arg1.Y, Is.GreaterThanOrEqualTo(arg2.Y), "Y");
                Assert.That(arg1.Z, Is.GreaterThanOrEqualTo(arg2.Z), "Z");
            });
        }

        public static void GreaterOrEqual(Vector4 arg1, Vector4 arg2)
        {
            Assert.Multiple(() =>
            {
                Assert.That(arg1.X, Is.GreaterThanOrEqualTo(arg2.X), "X");
                Assert.That(arg1.Y, Is.GreaterThanOrEqualTo(arg2.Y), "Y");
                Assert.That(arg1.Z, Is.GreaterThanOrEqualTo(arg2.Z), "Z");
                Assert.That(arg1.W, Is.GreaterThanOrEqualTo(arg2.W), "W");
            });
        }

        public static void AngleLessOrEqual(Vector2 a, Vector2 b, double radians)
        {
            var angle = (a, b).GetAngle();

            Assert.That(angle, Is.LessThanOrEqualTo(radians), "Angle");
        }

        public static void AngleLessOrEqual(Vector3 a, Vector3 b, double radians)
        {
            var angle = (a, b).GetAngle();

            Assert.That(angle, Is.LessThanOrEqualTo(radians), "Angle");
        }

        public static void AngleLessOrEqual(Quaternion a, Quaternion b, double radians)
        {
            var angle = (a, b).GetAngle();

            Assert.That(angle, Is.LessThanOrEqualTo(radians), "Angle");
        }
    }
}