using System;

namespace Oni
{
    internal struct Matrix : IEquatable<Matrix>
    {
        public float M11, M12, M13, M14;
        public float M21, M22, M23, M24;
        public float M31, M32, M33, M34;
        public float M41, M42, M43, M44;

        public Matrix(float m11, float m12, float m13, float m14,
            float m21, float m22, float m23, float m24,
            float m31, float m32, float m33, float m34,
            float m41, float m42, float m43, float m44)
        {
            M11 = m11; M12 = m12; M13 = m13; M14 = m14;
            M21 = m21; M22 = m22; M23 = m23; M24 = m24;
            M31 = m31; M32 = m32; M33 = m33; M34 = m34;
            M41 = m41; M42 = m42; M43 = m43; M44 = m44;
        }

        public Matrix(float[] values)
        {
            M11 = values[0]; M12 = values[4]; M13 = values[8]; M14 = values[12];
            M21 = values[1]; M22 = values[5]; M23 = values[9]; M24 = values[13];
            M31 = values[2]; M32 = values[6]; M33 = values[10]; M34 = values[14];
            M41 = values[3]; M42 = values[7]; M43 = values[11]; M44 = values[15];
        }

        public void CopyTo(float[] values)
        {
            values[0] = M11;
            values[1] = M21;
            values[2] = M31;
            values[3] = M41;

            values[4] = M12;
            values[5] = M22;
            values[6] = M32;
            values[7] = M42;

            values[8] = M13;
            values[9] = M23;
            values[10] = M33;
            values[11] = M43;

            values[12] = M14;
            values[13] = M24;
            values[14] = M34;
            values[15] = M44;
        }

        public static Matrix CreateTranslation(float x, float y, float z)
        {
            Matrix r = Identity;

            r.M41 = x;
            r.M42 = y;
            r.M43 = z;

            return r;
        }

        public static Matrix CreateTranslation(Vector3 v) => CreateTranslation(v.X, v.Y, v.Z);

        public static Matrix CreateScale(float sx, float sy, float sz)
        {
            Matrix r = Identity;

            r.M11 = sx;
            r.M22 = sy;
            r.M33 = sz;

            return r;
        }

        public static Matrix CreateScale(float s) => CreateScale(s, s, s);
        public static Matrix CreateScale(Vector3 s) => CreateScale(s.X, s.Y, s.Z);

        public static Matrix CreateRotationX(float angle)
        {
            float cos = FMath.Cos(angle);
            float sin = FMath.Sin(angle);

            Matrix r = Identity;
            r.M22 = cos;
            r.M23 = sin;
            r.M32 = -sin;
            r.M33 = cos;
            return r;
        }

        public static Matrix CreateRotationY(float angle)
        {
            float cos = FMath.Cos(angle);
            float sin = FMath.Sin(angle);

            Matrix r = Identity;
            r.M11 = cos;
            r.M13 = -sin;
            r.M31 = sin;
            r.M33 = cos;
            return r;
        }

        public static Matrix CreateRotationZ(float angle)
        {
            float cos = FMath.Cos(angle);
            float sin = FMath.Sin(angle);

            Matrix r = Identity;
            r.M11 = cos;
            r.M12 = sin;
            r.M21 = -sin;
            r.M22 = cos;
            return r;
        }

        public static Matrix CreateFromAxisAngle(Vector3 axis, float angle)
        {
            float sin = FMath.Sin(angle);
            float cos = FMath.Cos(angle);

            float x = axis.X;
            float y = axis.Y;
            float z = axis.Z;
            float xx = x * x;
            float yy = y * y;
            float zz = z * z;
            float xy = x * y;
            float xz = x * z;
            float yz = y * z;

            Matrix r = Identity;
            r.M11 = xx + (cos * (1.0f - xx));
            r.M12 = (xy - (cos * xy)) + (sin * z);
            r.M13 = (xz - (cos * xz)) - (sin * y);
            r.M21 = (xy - (cos * xy)) - (sin * z);
            r.M22 = yy + (cos * (1.0f - yy));
            r.M23 = (yz - (cos * yz)) + (sin * x);
            r.M31 = (xz - (cos * xz)) + (sin * y);
            r.M32 = (yz - (cos * yz)) - (sin * x);
            r.M33 = zz + (cos * (1.0f - zz));
            return r;
        }

