#include "GLKMatrix.h"
#include <cmath>


GLKMatrix4 GLKMatrix4Multiply(GLKMatrix4 matrixLeft, GLKMatrix4 matrixRight)
{
    GLKMatrix4 m;
    for (int j = 0; j < 16; j++)
        m.m[j] = 0;
    for (int j = 0; j < 4; j++)
      for (int k = 0; k < 4; k++)
        for (int i = 0; i < 4; i++)
          m.m[j+k*4] += matrixLeft.m[j+i*4] * matrixRight.m[i+k*4];
    return m;
}


GLKMatrix4 GLKMatrix4MakePerspective(float fovyRadians, float aspect, float near, float far)
{
    float ctan = 1.0f / tanf(fovyRadians / 2.0f);
    float inlf = 1.0f / (near - far);
    GLKMatrix4 m = { ctan / aspect, 0.0f, 0.0f, 0.0f,
                     0.0f, ctan, 0.0f, 0.0f,
                     0.0f, 0.0f, (far + near) * inlf, -1.0f,
                     0.0f, 0.0f, (2.0f * far * near) * inlf, 0.0f };
    return m;
}


GLKMatrix4 GLKMatrix4MakeOrtho(float left, float right, float bottom, float top, float near, float far)
{
    float ral = right + left;
    float tab = top + bottom;
    float fan = far + near;
    float rsl = 1.0 / (right - left);
    float tsb = 1.0 / (top - bottom);
    float fsn = 1.0 / (far - near);
    GLKMatrix4 m = { 2.0f * rsl, 0.0f, 0.0f, 0.0f,
                     0.0f, 2.0f * tsb, 0.0f, 0.0f,
                     0.0f, 0.0f, -2.0f * fsn, 0.0f,
                     -ral * rsl, -tab * tsb, -fan * fsn, 1.0f };
    return m;
}


GLKMatrix4 GLKMatrix4MakeTranslation(float tx, float ty, float tz)
{
    GLKMatrix4 m = { 1.0, 0.0, 0.0, 0.0,   0.0, 1.0, 0.0, 0.0,   0.0, 0.0, 1.0, 0.0,   tx, ty, tz, 1.0  };
    return m;
}


GLKMatrix4 GLKMatrix4MakeRotation(float radians, float x, float y, float z)
{
    float len = 1.0 / sqrt(x*x + y*y + z*z);
    float v[3] = { x*len, y*len, z*len };
    float cos = cosf(radians);
    float cosp = 1.0f - cos;
    float sin = sinf(radians);
    GLKMatrix4 m = { cos + cosp * v[0] * v[0],
                     cosp * v[0] * v[1] + v[2] * sin,
                     cosp * v[0] * v[2] - v[1] * sin,
                     0.0f, cosp * v[0] * v[1] - v[2] * sin,
                     cos + cosp * v[1] * v[1],
                     cosp * v[1] * v[2] + v[0] * sin,
                     0.0f, cosp * v[0] * v[2] + v[1] * sin,
                     cosp * v[1] * v[2] - v[0] * sin,
                     cos + cosp * v[2] * v[2],
                     0.0f, 0.0f, 0.0f, 0.0f, 1.0f };
    return m;
}
   

GLKMatrix4 GLKMatrix4Rotate(GLKMatrix4 matrix, float radians, float x, float y, float z)
{
    GLKMatrix4 rm = GLKMatrix4MakeRotation(radians, x, y, z);
    return GLKMatrix4Multiply(matrix, rm);
}


GLKMatrix4 GLKMatrix4Scale(GLKMatrix4 matrix, float sx, float sy, float sz)
{
    GLKMatrix4 m = { matrix.m[0] * sx, matrix.m[1] * sx, matrix.m[2] * sx, matrix.m[3] * sx,
                     matrix.m[4] * sy, matrix.m[5] * sy, matrix.m[6] * sy, matrix.m[7] * sy,
                     matrix.m[8] * sz, matrix.m[9] * sz, matrix.m[10] * sz, matrix.m[11] * sz,
                     matrix.m[12], matrix.m[13], matrix.m[14], matrix.m[15] };
    return m;
}


