/* ================================================================
Mat4
----------------
This file defines the class for 4 dimensional matrices. It relies on
Vec4 for functionality.
================================================================ */
#include "Mat4.hpp"
#include <string.h> // memset
#include <iostream>

Mat4::Mat4() {
  vecs[0][0] = 1.0f;
  vecs[1][1] = 1.0f;
  vecs[2][2] = 1.0f;
  vecs[3][3] = 1.0f;
}
Mat4::~Mat4() {}
Mat4::Mat4(Vec4 a_, Vec4 b_, Vec4 c_, Vec4 d_) {
  vecs[0] = a_;
  vecs[1] = b_;
  vecs[2] = c_;
  vecs[3] = d_;
}

float *Mat4::toFloats(float *floats) {
  int i;
  float *m = &vecs[0][0];
  for (i = 0; i < 16; i++) {
    floats[i] = m[i];
  }
  return floats;
}
Mat4 Mat4::getInverse(Mat4 out) {
   float *m = &vecs[0][0]; // just so my brain doesn't explode
   out[0][0] = m[5]  * m[10] * m[15] - 
               m[5]  * m[11] * m[14] - 
               m[9]  * m[6]  * m[15] + 
               m[9]  * m[7]  * m[14] +
               m[13] * m[6]  * m[11] - 
               m[13] * m[7]  * m[10];
  out[0][1] = -m[1]  * m[10] * m[15] + 
               m[1]  * m[11] * m[14] + 
               m[9]  * m[2] * m[15] - 
               m[9]  * m[3] * m[14] - 
               m[13] * m[2] * m[11] + 
               m[13] * m[3] * m[10];
  out[0][2] = m[1]  * m[6] * m[15] - 
              m[1]  * m[7] * m[14] - 
              m[5]  * m[2] * m[15] + 
              m[5]  * m[3] * m[14] + 
              m[13] * m[2] * m[7] - 
              m[13] * m[3] * m[6];
  out[0][3] = -m[1] * m[6] * m[11] + 
               m[1] * m[7] * m[10] + 
               m[5] * m[2] * m[11] - 
               m[5] * m[3] * m[10] - 
               m[9] * m[2] * m[7] + 
               m[9] * m[3] * m[6];
  out[1][0] = -m[4]  * m[10] * m[15] + 
               m[4]  * m[11] * m[14] + 
               m[8]  * m[6]  * m[15] - 
               m[8]  * m[7]  * m[14] - 
               m[12] * m[6]  * m[11] + 
               m[12] * m[7]  * m[10];
  out[1][1] = m[0]  * m[10] * m[15] - 
              m[0]  * m[11] * m[14] - 
              m[8]  * m[2] * m[15] + 
              m[8]  * m[3] * m[14] + 
              m[12] * m[2] * m[11] - 
              m[12] * m[3] * m[10];
  out[1][2] = -m[0]  * m[6] * m[15] + 
              m[0]  * m[7] * m[14] + 
              m[4]  * m[2] * m[15] - 
              m[4]  * m[3] * m[14] - 
              m[12] * m[2] * m[7] + 
              m[12] * m[3] * m[6];
  out[1][3] = m[0] * m[6] * m[11] - 
             m[0] * m[7] * m[10] - 
             m[4] * m[2] * m[11] + 
             m[4] * m[3] * m[10] + 
             m[8] * m[2] * m[7] - 
             m[8] * m[3] * m[6];
  out[2][0] = m[4]  * m[9] * m[15] - 
             m[4]  * m[11] * m[13] - 
             m[8]  * m[5] * m[15] + 
             m[8]  * m[7] * m[13] + 
             m[12] * m[5] * m[11] - 
             m[12] * m[7] * m[9];
  out[2][1] = -m[0]  * m[9] * m[15] + 
              m[0]  * m[11] * m[13] + 
              m[8]  * m[1] * m[15] - 
              m[8]  * m[3] * m[13] - 
              m[12] * m[1] * m[11] + 
              m[12] * m[3] * m[9];
  out[2][2] = m[0]  * m[5] * m[15] - 
              m[0]  * m[7] * m[13] - 
              m[4]  * m[1] * m[15] + 
              m[4]  * m[3] * m[13] + 
              m[12] * m[1] * m[7] - 
              m[12] * m[3] * m[5];
  out[2][3] = -m[0] * m[5] * m[11] + 
               m[0] * m[7] * m[9] + 
               m[4] * m[1] * m[11] - 
               m[4] * m[3] * m[9] - 
               m[8] * m[1] * m[7] + 
               m[8] * m[3] * m[5];
  out[3][0] = -m[4]  * m[9] * m[14] + 
               m[4]  * m[10] * m[13] +
               m[8]  * m[5] * m[14] - 
               m[8]  * m[6] * m[13] - 
               m[12] * m[5] * m[10] + 
               m[12] * m[6] * m[9];
  out[3][1] = m[0]  * m[9] * m[14] - 
              m[0]  * m[10] * m[13] - 
              m[8]  * m[1] * m[14] + 
              m[8]  * m[2] * m[13] + 
              m[12] * m[1] * m[10] - 
              m[12] * m[2] * m[9];
  out[3][2] = -m[0]  * m[5] * m[14] + 
               m[0]  * m[6] * m[13] + 
               m[4]  * m[1] * m[14] - 
               m[4]  * m[2] * m[13] - 
               m[12] * m[1] * m[6] + 
               m[12] * m[2] * m[5];
  out[3][3] = m[0] * m[5] * m[10] - 
              m[0] * m[6] * m[9] - 
              m[4] * m[1] * m[10] + 
              m[4] * m[2] * m[9] + 
              m[8] * m[1] * m[6] - 
              m[8] * m[2] * m[5];

  float det = m[0] * out[0][0] + m[1] * out[1][0] + m[2] * out[2][0] + m[3] * out[3][0];
  //if (det == 0) return false;
  det = 1.0f / det;

  int i;
  float *o_m = &out[0][0]; // just so my brain doesn't explode
  for (i = 0; i < 16; i++) {
    o_m[i] = o_m[i] * det;
  }
  return out;
 }