        public static Matrix CreateFromQuaternion(Quaternion q)
        {
            float xx = q.X * q.X;
            float yy = q.Y * q.Y;
            float zz = q.Z * q.Z;
            float xy = q.X * q.Y;
            float zw = q.Z * q.W;
            float zx = q.Z * q.X;
            float yw = q.Y * q.W;
            float yz = q.Y * q.Z;
            float xw = q.X * q.W;

            Matrix r = Identity;

            r.M11 = 1.0f - 2.0f * (yy + zz);
            r.M12 = 2.0f * (xy + zw);
            r.M13 = 2.0f * (zx - yw);

            r.M21 = 2.0f * (xy - zw);
            r.M22 = 1.0f - 2.0f * (zz + xx);
            r.M23 = 2.0f * (yz + xw);

            r.M31 = 2.0f * (zx + yw);
            r.M32 = 2.0f * (yz - xw);
            r.M33 = 1.0f - 2.0f * (yy + xx);

            return r;
        }

        public static Matrix operator +(Matrix m1, Matrix m2)
        {
            m1.M11 += m2.M11;
            m1.M12 += m2.M12;
            m1.M13 += m2.M13;
            m1.M14 += m2.M14;
            m1.M21 += m2.M21;
            m1.M22 += m2.M22;
            m1.M23 += m2.M23;
            m1.M24 += m2.M24;
            m1.M31 += m2.M31;
            m1.M32 += m2.M32;
            m1.M33 += m2.M33;
            m1.M34 += m2.M34;
            m1.M41 += m2.M41;
            m1.M42 += m2.M42;
            m1.M43 += m2.M43;
            m1.M44 += m2.M44;

            return m1;
        }

        public static Matrix operator -(Matrix m1, Matrix m2)
        {
            m1.M11 -= m2.M11;
            m1.M12 -= m2.M12;
            m1.M13 -= m2.M13;
            m1.M14 -= m2.M14;
            m1.M21 -= m2.M21;
            m1.M22 -= m2.M22;
            m1.M23 -= m2.M23;
            m1.M24 -= m2.M24;
            m1.M31 -= m2.M31;
            m1.M32 -= m2.M32;
            m1.M33 -= m2.M33;
            m1.M34 -= m2.M34;
            m1.M41 -= m2.M41;
            m1.M42 -= m2.M42;
            m1.M43 -= m2.M43;
            m1.M44 -= m2.M44;

            return m1;
        }

        public static Matrix operator *(Matrix m, float s)
        {
            m.M11 *= s;
            m.M12 *= s;
            m.M13 *= s;
            m.M14 *= s;
            m.M21 *= s;
            m.M22 *= s;
            m.M23 *= s;
            m.M24 *= s;
            m.M31 *= s;
            m.M32 *= s;
            m.M33 *= s;
            m.M34 *= s;
            m.M41 *= s;
            m.M42 *= s;
            m.M43 *= s;
            m.M44 *= s;

            return m;
        }

        public static Matrix operator *(float s, Matrix m) => m * s;

        public static Matrix operator /(Matrix m, float s) => m * (1.0f / s);

