#include "matrix3d.hpp"
#include <cmath>

namespace engine::math {

void matrix3d::setFloatArray(const float32 m[3][3]) {
  this->_m[0][0] = m[0][0];
  this->_m[0][1] = m[0][1];
  this->_m[0][2] = m[0][2];

  this->_m[1][0] = m[1][0];
  this->_m[1][1] = m[1][1];
  this->_m[1][2] = m[1][2];
  this->_m[2][0] = m[2][0];
  this->_m[2][1] = m[2][1];
  this->_m[2][2] = m[2][2];
};

void matrix3d::setAll(float32 v) {
  this->_m[0][0] = v;
  this->_m[0][1] = v;
  this->_m[0][2] = v;

  this->_m[1][0] = v;
  this->_m[1][1] = v;
  this->_m[1][2] = v;

  this->_m[2][0] = v;
  this->_m[2][1] = v;
  this->_m[2][2] = v;
};

vector3d matrix3d::operator[](int i) {
  float32 *row = this->_m[i];
  return vector3d(*row, *(row + 1), *(row + 2));
};

const vector3d matrix3d::operator[](int i) const {
  return vector3d(this->_m[i][0], this->_m[i][1], this->_m[i][2]);
};

float32 matrix3d::operator()(int row, int column) {
  return this->_m[row][column];
};

float32 matrix3d::operator()(int row, int column) const {
  return this->_m[row][column];
};

// Default constructor all entries are 0
matrix3d::matrix3d() { this->setAll(0); };

// Default destructor just does nothing
matrix3d::~matrix3d(){};

// Constructor based on other matrix3d instance
matrix3d::matrix3d(const matrix3d &m){};

matrix3d::matrix3d(float32 a00, float32 a01, float32 a02, float32 a10,
                   float32 a11, float32 a12, float32 a20, float32 a21,
                   float32 a22) {
  this->_m[0][0] = a00;
  this->_m[0][1] = a01;
  this->_m[0][2] = a02;

  this->_m[1][0] = a10;
  this->_m[1][1] = a11;
  this->_m[1][2] = a12;

  this->_m[2][0] = a20;
  this->_m[2][1] = a21;
  this->_m[2][2] = a22;
};
// Constructor that sets matrix entries based in a two dimentional float32 3x3
// array
matrix3d::matrix3d(float32 m[3][3]) { this->setFloatArray(m); };

// matrix3 Constructor with 3 vector3d each representing a row
matrix3d::matrix3d(const vector3d r1, const vector3d r2, const vector3d r3) {
  this->_m[0][0] = r1[0];
  this->_m[0][1] = r1[1];
  this->_m[0][2] = r1[2];

  this->_m[1][0] = r2[0];
  this->_m[1][1] = r2[1];
  this->_m[1][2] = r2[2];

  this->_m[2][0] = r3[0];
  this->_m[2][1] = r3[1];
  this->_m[2][2] = r3[2];
};

std::string matrix3d::to_string() const {
  std::string str = "\n";
  str += "[" + std::to_string(this->_m[0][0]) + ", " +
         std::to_string(this->_m[0][1]) + ", " +
         std::to_string(this->_m[0][2]) + "]\n";
  str += "[" + std::to_string(this->_m[1][0]) + ", " +
         std::to_string(this->_m[1][1]) + ", " +
         std::to_string(this->_m[1][2]) + "]\n";
  str += "[" + std::to_string(this->_m[2][0]) + ", " +
         std::to_string(this->_m[2][1]) + ", " +
         std::to_string(this->_m[2][2]) + "]\n";
  return str;
}

// Overload of << operator. Usefull to combine this with ostream stream
std::ostream &operator<<(std::ostream &stream, matrix3d const &m) {
  std::string m_str = m.to_string();
  stream << m.to_string();
  return stream;
};

// Just builds an id matrix
matrix3d matrix3d::id() {
  float32 id[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
  return matrix3d(id);
};

matrix3d matrix3d::trans(const matrix3d &matrix) {
  return matrix3d(
      matrix(0, 0), matrix(1, 0), matrix(2, 0), // First column into first row
      matrix(0, 1), matrix(1, 1), matrix(2, 1), // Second column into second row
      matrix(0, 2), matrix(1, 2), matrix(2, 2)  // Third column into third row
  );
};

bool matrix3d::equals(const matrix3d &first, const matrix3d &second) {
  return first(0, 0) == second(0, 0) && first(0, 1) == second(0, 1) &&
         first(0, 2) == second(0, 2) && first(1, 0) == second(1, 0) &&
         first(1, 1) == second(1, 1) && first(1, 2) == second(1, 2) &&
         first(2, 0) == second(2, 0) && first(2, 1) == second(2, 1) &&
         first(2, 2) == second(2, 2);
}

float32 matrix3d::det(const matrix3d &m) {
  return m(0, 0) * DET(m(1, 1), m(1, 2), m(2, 1), m(2, 2)) -
         m(0, 1) * DET(m(1, 0), m(1, 2), m(2, 0), m(2, 2)) +
         m(0, 2) * DET(m(1, 0), m(1, 1), m(2, 0), m(2, 1));
};

matrix3d matrix3d::inv(const matrix3d &m) {
  // Rows as vectors
  const vector3d &a = m[0];
  const vector3d &b = m[1];
  const vector3d &c = m[2];

  // Cross product of rows
  vector3d r0 = vector3d::cross(b, c);
  vector3d r1 = vector3d::cross(c, a);
  vector3d r2 = vector3d::cross(a, b);

  // determinant inverse
  float32 idet = 1.0f / vector3d::dot(r2, c);

  return matrix3d(INVDET(r0, idet), INVDET(r1, idet), INVDET(r2, idet));
};

matrix3d matrix3d::rotX(float32 angle) {
  float32 _cos = cos(angle);
  float32 _sin = sin(angle);
  return matrix3d(1, 0, 0, 0, _cos, -_sin, 0, _sin, _cos);
};

matrix3d matrix3d::rotY(float32 angle) {
  float32 _cos = cos(angle);
  float32 _sin = sin(angle);

  return matrix3d(_cos, 0, _sin, 0, 1, 0, -_sin, 0, _cos);
};

matrix3d matrix3d::rotZ(float32 angle) {
  float32 _cos = cos(angle);
  float32 _sin = sin(angle);
  return matrix3d(_cos, -_sin, 0, _sin, _cos, 0, 0, 0, 1);
};

matrix3d matrix3d::rot(float32 angle, const vector3d &axis) {
  float32 _cos = cos(angle);
  float32 _sin = sin(angle);
  float32 d = 1 - _cos;

  float32 x = axis[0] * d;
  float32 y = axis[1] * d;
  float32 z = axis[2] * d;
  float32 ax_y = x * axis[1];
  float32 ax_z = x * axis[2];
  float32 ay_z = y * axis[2];

  return matrix3d(
      _cos + x * axis[0], ax_y - _sin * axis[2], ax_z + _sin * axis[1],
      ax_y + _sin * axis[2], _cos + y * axis[1], ay_z - _sin * axis[0],
      ax_z - _sin * axis[1], ay_z + _sin * axis[0], _cos + z * axis[2]);
};

matrix3d matrix3d::scale(float32 scalex, float32 scaley, float32 scalez) {
  return matrix3d(scalex, 0, 0, 0, scaley, 0, 0, 0, scalez);
};

matrix3d matrix3d::scale(const vector3d &v) {
  return matrix3d(v.x, 0, 0, 0, v.y, 0, 0, 0, v.z);
}

matrix3d matrix3d::scale(float32 scale, const vector3d &vector) {
  scale -= 1;
  float32 x = vector[0] * scale;
  float32 y = vector[1] * scale;
  float32 z = vector[2] * scale;
  float vx_vy = x * vector[1];
  float vx_vz = x * vector[2];
  float vy_vz = y * vector[2];
  return matrix3d(x * vector[0] + 1, vx_vy, vx_vz, vx_vy, y * vector[1] + 1,
                  vx_vz, vx_vz, vy_vz, z * vector[2] + 1);
};

matrix3d matrix3d::skew(float32 angle, const vector3d v1, const vector3d &v2) {
  float32 t = tan(angle);
  float32 x = v1[0];
  float32 y = v1[1];
  float32 z = v1[2];

  return matrix3d();
};

matrix3d matrix3d::reflect(const vector3d &v) {
  float32 x = v.x * -2.0;
  float32 y = v.y * -2.0;
  float32 z = v.z * -2.0;

  float32 vx_vy = x * v.y;
  float32 vx_vz = x * v.z;
  float32 vy_vz = y * v.z;

  return matrix3d(x * v.x + 1, vx_vy, vx_vz, vx_vy, y * v.y + 1, vy_vz, vx_vz,
                  vy_vz, z * v.z + 1);
};

matrix3d matrix3d::involution(const vector3d &v) {
  float32 x = v.x * 2.0;
  float32 y = v.y * 2.0;
  float32 z = v.z * 2.0;

  float32 vx_vy = x * v.y;
  float32 vx_vz = x * v.z;
  float32 vy_vz = y * v.z;

  return matrix3d(x * v.x - 1, vx_vy, vx_vz, vx_vy, y * v.y - 1, vy_vz, vx_vz,
                  vy_vz, z * v.z - 1);
};

// Overload of + operator. This will add 2 matrix3d instances
matrix3d operator+(const matrix3d &left, const matrix3d &right) {
  MATRIX(add, 3);
  add[0][0] = left[0][0] + right[0][0];
  add[0][1] = left[0][1] + right[0][1];
  add[0][2] = left[0][2] + right[0][2];

  add[1][0] = left[1][0] + right[1][0];
  add[1][1] = left[1][1] + right[1][1];
  add[1][2] = left[1][2] + right[1][2];

  add[2][0] = left[2][0] + right[2][0];
  add[2][1] = left[2][1] + right[2][1];
  add[2][2] = left[2][2] + right[2][2];

  return matrix3d(add);
};

matrix3d operator-(const matrix3d &left, const matrix3d &right) {
  MATRIX(m_temp, 3);
  m_temp[0][0] = left[0][0] - right[0][0];
  m_temp[0][1] = left[0][1] - right[0][1];
  m_temp[0][2] = left[0][2] - right[0][2];

  m_temp[1][0] = left[1][0] - right[1][0];
  m_temp[1][1] = left[1][1] - right[1][1];
  m_temp[1][2] = left[1][2] - right[1][2];

  m_temp[2][0] = left[2][0] - right[2][0];
  m_temp[2][1] = left[2][1] - right[2][1];
  m_temp[2][2] = left[2][2] - right[2][2];

  return matrix3d(m_temp);
};

// Multiplication of two 3d matrices
matrix3d operator*(const matrix3d &a, const matrix3d &b) {
  float32 m00 = a(0, 0) * b(0, 0) + a(0, 1) * b(1, 0) +
                a(0, 2) * b(2, 0); // a first row times b first column
  float32 m01 = a(0, 0) * b(0, 1) + a(0, 1) * b(1, 1) +
                a(0, 2) * b(2, 1); // a first row times b second column
  float32 m02 = a(0, 0) * b(0, 2) + a(0, 1) * b(1, 2) +
                a(0, 2) * b(2, 2); // a first row times b third column
  float32 m10 = a(1, 0) * b(0, 0) + a(1, 1) * b(1, 0) +
                a(1, 2) * b(2, 0); // a second row times b first column
  float32 m11 = a(1, 0) * b(0, 1) + a(1, 1) * b(1, 1) +
                a(1, 2) * b(2, 1); // a second row times b second column
  float32 m12 = a(1, 0) * b(0, 2) + a(1, 1) * b(1, 2) +
                a(1, 2) * b(2, 2); // a second row times b third column
  float32 m20 = a(2, 0) * b(0, 0) + a(2, 1) * b(1, 0) +
                a(2, 2) * b(2, 0); // a third row times b first column
  float32 m21 = a(2, 0) * b(0, 1) + a(2, 1) * b(1, 1) +
                a(2, 2) * b(2, 1); // a third column times b second column
  float32 m22 = a(2, 0) * b(0, 2) + a(2, 1) * b(1, 2) +
                a(2, 2) * b(2, 2); // a third column times b third column
  return matrix3d(m00, m01, m02, m10, m11, m12, m20, m21, m22);
};

matrix3d operator*(const matrix3d &matrix, const float32 scalar) {
  return matrix3d(
      matrix(0, 0) * scalar, matrix(0, 1) * scalar, matrix(0, 2) * scalar,
      matrix(1, 0) * scalar, matrix(1, 1) * scalar, matrix(1, 2) * scalar,
      matrix(2, 0) * scalar, matrix(2, 1) * scalar, matrix(2, 2) * scalar);
};

matrix3d operator*(const float scalar, const matrix3d &matrix) {
  return matrix * scalar;
}

vector3d operator*(const matrix3d &matrix, const vector3d &vector) {

  return vector3d(matrix(0, 0) * vector[0] + matrix(0, 1) * vector[1] +
                      matrix(0, 2) * vector[2], // First row times vector
                  matrix(1, 0) * vector[0] + matrix(1, 1) * vector[1] +
                      matrix(1, 2) * vector[2], // Second row times vector
                  matrix(2, 0) * vector[0] + matrix(2, 1) * vector[1] +
                      matrix(2, 2) * vector[2]); // Third row times vector
};

vector3d operator*(const vector3d &vector, const matrix3d &matrix) {
  return vector3d(vector[0] * matrix(0, 0) + vector[1] * matrix(1, 0) +
                      vector[2] * matrix(2, 0), // vector times first column
                  vector[0] * matrix(0, 1) + vector[1] * matrix(1, 1) +
                      vector[2] * matrix(2, 1), // vector times second column
                  vector[0] * matrix(0, 2) + vector[1] * matrix(1, 2) +
                      vector[2] * matrix(2, 2)); // vector times third column
}
}; // namespace engine::math