void Mat4::print() {
  int i;
  float *m = &vecs[0][0];
  for (i = 0; i < 16; i++) {
    std::cout << "[" << i << "]=" << m[i] << " ";
    if (i == 3 || i == 7 || i == 11 || i == 15) std::cout << std::endl;
  }
  std::cout << std::endl;
}

Mat4 Mat4::operator*(const Mat4 &mat) {
  Vec4 x(
    vecs[0][0] * mat[0][0] + vecs[1][0] * mat[0][1] + vecs[2][0] * mat[0][2] + vecs[3][0] * mat[0][3],
    vecs[0][1] * mat[0][0] + vecs[1][1] * mat[0][1] + vecs[2][1] * mat[0][2] + vecs[3][1] * mat[0][3],
    vecs[0][2] * mat[0][0] + vecs[1][2] * mat[0][1] + vecs[2][2] * mat[0][2] + vecs[3][2] * mat[0][3],
    vecs[0][3] * mat[0][0] + vecs[1][3] * mat[0][1] + vecs[2][3] * mat[0][2] + vecs[3][3] * mat[0][3]
  );
  Vec4 y(
    vecs[0][0] * mat[1][0] + vecs[1][0] * mat[1][1] + vecs[2][0] * mat[1][2] + vecs[3][0] * mat[1][3],
    vecs[0][1] * mat[1][0] + vecs[1][1] * mat[1][1] + vecs[2][1] * mat[1][2] + vecs[3][1] * mat[1][3],
    vecs[0][2] * mat[1][0] + vecs[1][2] * mat[1][1] + vecs[2][2] * mat[1][2] + vecs[3][2] * mat[1][3],
    vecs[0][3] * mat[1][0] + vecs[1][3] * mat[1][1] + vecs[2][3] * mat[1][2] + vecs[3][3] * mat[1][3]
  );
  Vec4 z(
    vecs[0][0] * mat[2][0] + vecs[1][0] * mat[2][1] + vecs[2][0] * mat[2][2] + vecs[3][0] * mat[2][3],
    vecs[0][1] * mat[2][0] + vecs[1][1] * mat[2][1] + vecs[2][1] * mat[2][2] + vecs[3][1] * mat[2][3],
    vecs[0][2] * mat[2][0] + vecs[1][2] * mat[2][1] + vecs[2][2] * mat[2][2] + vecs[3][2] * mat[2][3],
    vecs[0][3] * mat[2][0] + vecs[1][3] * mat[2][1] + vecs[2][3] * mat[2][2] + vecs[3][3] * mat[2][3]
  );
  Vec4 w(
    vecs[0][0] * mat[3][0] + vecs[1][0] * mat[3][1] + vecs[2][0] * mat[3][2] + vecs[3][0] * mat[3][3],
    vecs[0][1] * mat[3][0] + vecs[1][1] * mat[3][1] + vecs[2][1] * mat[3][2] + vecs[3][1] * mat[3][3],
    vecs[0][2] * mat[3][0] + vecs[1][2] * mat[3][1] + vecs[2][2] * mat[3][2] + vecs[3][2] * mat[3][3],
    vecs[0][3] * mat[3][0] + vecs[1][3] * mat[3][1] + vecs[2][3] * mat[3][2] + vecs[3][3] * mat[3][3]
  );
  return Mat4(x, y, z, w);
}
Vec4 Mat4::operator*(const Vec4 &vec) {
  return Vec4(
    vecs[0][0] * vec[0] + vecs[1][0] * vec[1] + vecs[2][0] * vec[2] + vecs[3][0] * vec[3],
    vecs[0][1] * vec[0] + vecs[1][1] * vec[1] + vecs[2][1] * vec[2] + vecs[3][1] * vec[3],
    vecs[0][2] * vec[0] + vecs[1][2] * vec[1] + vecs[2][2] * vec[2] + vecs[3][2] * vec[3],
    vecs[0][3] * vec[0] + vecs[1][3] * vec[1] + vecs[2][3] * vec[2] + vecs[3][3] * vec[3]
  );
}
Mat4 Mat4::operator/=(float s) {
  vecs[0] /= s;
  vecs[1] /= s;
  vecs[2] /= s;
  vecs[3] /= s;
  return *this;
}
const Vec4& Mat4::operator[](int index) const {
  return vecs[index];
}
Vec4& Mat4::operator[](int index) {
  return vecs[index];
}