///////////////////////////////////////////////////////////////////////////////
// Matrice.cpp
// ===========
// NxN Matrix Math classes
//
// The elements of the matrix are stored as column major order.
// | 0 2 |    | 0 3 6 |    |  0  4  8 12 |
// | 1 3 |    | 1 4 7 |    |  1  5  9 13 |
//            | 2 5 8 |    |  2  6 10 14 |
//                         |  3  7 11 15 |
//
//  AUTHOR: Song Ho Ahn (song.ahn@gmail.com)
// CREATED: 2005-06-24
// UPDATED: 2014-09-21
//
// Copyright (C) 2005 Song Ho Ahn
///////////////////////////////////////////////////////////////////////////////

#include <cmath>
#include <algorithm>
#include "matrices.hpp"

namespace lineag {

const float DEG2RAD = 3.141593f / 180;
const float EPSILON = 0.00001f;

///////////////////////////////////////////////////////////////////////////////
// transpose 2x2 matrix
///////////////////////////////////////////////////////////////////////////////
Matrix2& Matrix2::transpose() {
  std::swap(m[1], m[2]);
  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// return the determinant of 2x2 matrix
///////////////////////////////////////////////////////////////////////////////
float Matrix2::getDeterminant() { return m[0] * m[3] - m[1] * m[2]; }

///////////////////////////////////////////////////////////////////////////////
// inverse of 2x2 matrix
// If cannot find inverse, set identity matrix
///////////////////////////////////////////////////////////////////////////////
Matrix2& Matrix2::invert() {
  float determinant = getDeterminant();
  if (fabs(determinant) <= EPSILON) {
    return identity();
  }

  float tmp = m[0];  // copy the first element
  float invDeterminant = 1.0f / determinant;
  m[0] = invDeterminant * m[3];
  m[1] = -invDeterminant * m[1];
  m[2] = -invDeterminant * m[2];
  m[3] = invDeterminant * tmp;

  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// transpose 3x3 matrix
///////////////////////////////////////////////////////////////////////////////
Matrix3& Matrix3::transpose() {
  std::swap(m[1], m[3]);
  std::swap(m[2], m[6]);
  std::swap(m[5], m[7]);

  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// return determinant of 3x3 matrix
///////////////////////////////////////////////////////////////////////////////
float Matrix3::getDeterminant() {
  return m[0] * (m[4] * m[8] - m[5] * m[7]) - m[1] * (m[3] * m[8] - m[5] * m[6]) + m[2] * (m[3] * m[7] - m[4] * m[6]);
}

///////////////////////////////////////////////////////////////////////////////
// inverse 3x3 matrix
// If cannot find inverse, set identity matrix
///////////////////////////////////////////////////////////////////////////////
Matrix3& Matrix3::invert() {
  float determinant, invDeterminant;
  float tmp[9];

  tmp[0] = m[4] * m[8] - m[5] * m[7];
  tmp[1] = m[2] * m[7] - m[1] * m[8];
  tmp[2] = m[1] * m[5] - m[2] * m[4];
  tmp[3] = m[5] * m[6] - m[3] * m[8];
  tmp[4] = m[0] * m[8] - m[2] * m[6];
  tmp[5] = m[2] * m[3] - m[0] * m[5];
  tmp[6] = m[3] * m[7] - m[4] * m[6];
  tmp[7] = m[1] * m[6] - m[0] * m[7];
  tmp[8] = m[0] * m[4] - m[1] * m[3];

  // check determinant if it is 0
  determinant = m[0] * tmp[0] + m[1] * tmp[3] + m[2] * tmp[6];
  if (fabs(determinant) <= EPSILON) {
    return identity();  // cannot inverse, make it idenety matrix
  }

  // divide by the determinant
  invDeterminant = 1.0f / determinant;
  m[0] = invDeterminant * tmp[0];
  m[1] = invDeterminant * tmp[1];
  m[2] = invDeterminant * tmp[2];
  m[3] = invDeterminant * tmp[3];
  m[4] = invDeterminant * tmp[4];
  m[5] = invDeterminant * tmp[5];
  m[6] = invDeterminant * tmp[6];
  m[7] = invDeterminant * tmp[7];
  m[8] = invDeterminant * tmp[8];

  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// transpose 4x4 matrix
///////////////////////////////////////////////////////////////////////////////
Matrix4& Matrix4::transpose() {
  std::swap(m[1], m[4]);
  std::swap(m[2], m[8]);
  std::swap(m[3], m[12]);
  std::swap(m[6], m[9]);
  std::swap(m[7], m[13]);
  std::swap(m[11], m[14]);

  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// inverse 4x4 matrix
///////////////////////////////////////////////////////////////////////////////
Matrix4& Matrix4::invert() {
  // If the 4th row is [0,0,0,1] then it is affine matrix and
  // it has no projective transformation.
  if (m[3] == 0 && m[7] == 0 && m[11] == 0 && m[15] == 1)
    this->invertAffine();
  else {
    this->invertGeneral();
    /*@@ invertProjective() is not optimized (slower than generic one)
    if(fabs(m[0]*m[5] - m[1]*m[4]) > EPSILON)
        this->invertProjective();   // inverse using matrix partition
    else
        this->invertGeneral();      // generalized inverse
    */
  }

  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// compute the inverse of 4x4 Euclidean transformation matrix
//
// Euclidean transformation is translation, rotation, and reflection.
// With Euclidean transform, only the position and orientation of the object
// will be changed. Euclidean transform does not change the shape of an object
// (no scaling). Length and angle are reserved.
//
// Use inverseAffine() if the matrix has scale and shear transformation.
//
// M = [ R | T ]
//     [ --+-- ]    (R denotes 3x3 rotation/reflection matrix)
//     [ 0 | 1 ]    (T denotes 1x3 translation matrix)
//
// y = M*x  ->  y = R*x + T  ->  x = R^-1*(y - T)  ->  x = R^T*y - R^T*T
// (R is orthogonal,  R^-1 = R^T)
//
//  [ R | T ]-1    [ R^T | -R^T * T ]    (R denotes 3x3 rotation matrix)
//  [ --+-- ]   =  [ ----+--------- ]    (T denotes 1x3 translation)
//  [ 0 | 1 ]      [  0  |     1    ]    (R^T denotes R-transpose)
///////////////////////////////////////////////////////////////////////////////
Matrix4& Matrix4::invertEuclidean() {
  // transpose 3x3 rotation matrix part
  // | R^T | 0 |
  // | ----+-- |
  // |  0  | 1 |
  float tmp;
  tmp = m[1];
  m[1] = m[4];
  m[4] = tmp;
  tmp = m[2];
  m[2] = m[8];
  m[8] = tmp;
  tmp = m[6];
  m[6] = m[9];
  m[9] = tmp;

  // compute translation part -R^T * T
  // | 0 | -R^T x |
  // | --+------- |
  // | 0 |   0    |
  float x = m[12];
  float y = m[13];
  float z = m[14];
  m[12] = -(m[0] * x + m[4] * y + m[8] * z);
  m[13] = -(m[1] * x + m[5] * y + m[9] * z);
  m[14] = -(m[2] * x + m[6] * y + m[10] * z);

  // last row should be unchanged (0,0,0,1)

  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// compute the inverse of a 4x4 affine transformation matrix
//
// Affine transformations are generalizations of Euclidean transformations.
// Affine transformation includes translation, rotation, reflection, scaling,
// and shearing. Length and angle are NOT preserved.
// M = [ R | T ]
//     [ --+-- ]    (R denotes 3x3 rotation/scale/shear matrix)
//     [ 0 | 1 ]    (T denotes 1x3 translation matrix)
//
// y = M*x  ->  y = R*x + T  ->  x = R^-1*(y - T)  ->  x = R^-1*y - R^-1*T
//
//  [ R | T ]-1   [ R^-1 | -R^-1 * T ]
//  [ --+-- ]   = [ -----+---------- ]
//  [ 0 | 1 ]     [  0   +     1     ]
///////////////////////////////////////////////////////////////////////////////
Matrix4& Matrix4::invertAffine() {
  // R^-1
  Matrix3 r(m[0], m[1], m[2], m[4], m[5], m[6], m[8], m[9], m[10]);
  r.invert();
  m[0] = r[0];
  m[1] = r[1];
  m[2] = r[2];
  m[4] = r[3];
  m[5] = r[4];
  m[6] = r[5];
  m[8] = r[6];
  m[9] = r[7];
  m[10] = r[8];

  // -R^-1 * T
  float x = m[12];
  float y = m[13];
  float z = m[14];
  m[12] = -(r[0] * x + r[3] * y + r[6] * z);
  m[13] = -(r[1] * x + r[4] * y + r[7] * z);
  m[14] = -(r[2] * x + r[5] * y + r[8] * z);

  // last row should be unchanged (0,0,0,1)
  // m[3] = m[7] = m[11] = 0.0f;
  // m[15] = 1.0f;

  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// inverse matrix using matrix partitioning (blockwise inverse)
// It devides a 4x4 matrix into 4 of 2x2 matrices. It works in case of where
// det(A) != 0. If not, use the generic inverse method
// inverse formula.
// M = [ A | B ]    A, B, C, D are 2x2 matrix blocks
//     [ --+-- ]    det(M) = |A| * |D - ((C * A^-1) * B)|
//     [ C | D ]
//
// M^-1 = [ A' | B' ]   A' = A^-1 - (A^-1 * B) * C'
//        [ ---+--- ]   B' = (A^-1 * B) * -D'
//        [ C' | D' ]   C' = -D' * (C * A^-1)
//                      D' = (D - ((C * A^-1) * B))^-1
//
// NOTE: I wrap with () if it it used more than once.
//       The matrix is invertable even if det(A)=0, so must check det(A) before
//       calling this function, and use invertGeneric() instead.
///////////////////////////////////////////////////////////////////////////////
Matrix4& Matrix4::invertProjective() {
  // partition
  Matrix2 a(m[0], m[1], m[4], m[5]);
  Matrix2 b(m[8], m[9], m[12], m[13]);
  Matrix2 c(m[2], m[3], m[6], m[7]);
  Matrix2 d(m[10], m[11], m[14], m[15]);

  // pre-compute repeated parts
  a.invert();              // A^-1
  Matrix2 ab = a * b;      // A^-1 * B
  Matrix2 ca = c * a;      // C * A^-1
  Matrix2 cab = ca * b;    // C * A^-1 * B
  Matrix2 dcab = d - cab;  // D - C * A^-1 * B

  // check determinant if |D - C * A^-1 * B| = 0
  // NOTE: this function assumes det(A) is already checked. if |A|=0 then,
  //      cannot use this function.
  float determinant = dcab[0] * dcab[3] - dcab[1] * dcab[2];
  if (fabs(determinant) <= EPSILON) {
    return identity();
  }

  // compute D' and -D'
  Matrix2 d1 = dcab;  //  (D - C * A^-1 * B)
  d1.invert();        //  (D - C * A^-1 * B)^-1
  Matrix2 d2 = -d1;   // -(D - C * A^-1 * B)^-1

  // compute C'
  Matrix2 c1 = d2 * ca;  // -D' * (C * A^-1)

  // compute B'
  Matrix2 b1 = ab * d2;  // (A^-1 * B) * -D'

  // compute A'
  Matrix2 a1 = a - (ab * c1);  // A^-1 - (A^-1 * B) * C'

  // assemble inverse matrix
  m[0] = a1[0];
  m[4] = a1[2]; /*|*/
  m[8] = b1[0];
  m[12] = b1[2];
  m[1] = a1[1];
  m[5] = a1[3]; /*|*/
  m[9] = b1[1];
  m[13] = b1[3];
  /*-----------------------------+-----------------------------*/
  m[2] = c1[0];
  m[6] = c1[2]; /*|*/
  m[10] = d1[0];
  m[14] = d1[2];
  m[3] = c1[1];
  m[7] = c1[3]; /*|*/
  m[11] = d1[1];
  m[15] = d1[3];

  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// compute the inverse of a general 4x4 matrix using Cramer's Rule
// If cannot find inverse, return indentity matrix
// M^-1 = adj(M) / det(M)
///////////////////////////////////////////////////////////////////////////////
Matrix4& Matrix4::invertGeneral() {
  // get cofactors of minor matrices
  float cofactor0 = getCofactor(m[5], m[6], m[7], m[9], m[10], m[11], m[13], m[14], m[15]);
  float cofactor1 = getCofactor(m[4], m[6], m[7], m[8], m[10], m[11], m[12], m[14], m[15]);
  float cofactor2 = getCofactor(m[4], m[5], m[7], m[8], m[9], m[11], m[12], m[13], m[15]);
  float cofactor3 = getCofactor(m[4], m[5], m[6], m[8], m[9], m[10], m[12], m[13], m[14]);

  // get determinant
  float determinant = m[0] * cofactor0 - m[1] * cofactor1 + m[2] * cofactor2 - m[3] * cofactor3;
  if (fabs(determinant) <= EPSILON) {
    return identity();
  }

  // get rest of cofactors for adj(M)
  float cofactor4 = getCofactor(m[1], m[2], m[3], m[9], m[10], m[11], m[13], m[14], m[15]);
  float cofactor5 = getCofactor(m[0], m[2], m[3], m[8], m[10], m[11], m[12], m[14], m[15]);
  float cofactor6 = getCofactor(m[0], m[1], m[3], m[8], m[9], m[11], m[12], m[13], m[15]);
  float cofactor7 = getCofactor(m[0], m[1], m[2], m[8], m[9], m[10], m[12], m[13], m[14]);

  float cofactor8 = getCofactor(m[1], m[2], m[3], m[5], m[6], m[7], m[13], m[14], m[15]);
  float cofactor9 = getCofactor(m[0], m[2], m[3], m[4], m[6], m[7], m[12], m[14], m[15]);
  float cofactor10 = getCofactor(m[0], m[1], m[3], m[4], m[5], m[7], m[12], m[13], m[15]);
  float cofactor11 = getCofactor(m[0], m[1], m[2], m[4], m[5], m[6], m[12], m[13], m[14]);

  float cofactor12 = getCofactor(m[1], m[2], m[3], m[5], m[6], m[7], m[9], m[10], m[11]);
  float cofactor13 = getCofactor(m[0], m[2], m[3], m[4], m[6], m[7], m[8], m[10], m[11]);
  float cofactor14 = getCofactor(m[0], m[1], m[3], m[4], m[5], m[7], m[8], m[9], m[11]);
  float cofactor15 = getCofactor(m[0], m[1], m[2], m[4], m[5], m[6], m[8], m[9], m[10]);

  // build inverse matrix = adj(M) / det(M)
  // adjugate of M is the transpose of the cofactor matrix of M
  float invDeterminant = 1.0f / determinant;
  m[0] = invDeterminant * cofactor0;
  m[1] = -invDeterminant * cofactor4;
  m[2] = invDeterminant * cofactor8;
  m[3] = -invDeterminant * cofactor12;

  m[4] = -invDeterminant * cofactor1;
  m[5] = invDeterminant * cofactor5;
  m[6] = -invDeterminant * cofactor9;
  m[7] = invDeterminant * cofactor13;

  m[8] = invDeterminant * cofactor2;
  m[9] = -invDeterminant * cofactor6;
  m[10] = invDeterminant * cofactor10;
  m[11] = -invDeterminant * cofactor14;

  m[12] = -invDeterminant * cofactor3;
  m[13] = invDeterminant * cofactor7;
  m[14] = -invDeterminant * cofactor11;
  m[15] = invDeterminant * cofactor15;

  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// return determinant of 4x4 matrix
///////////////////////////////////////////////////////////////////////////////
float Matrix4::getDeterminant() {
  return m[0] * getCofactor(m[5], m[6], m[7], m[9], m[10], m[11], m[13], m[14], m[15]) -
         m[1] * getCofactor(m[4], m[6], m[7], m[8], m[10], m[11], m[12], m[14], m[15]) +
         m[2] * getCofactor(m[4], m[5], m[7], m[8], m[9], m[11], m[12], m[13], m[15]) -
         m[3] * getCofactor(m[4], m[5], m[6], m[8], m[9], m[10], m[12], m[13], m[14]);
}

///////////////////////////////////////////////////////////////////////////////
// compute cofactor of 3x3 minor matrix without sign
// input params are 9 elements of the minor matrix
// NOTE: The caller must know its sign.
///////////////////////////////////////////////////////////////////////////////
float Matrix4::getCofactor(float m0, float m1, float m2, float m3, float m4, float m5, float m6, float m7, float m8) {
  return m0 * (m4 * m8 - m5 * m7) - m1 * (m3 * m8 - m5 * m6) + m2 * (m3 * m7 - m4 * m6);
}

///////////////////////////////////////////////////////////////////////////////
// translate this matrix by (x, y, z)
///////////////////////////////////////////////////////////////////////////////
Matrix4& Matrix4::translate(const Vector3& v) { return translate(v.x, v.y, v.z); }

Matrix4& Matrix4::translate(float x, float y, float z) {
  m[0] += m[3] * x;
  m[4] += m[7] * x;
  m[8] += m[11] * x;
  m[12] += m[15] * x;
  m[1] += m[3] * y;
  m[5] += m[7] * y;
  m[9] += m[11] * y;
  m[13] += m[15] * y;
  m[2] += m[3] * z;
  m[6] += m[7] * z;
  m[10] += m[11] * z;
  m[14] += m[15] * z;

  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// uniform scale
///////////////////////////////////////////////////////////////////////////////
Matrix4& Matrix4::scale(float s) { return scale(s, s, s); }

Matrix4& Matrix4::scale(float x, float y, float z) {
  m[0] *= x;
  m[4] *= x;
  m[8] *= x;
  m[12] *= x;
  m[1] *= y;
  m[5] *= y;
  m[9] *= y;
  m[13] *= y;
  m[2] *= z;
  m[6] *= z;
  m[10] *= z;
  m[14] *= z;
  return *this;
}

///////////////////////////////////////////////////////////////////////////////
// build a rotation matrix with given angle(degree) and rotation axis, then
// multiply it with this object
///////////////////////////////////////////////////////////////////////////////
Matrix4& Matrix4::rotate(float angle, const Vector3& axis) { return rotate(angle, axis.x, axis.y, axis.z); }

Matrix4& Matrix4::rotate(float angle, float x, float y, float z) {
  float c = cosf(angle * DEG2RAD);  // cosine
  float s = sinf(angle * DEG2RAD);  // sine
  float c1 = 1.0f - c;              // 1 - c
  float m0 = m[0], m4 = m[4], m8 = m[8], m12 = m[12], m1 = m[1], m5 = m[5], m9 = m[9], m13 = m[13], m2 = m[2],
        m6 = m[6], m10 = m[10], m14 = m[14];

  // build rotation matrix
  float r0 = x * x * c1 + c;
  float r1 = x * y * c1 + z * s;
  float r2 = x * z * c1 - y * s;
  float r4 = x * y * c1 - z * s;
  float r5 = y * y * c1 + c;
  float r6 = y * z * c1 + x * s;
  float r8 = x * z * c1 + y * s;
  float r9 = y * z * c1 - x * s;
  float r10 = z * z * c1 + c;

  // multiply rotation matrix
  m[0] = r0 * m0 + r4 * m1 + r8 * m2;
  m[1] = r1 * m0 + r5 * m1 + r9 * m2;
  m[2] = r2 * m0 + r6 * m1 + r10 * m2;
  m[4] = r0 * m4 + r4 * m5 + r8 * m6;
  m[5] = r1 * m4 + r5 * m5 + r9 * m6;
  m[6] = r2 * m4 + r6 * m5 + r10 * m6;
  m[8] = r0 * m8 + r4 * m9 + r8 * m10;
  m[9] = r1 * m8 + r5 * m9 + r9 * m10;
  m[10] = r2 * m8 + r6 * m9 + r10 * m10;
  m[12] = r0 * m12 + r4 * m13 + r8 * m14;
  m[13] = r1 * m12 + r5 * m13 + r9 * m14;
  m[14] = r2 * m12 + r6 * m13 + r10 * m14;

  return *this;
}

Matrix4& Matrix4::rotateX(float angle) {
  float c = cosf(angle * DEG2RAD);
  float s = sinf(angle * DEG2RAD);
  float m1 = m[1], m2 = m[2], m5 = m[5], m6 = m[6], m9 = m[9], m10 = m[10], m13 = m[13], m14 = m[14];

  m[1] = m1 * c + m2 * -s;
  m[2] = m1 * s + m2 * c;
  m[5] = m5 * c + m6 * -s;
  m[6] = m5 * s + m6 * c;
  m[9] = m9 * c + m10 * -s;
  m[10] = m9 * s + m10 * c;
  m[13] = m13 * c + m14 * -s;
  m[14] = m13 * s + m14 * c;

  return *this;
}

Matrix4& Matrix4::rotateY(float angle) {
  float c = cosf(angle * DEG2RAD);
  float s = sinf(angle * DEG2RAD);
  float m0 = m[0], m2 = m[2], m4 = m[4], m6 = m[6], m8 = m[8], m10 = m[10], m12 = m[12], m14 = m[14];

  m[0] = m0 * c + m2 * s;
  m[2] = m0 * -s + m2 * c;
  m[4] = m4 * c + m6 * s;
  m[6] = m4 * -s + m6 * c;
  m[8] = m8 * c + m10 * s;
  m[10] = m8 * -s + m10 * c;
  m[12] = m12 * c + m14 * s;
  m[14] = m12 * -s + m14 * c;

  return *this;
}

Matrix4& Matrix4::rotateZ(float angle) {
  float c = cosf(angle * DEG2RAD);
  float s = sinf(angle * DEG2RAD);
  float m0 = m[0], m1 = m[1], m4 = m[4], m5 = m[5], m8 = m[8], m9 = m[9], m12 = m[12], m13 = m[13];

  m[0] = m0 * c + m1 * -s;
  m[1] = m0 * s + m1 * c;
  m[4] = m4 * c + m5 * -s;
  m[5] = m4 * s + m5 * c;
  m[8] = m8 * c + m9 * -s;
  m[9] = m8 * s + m9 * c;
  m[12] = m12 * c + m13 * -s;
  m[13] = m12 * s + m13 * c;

  return *this;
}
}  // namespace lineag