        public static Matrix operator *(Matrix m1, Matrix m2)
        {
            Matrix r;

            r.M11 = m1.M11 * m2.M11 + m1.M12 * m2.M21 + m1.M13 * m2.M31 + m1.M14 * m2.M41;
            r.M12 = m1.M11 * m2.M12 + m1.M12 * m2.M22 + m1.M13 * m2.M32 + m1.M14 * m2.M42;
            r.M13 = m1.M11 * m2.M13 + m1.M12 * m2.M23 + m1.M13 * m2.M33 + m1.M14 * m2.M43;
            r.M14 = m1.M11 * m2.M14 + m1.M12 * m2.M24 + m1.M13 * m2.M34 + m1.M14 * m2.M44;
            r.M21 = m1.M21 * m2.M11 + m1.M22 * m2.M21 + m1.M23 * m2.M31 + m1.M24 * m2.M41;
            r.M22 = m1.M21 * m2.M12 + m1.M22 * m2.M22 + m1.M23 * m2.M32 + m1.M24 * m2.M42;
            r.M23 = m1.M21 * m2.M13 + m1.M22 * m2.M23 + m1.M23 * m2.M33 + m1.M24 * m2.M43;
            r.M24 = m1.M21 * m2.M14 + m1.M22 * m2.M24 + m1.M23 * m2.M34 + m1.M24 * m2.M44;
            r.M31 = m1.M31 * m2.M11 + m1.M32 * m2.M21 + m1.M33 * m2.M31 + m1.M34 * m2.M41;
            r.M32 = m1.M31 * m2.M12 + m1.M32 * m2.M22 + m1.M33 * m2.M32 + m1.M34 * m2.M42;
            r.M33 = m1.M31 * m2.M13 + m1.M32 * m2.M23 + m1.M33 * m2.M33 + m1.M34 * m2.M43;
            r.M34 = m1.M31 * m2.M14 + m1.M32 * m2.M24 + m1.M33 * m2.M34 + m1.M34 * m2.M44;
            r.M41 = m1.M41 * m2.M11 + m1.M42 * m2.M21 + m1.M43 * m2.M31 + m1.M44 * m2.M41;
            r.M42 = m1.M41 * m2.M12 + m1.M42 * m2.M22 + m1.M43 * m2.M32 + m1.M44 * m2.M42;
            r.M43 = m1.M41 * m2.M13 + m1.M42 * m2.M23 + m1.M43 * m2.M33 + m1.M44 * m2.M43;
            r.M44 = m1.M41 * m2.M14 + m1.M42 * m2.M24 + m1.M43 * m2.M34 + m1.M44 * m2.M44;

            return r;
        }

        public Matrix Transpose()
        {
            Matrix t;

            t.M11 = M11;
            t.M12 = M21;
            t.M13 = M31;
            t.M14 = M41;
            t.M21 = M12;
            t.M22 = M22;
            t.M23 = M32;
            t.M24 = M42;
            t.M31 = M13;
            t.M32 = M23;
            t.M33 = M33;
            t.M34 = M43;
            t.M41 = M14;
            t.M42 = M24;
            t.M43 = M34;
            t.M44 = M44;

            return t;
        }

        public static bool operator ==(Matrix m1, Matrix m2) => m1.Equals(m2);
        public static bool operator !=(Matrix m1, Matrix m2) => !m1.Equals(m2);

        public Vector3 XAxis
        {
            get
            {
                return new Vector3(M11, M12, M13);
            }
            set
            {
                M11 = value.X;
                M12 = value.Y;
                M13 = value.Z;
            }
        }

        public Vector3 YAxis
        {
            get
            {
                return new Vector3(M21, M22, M23);
            }
            set
            {
                M21 = value.X;
                M22 = value.Y;
                M23 = value.Z;
            }
        }

        public Vector3 ZAxis
        {
            get
            {
                return new Vector3(M31, M32, M33);
            }
            set
            {
                M31 = value.X;
                M32 = value.Y;
                M33 = value.Z;
            }
        }

        public Vector3 Scale
        {
            get
            {
                return new Vector3(M11, M22, M33);
            }
            set
            {
                M11 = value.X;
                M22 = value.Y;
                M33 = value.Z;
            }
        }

        public Vector3 Translation
        {
            get
            {
                return new Vector3(M41, M42, M43);
            }
            set
            {
                M41 = value.X;
                M42 = value.Y;
                M43 = value.Z;
            }
        }

        public bool Equals(Matrix other)
        {
            return (M11 == other.M11 && M12 == other.M12 && M13 == other.M13 && M14 == other.M14
                && M21 == other.M21 && M22 == other.M22 && M23 == other.M23 && M24 == other.M24
                && M31 == other.M31 && M32 == other.M32 && M33 == other.M33 && M34 == other.M34
                && M41 == other.M41 && M42 == other.M42 && M43 == other.M43 && M44 == other.M44);
        }

        public override bool Equals(object obj) => obj is Matrix && Equals((Matrix)obj);

        public override int GetHashCode()
        {
            return M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode()
                 ^ M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode()
                 ^ M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode()
                 ^ M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode();
        }

        public override string ToString()
        {
            return string.Format("{{M11:{0} M12:{1} M13:{2} M14:{3}}}\n{{M21:{4} M22:{5} M23:{6} M24:{7}}}\n{{M31:{8} M32:{9} M33:{10} M34:{11}}}\n{{M41:{12} M42:{13} M43:{14} M44:{15}}}",
                M11, M12, M13, M14,
                M21, M22, M23, M24,
                M31, M32, M33, M34,
                M41, M42, M43, M44);
        }

