/*!
 * \file src/Matrix.cpp
 * \brief 3D Matrix
 *
 * \author Mongoose
 */

#include <stdio.h>
#include <math.h>

#include "Matrix.h"

Matrix::Matrix() {
    setIdentity();
}

Matrix::Matrix(matrix_t m) {
    setMatrix(m);
}

Matrix::Matrix(Quaternion &q) {
    matrix_t m;
    q.getMatrix(m);
    setMatrix(m);
}

Matrix::~Matrix() {
}

bool Matrix::getInvert(matrix_t out) {
    matrix_t m;

#ifdef COLUMN_ORDER
    getMatrix(m);
#else
    getTransposeMatrix(m);
#endif

    /* Mongoose: This code was from a Jeff Lander tutorial which was based
       on MESA GL's InvertMatrix */

    /* NB. OpenGL Matrices are COLUMN major. */
#define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
#define MAT(m,r,c) (m)[(c)*4+(r)]

    float wtmp[4][8];
    float m0, m1, m2, m3, s;
    float *r0, *r1, *r2, *r3;

    r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];

    r0[0] = MAT(m,0,0), r0[1] = MAT(m,0,1),
    r0[2] = MAT(m,0,2), r0[3] = MAT(m,0,3),
    r0[4] = 1.0f, r0[5] = r0[6] = r0[7] = 0.0f,

    r1[0] = MAT(m,1,0), r1[1] = MAT(m,1,1),
    r1[2] = MAT(m,1,2), r1[3] = MAT(m,1,3),
    r1[5] = 1.0f, r1[4] = r1[6] = r1[7] = 0.0f,

    r2[0] = MAT(m,2,0), r2[1] = MAT(m,2,1),
    r2[2] = MAT(m,2,2), r2[3] = MAT(m,2,3),
    r2[6] = 1.0f, r2[4] = r2[5] = r2[7] = 0.0f,

    r3[0] = MAT(m,3,0), r3[1] = MAT(m,3,1),
    r3[2] = MAT(m,3,2), r3[3] = MAT(m,3,3),
    r3[7] = 1.0f, r3[4] = r3[5] = r3[6] = 0.0f;

    /* choose pivot - or die */
    if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
    if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
    if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
    if (0.0f == r0[0])  return false;

    /* eliminate first variable     */
    m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
    s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
    s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
    s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
    s = r0[4];
    if (s != 0.0f) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
    s = r0[5];
    if (s != 0.0f) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
    s = r0[6];
    if (s != 0.0f) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
    s = r0[7];
    if (s != 0.0f) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }

    /* choose pivot - or die */
    if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
    if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
    if (0.0f == r1[1])  return false;

    /* eliminate second variable */
    m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
    r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
    r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
    s = r1[4]; if (0.0f != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
    s = r1[5]; if (0.0f != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
    s = r1[6]; if (0.0f != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
    s = r1[7]; if (0.0f != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }

    /* choose pivot - or die */
    if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
    if (0.0f == r2[2])  return false;

    /* eliminate third variable */
    m3 = r3[2]/r2[2];
    r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
    r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
    r3[7] -= m3 * r2[7];

    /* last check */
    if (0.0f == r3[3]) return false;

    s = 1.0f/r3[3];              /* now back substitute row 3 */
    r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;

    m2 = r2[3];                 /* now back substitute row 2 */
    s  = 1.0f/r2[2];
    r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
    r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
    m1 = r1[3];
    r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
    r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
    m0 = r0[3];
    r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
    r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;

    m1 = r1[2];                 /* now back substitute row 1 */
    s  = 1.0f/r1[1];
    r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
    r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
    m0 = r0[2];
    r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
    r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;

    m0 = r0[1];                 /* now back substitute row 0 */
    s  = 1.0f/r0[0];
    r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
    r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);

    MAT(out,0,0) = r0[4];
    MAT(out,0,1) = r0[5], MAT(out,0,2) = r0[6];
    MAT(out,0,3) = r0[7], MAT(out,1,0) = r1[4];
    MAT(out,1,1) = r1[5], MAT(out,1,2) = r1[6];
    MAT(out,1,3) = r1[7], MAT(out,2,0) = r2[4];
    MAT(out,2,1) = r2[5], MAT(out,2,2) = r2[6];
    MAT(out,2,3) = r2[7], MAT(out,3,0) = r3[4];
    MAT(out,3,1) = r3[5], MAT(out,3,2) = r3[6];
    MAT(out,3,3) = r3[7];

    return true;
#undef MAT
#undef SWAP_ROWS
}

void Matrix::getMatrix(matrix_t mat) {
    copy(mMatrix, mat);
}

void Matrix::getTransposeMatrix(matrix_t m) {
    m[ 0]= mMatrix[0]; m[ 1]= mMatrix[4]; m[ 2]= mMatrix[ 8]; m[ 3]=mMatrix[12];
    m[ 4]= mMatrix[1]; m[ 5]= mMatrix[5]; m[ 6]= mMatrix[ 9]; m[ 7]=mMatrix[13];
    m[ 8]= mMatrix[2]; m[ 9]= mMatrix[6]; m[10]= mMatrix[10]; m[11]=mMatrix[14];
    m[12]= mMatrix[3]; m[13]= mMatrix[7]; m[14]= mMatrix[11]; m[15]=mMatrix[15];
}

Matrix Matrix::multiply(const Matrix &a, const Matrix &b) {
    Matrix c;
    multiply(a.mMatrix, b.mMatrix, c.mMatrix);
    return c;
}

Matrix Matrix::operator *(const Matrix &a) {
    return multiply(a, *this);
}

Vector3d Matrix::operator *(Vector3d v) {
    vec_t x = v.mVec[0], y = v.mVec[1], z = v.mVec[2];

#ifdef COLUMN_ORDER
    return Vector3d(mMatrix[0]*x + mMatrix[4]*y + mMatrix[ 8]*z + mMatrix[12],
            mMatrix[1]*x + mMatrix[5]*y + mMatrix[ 9]*z + mMatrix[13],
            mMatrix[2]*x + mMatrix[6]*y + mMatrix[10]*z + mMatrix[14]);
#else
    return Vector3d(mMatrix[0]*x + mMatrix[1]*y + mMatrix[ 2]*z + mMatrix[ 3],
            mMatrix[4]*x + mMatrix[5]*y + mMatrix[ 6]*z + mMatrix[ 7],
            mMatrix[8]*x + mMatrix[9]*y + mMatrix[10]*z + mMatrix[11]);
#endif
}

void Matrix::multiply3v(vec3_t v, vec3_t result) {
    vec_t x = v[0], y = v[1], z = v[2];

    result[0] = mMatrix[0]*x + mMatrix[1]*y + mMatrix[ 2]*z + mMatrix[ 3];
    result[1] = mMatrix[4]*x + mMatrix[5]*y + mMatrix[ 6]*z + mMatrix[ 7];
    result[2] = mMatrix[8]*x + mMatrix[9]*y + mMatrix[10]*z + mMatrix[11];
}

void Matrix::multiply4v(vec4_t v, vec4_t result) {
    vec_t x = v[0], y = v[1], z = v[2], w = v[3];

    result[0] = mMatrix[ 0]*x + mMatrix[ 1]*y + mMatrix[ 2]*z + mMatrix[ 3]*w;
    result[1] = mMatrix[ 4]*x + mMatrix[ 5]*y + mMatrix[ 6]*z + mMatrix[ 7]*w;
    result[2] = mMatrix[ 8]*x + mMatrix[ 9]*y + mMatrix[10]*z + mMatrix[11]*w;
    result[3] = mMatrix[12]*x + mMatrix[13]*y + mMatrix[14]*z + mMatrix[15]*w;
}

void Matrix::print() {
    printf("{\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n}\n",
#ifdef COLUMN_ORDER
            mMatrix[0], mMatrix[4], mMatrix[ 8], mMatrix[12],
            mMatrix[1], mMatrix[5], mMatrix[ 9], mMatrix[13],
            mMatrix[2], mMatrix[6], mMatrix[10], mMatrix[14],
            mMatrix[3], mMatrix[7], mMatrix[11], mMatrix[15]);
#else
            mMatrix[ 0], mMatrix[ 1], mMatrix[ 2], mMatrix[ 3],
            mMatrix[ 4], mMatrix[ 5], mMatrix[ 6], mMatrix[ 7],
            mMatrix[ 8], mMatrix[ 9], mMatrix[10], mMatrix[11],
            mMatrix[12], mMatrix[13], mMatrix[14], mMatrix[15]);
#endif
}

