
// Only uncomment one of the following:
//#define M_HOLOLENS_CODE_WITH_OR_WITHOUT_HEADSET
//#define M_VIVE_OR_DESKTOP // in this code, distinguish between desktop or headset with GC.isInDesktopMode

#if M_HOLOLENS_CODE_WITH_OR_WITHOUT_HEADSET
#if UNITY_EDITOR // for code running within the unity editor
#define M_HOLOLENS_CODE_WITHOUT_HEADSET // hololens codebase, but running in unity player, without headset (e.g. for debugging network stuff)
#elif WINDOWS_UWP // for code running on the Hololens headset
#define M_HOLOLENS_CODE_WITH_HEADSET
#endif
#endif

#if UNITY_EDITOR || WINDOWS_UWP
#define M_UNITY_API
#endif


using System;

#if M_UNITY_API
using UnityEngine;
#endif

// Stores a 4x4 matrix as a 16-element array M in column-major form,
// where M[0], M[4], M[8], M[12] is the first row of the matrix,
// and M[0], M[1], M[2], M[3] is the first column.
// The item at row r and column c has index (r + c*4)

public class MMatrix4 {
    public float[] m = new float[16];

    public MMatrix4() { setToIdentity(); }

    public MMatrix4(
        double m0,  double m1,  double m2,  double m3,
        double m4,  double m5,  double m6,  double m7,
        double m8,  double m9,  double m10, double m11,
        double m12, double m13, double m14, double m15
    ) {
        m[ 0] = (float)m0;
        m[ 1] = (float)m1;
        m[ 2] = (float)m2;
        m[ 3] = (float)m3;
        m[ 4] = (float)m4;
        m[ 5] = (float)m5;
        m[ 6] = (float)m6;
        m[ 7] = (float)m7;
        m[ 8] = (float)m8;
        m[ 9] = (float)m9;
        m[10] = (float)m10;
        m[11] = (float)m11;
        m[12] = (float)m12;
        m[13] = (float)m13;
        m[14] = (float)m14;
        m[15] = (float)m15;
    }

    public void copy(MMatrix4 other) {
        for ( int i = 0; i < 16; ++i )
            m[i] = other.m[i];
    }

    public void setToIdentity() {
        m[ 0] = 1; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
        m[ 1] = 0; m[ 5] = 1; m[ 9] = 0; m[13] = 0;
        m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = 0;
        m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
    }
    public void setToTranslation( MVector3 v ) {
        m[ 0] = 1; m[ 4] = 0; m[ 8] = 0; m[12] = v.x;
        m[ 1] = 0; m[ 5] = 1; m[ 9] = 0; m[13] = v.y;
        m[ 2] = 0; m[ 6] = 0; m[10] = 1; m[14] = v.z;
        m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
    }
    public void setToScale( float sx, float sy, float sz ) {
        m[ 0] = sx; m[ 4] = 0;  m[ 8] = 0;  m[12] = 0;
        m[ 1] = 0;  m[ 5] = sy; m[ 9] = 0;  m[13] = 0;
        m[ 2] = 0;  m[ 6] = 0;  m[10] = sz; m[14] = 0;
        m[ 3] = 0;  m[ 7] = 0;  m[11] = 0;  m[15] = 1;
    }
    public void setToUniformScale( float s ) {
        m[ 0] = s; m[ 4] = 0; m[ 8] = 0; m[12] = 0;
        m[ 1] = 0; m[ 5] = s; m[ 9] = 0; m[13] = 0;
        m[ 2] = 0; m[ 6] = 0; m[10] = s; m[14] = 0;
        m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
    }

    public void setToRotation( float angle_in_radians, MVector3 v ) {
        // TODO_0 assert here that v is normalized
        float c = (float)Math.Cos( angle_in_radians );
        float s = (float)Math.Sin( angle_in_radians );
        float one_minus_c = 1 - c;
        m[ 0] = c + one_minus_c * v.x*v.x;
        m[ 5] = c + one_minus_c * v.y*v.y;
        m[10] = c + one_minus_c * v.z*v.z;
        m[ 1] = m[ 4] = one_minus_c * v.x*v.y;
        m[ 2] = m[ 8] = one_minus_c * v.x*v.z;
        m[ 6] = m[ 9] = one_minus_c * v.y*v.z;
        float xs = v.x * s;
        float ys = v.y * s;
        float zs = v.z * s;
        m[ 1] += zs;  m[ 4] -= zs;
        m[ 2] -= ys;  m[ 8] += ys;
        m[ 6] += xs;  m[ 9] -= xs;

        m[12] = 0;
        m[13] = 0;
        m[14] = 0;
        m[ 3] = 0; m[ 7] = 0; m[11] = 0; m[15] = 1;
    }

    public void setToRotationAroundPoint( MVector3 axis, float angle_in_radians, MVector3 center ) {
        MMatrix4 T = new MMatrix4();
        T.setToTranslation( center );
        MMatrix4 invT = new MMatrix4();
        invT.setToTranslation( - center );
        MMatrix4 R = new MMatrix4();
        R.setToRotation( angle_in_radians, axis );

        copy( T * R * invT );
    }