        private static readonly Matrix identity = new Matrix(
            1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 1.0f, 0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f,
            0.0f, 0.0f, 0.0f, 1.0f);

        public static Matrix Identity => identity;

        public Vector3 ToEuler()
        {
            float a = M11;
            float b = M21;
            float c, s, r;

            if (b == 0.0f)
            {
                c = FMath.Sign(a);
                s = 0.0f;
                r = Math.Abs(a);
            }
            else if (a == 0.0f)
            {
                c = 0.0f;
                s = FMath.Sign(b);
                r = Math.Abs(b);
            }
            else if (Math.Abs(b) > Math.Abs(a))
            {
                float t = a / b;
                float u = FMath.Sign(b) * FMath.Sqrt(1.0f + t * t);
                s = 1.0f / u;
                c = s * t;
                r = b * u;
            }
            else
            {
                float t = b / a;
                float u = FMath.Sign(a) * FMath.Sqrt(1.0f + t * t);
                c = 1.0f / u;
                s = c * t;
                r = a * u;
            }

            Vector3 e;
            e.Z = MathHelper.ToDegrees(-FMath.Atan2(s, c));
            e.Y = MathHelper.ToDegrees(FMath.Atan2(M31, r));
            e.X = MathHelper.ToDegrees(-FMath.Atan2(M32, M33));
            return e;
        }

        public float Determinant()
        {
            var m11 = M11;
            var m12 = M12;
            var m13 = M13;
            var m14 = M14;
            var m21 = M21;
            var m22 = M22;
            var m23 = M23;
            var m24 = M24;
            var m31 = M31;
            var m32 = M32;
            var m33 = M33;
            var m34 = M34;
            var m41 = M41;
            var m42 = M42;
            var m43 = M43;
            var m44 = M44;

            var d3434 = m33 * m44 - m34 * m43;
            var d3424 = m32 * m44 - m34 * m42;
            var d3423 = m32 * m43 - m33 * m42;
            var d3414 = m31 * m44 - m34 * m41;
            var d3413 = m31 * m43 - m33 * m41;
            var d3412 = m31 * m42 - m32 * m41;

            return m11 * (m22 * d3434 - m23 * d3424 + m24 * d3423)
                 - m12 * (m21 * d3434 - m23 * d3414 + m24 * d3413)
                 + m13 * (m21 * d3424 - m22 * d3414 + m24 * d3412)
                 - m14 * (m21 * d3423 - m22 * d3413 + m23 * d3412);
        }

        //Matrix m = Matrix.Identity;
        //m *= Matrix.CreateScale(3.3f, 1.3f, 7.6f);
        //m *= Matrix.CreateTranslation(2.3f, 4.5f, 6.7f);
        //m *= Matrix.CreateRotationY(1.2f);
        //m *= Matrix.CreateTranslation(2.3f, 4.5f, 6.7f);
        //m *= Matrix.CreateRotationY(1.2f);
        //m *= Matrix.CreateTranslation(2.3f, 4.5f, 6.7f);
        //m *= Matrix.CreateRotationY(1.2f);

        //Vector3 s, t;
        //Quaternion r;
        //m.Decompose(out s, out r, out t);
        //Matrix m2 = Matrix.CreateScale(s) * Matrix.CreateFromQuaternion(r) * Matrix.CreateTranslation(t);

        //Console.WriteLine(m2 - m);
        //return 0;

        //[StructLayout(LayoutKind.Sequential)]
        //private unsafe struct VectorBasis
        //{
        //    public Vector3* axis0;
        //    public Vector3* axis1;
        //    public Vector3* axis2;
        //}

        //[StructLayout(LayoutKind.Sequential)]
        //private struct CanonicalBasis
        //{
        //    public Vector3 axis0;
        //    public Vector3 axis1;
        //    public Vector3 axis2;
        //}

        //public unsafe bool Decompose(out Vector3 outScale, out Quaternion outRotation, out Vector3 outTranslation)
        //{
        //    outTranslation.X = M41;
        //    outTranslation.Y = M42;
        //    outTranslation.Z = M43;