bool Matrix::isIdentity() {
    // Hhhmm... floating point using direct comparisons
    /*
    if (mMatrix[ 0] == 1 && mMatrix[ 1] == 0 && mMatrix[ 2] == 0 &&
            mMatrix[ 3] == 0 &&    mMatrix[ 4] == 0 && mMatrix[ 5] == 1 &&
            mMatrix[ 6] == 0 && mMatrix[ 7] == 0 && mMatrix[ 8] == 0 &&
            mMatrix[ 9] == 0 && mMatrix[10] == 1 && mMatrix[11] == 0 &&
            mMatrix[12] == 0 && mMatrix[13] == 0 && mMatrix[14] == 0 &&
            mMatrix[15] == 1)
        return true;
    */
    if (equalEpsilon(mMatrix[ 0], 1.0) && equalEpsilon(mMatrix[ 1], 0.0) && equalEpsilon(mMatrix[ 2], 0.0) &&
        equalEpsilon(mMatrix[ 3], 0.0) && equalEpsilon(mMatrix[ 4], 0.0) && equalEpsilon(mMatrix[ 5], 1.0) &&
        equalEpsilon(mMatrix[ 6], 0.0) && equalEpsilon(mMatrix[ 7], 0.0) && equalEpsilon(mMatrix[ 8], 0.0) &&
        equalEpsilon(mMatrix[ 9], 0.0) && equalEpsilon(mMatrix[10], 1.0) && equalEpsilon(mMatrix[11], 0.0) &&
        equalEpsilon(mMatrix[12], 0.0) && equalEpsilon(mMatrix[13], 0.0) && equalEpsilon(mMatrix[14], 0.0) &&
        equalEpsilon(mMatrix[15], 1.0))
        return true;

    return false;
}