GLKMatrix4 GLKMatrix4Translate(GLKMatrix4 matrix, float tx, float ty, float tz)
{
    GLKMatrix4 m = { matrix.m[0], matrix.m[1], matrix.m[2], matrix.m[3],
                     matrix.m[4], matrix.m[5], matrix.m[6], matrix.m[7],
                     matrix.m[8], matrix.m[9], matrix.m[10], matrix.m[11],
                     matrix.m[0] * tx + matrix.m[4] * ty + matrix.m[8] * tz + matrix.m[12],
                     matrix.m[1] * tx + matrix.m[5] * ty + matrix.m[9] * tz + matrix.m[13],
                     matrix.m[2] * tx + matrix.m[6] * ty + matrix.m[10] * tz + matrix.m[14],
                     matrix.m[3] * tx + matrix.m[7] * ty + matrix.m[11] * tz + matrix.m[15] };
    return m;
}


GLKMatrix4 GLKMatrix4Transpose(GLKMatrix4 matrix)
{
    GLKMatrix4 m = { matrix.m[15], matrix.m[14], matrix.m[13], matrix.m[12],
                     matrix.m[11], matrix.m[10], matrix.m[9],  matrix.m[8],
                     matrix.m[7],  matrix.m[6],  matrix.m[5],  matrix.m[4],
                     matrix.m[3],  matrix.m[2],  matrix.m[1],  matrix.m[0] };
    return m;
}


GLKMatrix3 GLKMakeNormalMatrix(GLKMatrix4 matrix)
{
#define A(x,y)  matrix.m[y*4+x]
    double determinant = A(0,0)*(A(1,1)*A(2,2)-A(2,1)*A(1,2))-A(0,1)*(A(1,0)*A(2,2)-A(1,2)*A(2,0))+A(0,2)*(A(1,0)*A(2,1)-A(1,1)*A(2,0));
    float invdet = float(1.0 / determinant);
    GLKMatrix3 m = { 
        (A(1,1)*A(2,2)-A(2,1)*A(1,2))*invdet,
       -(A(0,1)*A(2,2)-A(0,2)*A(2,1))*invdet,
        (A(0,1)*A(1,2)-A(0,2)*A(1,1))*invdet,
       -(A(1,0)*A(2,2)-A(1,2)*A(2,0))*invdet,
        (A(0,0)*A(2,2)-A(0,2)*A(2,0))*invdet,
       -(A(0,0)*A(1,2)-A(1,0)*A(0,2))*invdet,
        (A(1,0)*A(2,1)-A(2,0)*A(1,1))*invdet,
       -(A(0,0)*A(2,1)-A(2,0)*A(0,1))*invdet,
        (A(0,0)*A(1,1)-A(1,0)*A(0,1))*invdet };
    return m;
#undef A
}




GLKMatrix3 GLKMatrix4GetMatrix3(GLKMatrix4 matrix)
{
    GLKMatrix3 m = { matrix.m[0], matrix.m[1], matrix.m[2],
                     matrix.m[4], matrix.m[5], matrix.m[6],
                     matrix.m[8], matrix.m[9], matrix.m[10] };
    return m;
}


GLKMatrix3 GLKMatrix3InvertAndTranspose(GLKMatrix3 matrix, bool *isInvertible)
{
#define A(x,y)  matrix.m[y*3+x]
    double determinant = A(0,0)*(A(1,1)*A(2,2)-A(2,1)*A(1,2))-A(0,1)*(A(1,0)*A(2,2)-A(1,2)*A(2,0))+A(0,2)*(A(1,0)*A(2,1)-A(1,1)*A(2,0));
    bool invertible = (determinant == 0.0) ? false : true;
    if (isInvertible)
      *isInvertible = invertible;
    if (!invertible)
    {
      GLKMatrix3 m;
      return m;
    }
    float invdet = float(1.0 / determinant);
//        int x1 = {2,0,0,1,0,1,2,2,1}[i];
//        A((i%3)?0:1,(i>2)?0:1) * A(i%3==2?1:2,i>5?2:1) - A(x1,
    GLKMatrix3 m = { 
        (A(1,1)*A(2,2)-A(2,1)*A(1,2))*invdet,
       -(A(0,1)*A(2,2)-A(0,2)*A(2,1))*invdet,
        (A(0,1)*A(1,2)-A(0,2)*A(1,1))*invdet,
       -(A(1,0)*A(2,2)-A(1,2)*A(2,0))*invdet,
        (A(0,0)*A(2,2)-A(0,2)*A(2,0))*invdet,
       -(A(0,0)*A(1,2)-A(1,0)*A(0,2))*invdet,
        (A(1,0)*A(2,1)-A(2,0)*A(1,1))*invdet,
       -(A(0,0)*A(2,1)-A(2,0)*A(0,1))*invdet,
        (A(0,0)*A(1,1)-A(1,0)*A(0,1))*invdet };
    return m;
#undef A
}