    public static MMatrix4 operator *(MMatrix4 a, MMatrix4 b) {
        MMatrix4 M = new MMatrix4();

        M.m[ 0] = a.m[ 0]*b.m[ 0] + a.m[ 4]*b.m[ 1] + a.m[ 8]*b.m[ 2] + a.m[12]*b.m[ 3];
        M.m[ 1] = a.m[ 1]*b.m[ 0] + a.m[ 5]*b.m[ 1] + a.m[ 9]*b.m[ 2] + a.m[13]*b.m[ 3];
        M.m[ 2] = a.m[ 2]*b.m[ 0] + a.m[ 6]*b.m[ 1] + a.m[10]*b.m[ 2] + a.m[14]*b.m[ 3];
        M.m[ 3] = a.m[ 3]*b.m[ 0] + a.m[ 7]*b.m[ 1] + a.m[11]*b.m[ 2] + a.m[15]*b.m[ 3];

        M.m[ 4] = a.m[ 0]*b.m[ 4] + a.m[ 4]*b.m[ 5] + a.m[ 8]*b.m[ 6] + a.m[12]*b.m[ 7];
        M.m[ 5] = a.m[ 1]*b.m[ 4] + a.m[ 5]*b.m[ 5] + a.m[ 9]*b.m[ 6] + a.m[13]*b.m[ 7];
        M.m[ 6] = a.m[ 2]*b.m[ 4] + a.m[ 6]*b.m[ 5] + a.m[10]*b.m[ 6] + a.m[14]*b.m[ 7];
        M.m[ 7] = a.m[ 3]*b.m[ 4] + a.m[ 7]*b.m[ 5] + a.m[11]*b.m[ 6] + a.m[15]*b.m[ 7];

        M.m[ 8] = a.m[ 0]*b.m[ 8] + a.m[ 4]*b.m[ 9] + a.m[ 8]*b.m[10] + a.m[12]*b.m[11];
        M.m[ 9] = a.m[ 1]*b.m[ 8] + a.m[ 5]*b.m[ 9] + a.m[ 9]*b.m[10] + a.m[13]*b.m[11];
        M.m[10] = a.m[ 2]*b.m[ 8] + a.m[ 6]*b.m[ 9] + a.m[10]*b.m[10] + a.m[14]*b.m[11];
        M.m[11] = a.m[ 3]*b.m[ 8] + a.m[ 7]*b.m[ 9] + a.m[11]*b.m[10] + a.m[15]*b.m[11];

        M.m[12] = a.m[ 0]*b.m[12] + a.m[ 4]*b.m[13] + a.m[ 8]*b.m[14] + a.m[12]*b.m[15];
        M.m[13] = a.m[ 1]*b.m[12] + a.m[ 5]*b.m[13] + a.m[ 9]*b.m[14] + a.m[13]*b.m[15];
        M.m[14] = a.m[ 2]*b.m[12] + a.m[ 6]*b.m[13] + a.m[10]*b.m[14] + a.m[14]*b.m[15];
        M.m[15] = a.m[ 3]*b.m[12] + a.m[ 7]*b.m[13] + a.m[11]*b.m[14] + a.m[15]*b.m[15];

        return M;
    }

    // transforms the given point by assuming that the point has an implicit homogeneous coordinate of 1
    public MVector3 multPoint( MVector3 b ) {
        float x = m[ 0]*b.x + m[ 4]*b.y + m[ 8]*b.z + m[12];
        float y = m[ 1]*b.x + m[ 5]*b.y + m[ 9]*b.z + m[13];
        float z = m[ 2]*b.x + m[ 6]*b.y + m[10]*b.z + m[14];
        float w = m[ 3]*b.x + m[ 7]*b.y + m[11]*b.z + m[15];
        return new MVector3(x/w,y/w,z/w);
    }
    // transforms the given vector by assuming that the vector has an implicit homogeneous coordinate of 0
    public MVector3 multVector( MVector3 b ) {
        float x = m[ 0]*b.x + m[ 4]*b.y + m[ 8]*b.z;
        float y = m[ 1]*b.x + m[ 5]*b.y + m[ 9]*b.z;
        float z = m[ 2]*b.x + m[ 6]*b.y + m[10]*b.z;
        return new MVector3(x,y,z);
    }




#if M_UNITY_API
    public MMatrix4(Matrix4x4 unityMatrix) {
        for ( int i = 0; i < 16; ++i )
            m[i] = unityMatrix[i];
    }
    public Matrix4x4 asUnityMatrix() {
        Matrix4x4 returnValue = new Matrix4x4();
        for ( int i = 0; i < 16; ++i )
            returnValue[i] = m[i];
        return returnValue;
    }
    public MMatrix4(Quaternion q) { // TODO_0 I have not thoroughly tested this, don't know if it works
        // MLogger.Log("quat is " + q.ToString());
        // Matrix4x4 unityMatrix = Matrix4x4.Rotate(q);
        Matrix4x4 unityMatrix = Matrix4x4.TRS(Vector3.zero, q, Vector3.one);
        // MLogger.Log("unity matrix is " + unityMatrix);
        for ( int i = 0; i < 16; ++i )
            m[i] = unityMatrix[i];
    }
#endif

    public string str() {
        string s = "";
        for ( int i = 0; i <= 14; ++i )
            s += string.Format("{0},",m[i]);
        s += string.Format("{0}",m[15]);
        return s;
    }
}