void Matrix::setMatrix(matrix_t mat) {
    copy(mat, mMatrix);
}

void Matrix::setIdentity() {
    mMatrix[ 0] = 1; mMatrix[ 1] = 0; mMatrix[ 2] = 0; mMatrix[ 3] = 0;
    mMatrix[ 4] = 0; mMatrix[ 5] = 1; mMatrix[ 6] = 0; mMatrix[ 7] = 0;
    mMatrix[ 8] = 0; mMatrix[ 9] = 0; mMatrix[10] = 1; mMatrix[11] = 0;
    mMatrix[12] = 0; mMatrix[13] = 0; mMatrix[14] = 0; mMatrix[15] = 1;
}

void Matrix::scale(const vec_t *xyz) {
    scale(xyz[0], xyz[1], xyz[2]);
}

void Matrix::scale(vec_t sx, vec_t sy, vec_t sz) {
    matrix_t smatrix;
    matrix_t tmp;

    smatrix[ 0] = sx; smatrix[ 1] = 0;  smatrix[ 2] = 0;  smatrix[ 3] = 0;
    smatrix[ 4] = 0;  smatrix[ 5] = sy; smatrix[ 6] = 0;  smatrix[ 7] = 0;
    smatrix[ 8] = 0;  smatrix[ 9] = 0;  smatrix[10] = sz; smatrix[11] = 0;
    smatrix[12] = 0;  smatrix[13] = 0;  smatrix[14] = 0;  smatrix[15] = 1;

    copy(mMatrix, tmp);
    multiply(tmp, smatrix, mMatrix);
}

void Matrix::rotate(const vec_t *xyz) {
    rotate(xyz[0], xyz[1], xyz[2]);
}

void Matrix::rotate(vec_t ax, vec_t ay, vec_t az) {
    matrix_t xmat, ymat, zmat, tmp, tmp2;

    xmat[ 0]=1;         xmat[ 1]=0;         xmat[ 2]=0;         xmat[ 3]=0;
    xmat[ 4]=0;         xmat[ 5]=cosf(ax);  xmat[ 6]=sinf(ax);  xmat[ 7]=0;
    xmat[ 8]=0;         xmat[ 9]=-sinf(ax); xmat[10]=cosf(ax);  xmat[11]=0;
    xmat[12]=0;         xmat[13]=0;         xmat[14]=0;         xmat[15]=1;

    ymat[ 0]=cosf(ay);  ymat[ 1]=0;         ymat[ 2]=-sinf(ay); ymat[ 3]=0;
    ymat[ 4]=0;         ymat[ 5]=1;         ymat[ 6]=0;         ymat[ 7]=0;
    ymat[ 8]=sinf(ay);  ymat[ 9]=0;         ymat[10]=cosf(ay);  ymat[11]=0;
    ymat[12]=0;         ymat[13]=0;         ymat[14]=0;         ymat[15]=1;

    zmat[ 0]=cosf(az);  zmat[ 1]=sinf(az);  zmat[ 2]=0;         zmat[ 3]=0;
    zmat[ 4]=-sinf(az); zmat[ 5]=cosf(az);  zmat[ 6]=0;         zmat[ 7]=0;
    zmat[ 8]=0;         zmat[ 9]=0;         zmat[10]=1;         zmat[11]=0;
    zmat[12]=0;         zmat[13]=0;         zmat[14]=0;         zmat[15]=1;

    multiply(mMatrix, ymat, tmp);
    multiply(tmp, xmat, tmp2);
    multiply(tmp2, zmat, mMatrix);
}

void Matrix::translate(const vec_t *xyz) {
    translate(xyz[0], xyz[1], xyz[2]);
}