        //    var rotation = new Matrix(
        //        M11, M12, M13, 0.0f, 
        //        M21, M22, M23, 0.0f, 
        //        M31, M32, M33, 0.0f, 
        //        0.0f, 0.0f, 0.0f, 1.0f);

        //    var vectorBasis = new VectorBasis {
        //        axis0 = (Vector3*)&rotation.M11,
        //        axis1 = (Vector3*)&rotation.M21,
        //        axis2 = (Vector3*)&rotation.M31
        //    };

        //    var canonicalBasis = new CanonicalBasis {
        //        axis0 = Vector3.UnitX,
        //        axis1 = Vector3.UnitY,
        //        axis2 = Vector3.UnitZ
        //    };

        //    var scale = new Vector3(
        //        vectorBasis.axis0->Length(),
        //        vectorBasis.axis1->Length(),
        //        vectorBasis.axis2->Length()
        //    );

        //    int xi, yi, zi;

        //    if (scale.X < scale.Y)
        //    {
        //        if (scale.Y < scale.Z)
        //        {
        //            xi = 2;
        //            yi = 1;
        //            zi = 0;
        //        }
        //        else
        //        {
        //            xi = 1;

        //            if (scale.X < scale.Z)
        //            {
        //                yi = 2;
        //                zi = 0;
        //            }
        //            else
        //            {
        //                yi = 0;
        //                zi = 2;
        //            }
        //        }
        //    }
        //    else
        //    {
        //        if (scale.X < scale.Z)
        //        {
        //            xi = 2;
        //            yi = 0;
        //            zi = 1;
        //        }
        //        else
        //        {
        //            xi = 0;

        //            if (scale.Y < scale.Z)
        //            {
        //                yi = 2;
        //                zi = 1;
        //            }
        //            else
        //            {
        //                yi = 1;
        //                zi = 2;
        //            }
        //        }
        //    }

        //    var pScale = &scale.X;

        //    var pvBasis = &vectorBasis.axis0;
        //    var pcBasis = &canonicalBasis.axis0;

        //    if (pScale[xi] < 0.0001f)
        //    {
        //        //
        //        // If the smallest scale is < 0.0001 then use the coresponding cannonical basis instead
        //        //

        //        pvBasis[xi] = &pcBasis[xi];
        //    }
        //    else
        //    {
        //        pvBasis[xi]->Normalize();
        //    }

        //    if (pScale[yi] < 0.0001f)
        //    {
        //        //
        //        // The second smallest scale is < 0.0001 too, build a perpendicular vector
        //        //

        //        float fx = Math.Abs(pvBasis[xi]->X);
        //        float fy = Math.Abs(pvBasis[xi]->Y);
        //        float fz = Math.Abs(pvBasis[xi]->Z);

        //        int yij;

        //        if (fx < fy)
        //        {
        //            if (fy < fz)
        //            {
        //                yij = 0;
        //            }
        //            else
        //            {
        //                if (fx < fz)
        //                    yij = 0;
        //                else
        //                    yij = 2;
        //            }
        //        }
        //        else
        //        {
        //            if (fx < fz)
        //            {
        //                yij = 1;
        //            }
        //            else
        //            {
        //                if (fy < fz)
        //                    yij = 1;
        //                else
        //                    yij = 2;
        //            }
        //        }

        //        pcBasis[yij] = Vector3.Cross(*pvBasis[yi], *pvBasis[xi]);
        //    }

        //    pvBasis[yi]->Normalize();

        //    if (pScale[zi] < 0.0001f)
        //        *(pvBasis[zi]) = Vector3.Cross(*pvBasis[yi], *pvBasis[xi]);
        //    else
        //        pvBasis[zi]->Normalize();

        //    float rotDet = rotation.Determinant();

        //    if (rotDet < 0.0f)
        //    {
        //        pScale[xi] = -pScale[xi];
        //        *(pvBasis[xi]) = -(*(pvBasis[xi]));
        //        rotDet = -rotDet;
        //    }

        //    outScale = scale;

        //    if (Math.Abs(rotDet - 1.0f) > 0.01f)
        //    {
        //        outRotation = Quaternion.Identity;
        //        return false;
        //    }
        //    else
        //    {
        //        outRotation = Quaternion.CreateFromRotationMatrix(rotation);
        //        return true;
        //    }
        //}
    }
}