void Matrix::translate(vec_t tx, vec_t ty, vec_t tz) {
    matrix_t tmat, tmp;

    tmat[ 0]=1;  tmat[ 1]=0;  tmat[ 2]=0;  tmat[ 3]=0;
    tmat[ 4]=0;  tmat[ 5]=1;  tmat[ 6]=0;  tmat[ 7]=0;
    tmat[ 8]=0;  tmat[ 9]=0;  tmat[10]=1;  tmat[11]=0;
    tmat[12]=tx; tmat[13]=ty; tmat[14]=tz; tmat[15]=1;

    copy(mMatrix, tmp);
    multiply(tmp, tmat, mMatrix);
}

void Matrix::copy(matrix_t source, matrix_t dest) {
    for (int i = 0; i < 16; i++)
        dest[i] = source[i];
}

void Matrix::multiply(const matrix_t a, const matrix_t b, matrix_t result) {
    /* Generated code for matrix mult
     * Code used:

    // char order is argument
    int i, j, k;
    if (order == 'r') {
        printf("// Row order\n");
    } else {
        printf("// Column order\n");
    }
    for (i = 0; i < 4; ++i) {
        for (j = 0; j < 4; ++j) {
            if (order == 'r') {
                printf("result[%2i] = ", j+i*4);
            } else {
                printf("result[%2i] = ", j+i*4);
            }
            for (k = 0; k < 4; ++k) {
                if (order == 'r') {
                    printf("a[%2i] * b[%2i]%s",
                    k+i*4, j+k*4, (k == 3) ? ";\n" : " + ");
                } else {
                    printf("a[%2i] * b[%2i]%s",
                    i+k*4, k+j*4, (k == 3) ? ";\n" : " + ");
                }
                //sum+=(elements[i+k*4]*m.elements[k+j*4]);
            }
            //result.elements[i+j*4]=sum;
        }
        printf("\n");
    }
    printf("\n");
    printf("// Transpose\n");
    for(i = 0; i < 4; ++i) {
        for (j = 0; j < 4; ++j) {
            printf("a[%2i] = b[%2i]%s",
            j+i*4, i+j*4, (j == 3) ? ";\n" : "; ");
        }
    }

     * was in test/Matrix.cpp
     */
#ifdef COLUMN_ORDER
    /* Column order */
    result[ 0] = a[ 0] * b[ 0] + a[ 4] * b[ 1] + a[ 8] * b[ 2] + a[12] * b[ 3];
    result[ 1] = a[ 0] * b[ 4] + a[ 4] * b[ 5] + a[ 8] * b[ 6] + a[12] * b[ 7];
    result[ 2] = a[ 0] * b[ 8] + a[ 4] * b[ 9] + a[ 8] * b[10] + a[12] * b[11];
    result[ 3] = a[ 0] * b[12] + a[ 4] * b[13] + a[ 8] * b[14] + a[12] * b[15];

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

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

    result[12] = a[ 3] * b[ 0] + a[ 7] * b[ 1] + a[11] * b[ 2] + a[15] * b[ 3];
    result[13] = a[ 3] * b[ 4] + a[ 7] * b[ 5] + a[11] * b[ 6] + a[15] * b[ 7];
    result[14] = a[ 3] * b[ 8] + a[ 7] * b[ 9] + a[11] * b[10] + a[15] * b[11];
    result[15] = a[ 3] * b[12] + a[ 7] * b[13] + a[11] * b[14] + a[15] * b[15];
#else
    /* Row order */
    result[ 0] = a[ 0] * b[ 0] + a[ 1] * b[ 4] + a[ 2] * b[ 8] + a[ 3] * b[12];
    result[ 1] = a[ 0] * b[ 1] + a[ 1] * b[ 5] + a[ 2] * b[ 9] + a[ 3] * b[13];
    result[ 2] = a[ 0] * b[ 2] + a[ 1] * b[ 6] + a[ 2] * b[10] + a[ 3] * b[14];
    result[ 3] = a[ 0] * b[ 3] + a[ 1] * b[ 7] + a[ 2] * b[11] + a[ 3] * b[15];

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

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

    result[12] = a[12] * b[ 0] + a[13] * b[ 4] + a[14] * b[ 8] + a[15] * b[12];
    result[13] = a[12] * b[ 1] + a[13] * b[ 5] + a[14] * b[ 9] + a[15] * b[13];
    result[14] = a[12] * b[ 2] + a[13] * b[ 6] + a[14] * b[10] + a[15] * b[14];
    result[15] = a[12] * b[ 3] + a[13] * b[ 7] + a[14] * b[11] + a[15] * b[15];
#endif
}