#include "../src/engine/math/matrix3d.hpp"
#include "../src/engine/math/point3d.hpp"
#include "../src/engine/math/vector3d.hpp"
#include <iostream>

using namespace engine::math;
using namespace std;

#define is_true(n) ((n == true) ? "yes" : "no")

int main() {
  vector3d v1 = vector3d(1, 2, 3);
  vector3d v2 = vector3d(3, 2, 1);
  vector3d v3 = vector3d(1, 1, 1);

  cout << v1 << endl;
  cout << v2 << endl;
  cout << (v1 - v2) + v3 << endl;
  cout << -v1 << endl;
  cout << vector3d::norm(v1) << endl;
  cout << vector3d::normalize(v1) << endl;
  cout << vector3d::norm(v3) << endl;
  cout << vector3d::normalize(v3) << endl;

  engine::math::matrix3d m = engine::math::matrix3d();
  engine::math::matrix3d id = matrix3d::id();
  cout << m << endl;
  cout << m[0][0] << endl;
  cout << id << endl;
  cout << id[0][0] << id[1][1] << id[2][2] << endl;
  cout << id(0, 0) << id(0, 1) << id(0, 2) << endl;

  matrix3d m1 = matrix3d(v1, v2, v3);
  matrix3d m2 = matrix3d(v3, v2, v1);

  cout << matrix3d::id() + matrix3d::id() << endl;
  cout << m1 * id << endl;
  cout << id * m1 << endl;
  cout << id * id << endl;
  cout << m1 * m2 << endl;
  cout << (id * 3.0f) * v1 << endl;
  cout << v1 * (id * 3.0f) << endl;

  matrix3d m3(1, 2, 3, 4, 5, 5, 7, 8, 9);

  cout << endl << m3 << endl;
  cout << endl << matrix3d::trans(m3) << endl;

  cout << "Are " << m1 << " and " << m2 << " equals? "
       << is_true(matrix3d::equals(m1, m2)) << endl;

  cout << "Are " << m2 << " and " << m2 << " equals? "
       << is_true(matrix3d::equals(m2, m2)) << endl;

  // Validate matrix properties
  cout << "Associative law for matrix add holds: "
       << is_true(matrix3d::equals((m1 + m2) + id, m1 + (m2 + id))) << endl;

  cout << "Comutative law for add holds: "
       << is_true(matrix3d::equals(m1 + m2, m2 + m1)) << endl;
  cout << "Associative for scalar multiplication: "
       << is_true(matrix3d::equals((2.0f * 2.0f) * m1, 2.0f * (2.0f * m1)))
       << endl;

  cout << "Comutative law for scalar multiplication: "
       << is_true(matrix3d::equals(2 * m1, m1 * 2)) << endl;
  cout << "Distributive law for scalar multiplication: "
       << is_true(matrix3d::equals((2 + 2) * m1, 2 * m1 + 2 * m1)) << endl;

  cout << "Associative for matrix multiplication: "
       << is_true(matrix3d::equals((m1 * m2) * m3, m1 * (m2 * m3))) << endl;

  cout << "Distributive laws for matrix: "
       << is_true(matrix3d::equals(m1 * (m2 + m3), m1 * m2 + m1 * m3))
       << " and "
       << is_true(matrix3d::equals((m1 + m2) * m3, m1 * m3 + m2 * m3)) << endl;

  cout << "Scalar factorization for matrices: "
       << is_true(matrix3d::equals((3 * m1) * m2, m1 * (3 * m2))) << " and "
       << is_true(matrix3d::equals((3 * m1) * m2, 3 * (m1 * m2))) << endl;

  cout << "Product rule for matrix trans: "
       << is_true(matrix3d::equals(matrix3d::trans(m1 * m2),
                                   matrix3d::trans(m2) * matrix3d::trans(m1)))
       << endl;

  cout << "Dot product comutative law: "
       << is_true(vector3d::dot(v1, v2) == vector3d::dot(v2, v1)) << endl;
  cout << "Dot product distributive law: "
       << is_true(vector3d::dot(v1, (v2 + v3)) ==
                  (vector3d::dot(v1, v2) + vector3d::dot(v1, v3)))
       << endl;

  cout << "Dot scalar factorization: "
       << is_true(vector3d::dot(2.0 * v1, v2) == vector3d::dot(v1, 2 * v2))
       << " and "
       << is_true(vector3d::dot(v1, 2 * v2) == 2 * vector3d::dot(v1, v2))
       << endl;

  cout << "Cross anticomutativity: "
       << is_true(vector3d::equals(vector3d::cross(v1, v2),
                                   -vector3d::cross(v2, v1)))
       << endl;

  cout << "Cross distributive law: "
       << is_true(vector3d::equals(vector3d::cross(v1, v2 + v3),
                                   vector3d::cross(v1, v2) +
                                       vector3d::cross(v1, v3)))
       << endl;

  cout << "Scalar factorization in cross: "
       << is_true(vector3d::equals(vector3d::cross(2 * v1, v2),
                                   vector3d::cross(v1, 2 * v2)))
       << " and "
       << is_true(vector3d::equals(vector3d::cross(v1, 2 * v2),
                                   2 * vector3d::cross(v1, v2)))
       << endl;

  cout << "Vector triple product: "
       << is_true(vector3d::equals(vector3d::cross(v1, vector3d::cross(v2, v3)),
                                   v2 * vector3d::dot(v1, v3) -
                                       v3 * vector3d::dot(v1, v2)))
       << endl;

  // Check lagrande id:
  // This means:
  // dot(cross(v1,v2),cross(v1,v2)) = dot(v1,v1)*dot(v2,v2) -
  // dot(v1,v2)*dot(v1,v2)
  cout << "Lagrange id: "
       << is_true(
              vector3d::dot(vector3d::cross(v1, v2), vector3d::cross(v1, v2)) ==
              (vector3d::dot(v1, v1) * vector3d::dot(v2, v2) -
               (vector3d::dot(v1, v2) * vector3d::dot(v1, v2))))
       << endl;

  // Projections
  cout << "Project [1,1,0] in [1,0,0]: "
       << vector3d::project(vector3d(1, 1, 0), vector3d(1, 0, 0)) << endl;

  cout << "Reject [1,1,0] over [1,0,0]: "
       << vector3d::reject(vector3d(1, 1, 0), vector3d(1, 0, 0)) << endl;

  // Checking determinant properties
  cout << "Det of identity matrix should be 1: "
       << is_true(matrix3d::det(id) == 1) << endl;

  cout << "Det(m^T) should be the same as Det(m): "
       << is_true(matrix3d::det(m3) == matrix3d::det(matrix3d::trans(m3)))
       << endl;

  matrix3d m4 = matrix3d(4, -1, 1, 4, 5, 3, -2, 0, 0);
  matrix3d m4_inv = matrix3d::inv(m4);
  float32 dm4 = matrix3d::det(m4);
  float32 dm4_inv = matrix3d::det(m4_inv);
  float inv_dm4 = 1.0f / dm4;
  float32 pdet = dm4 * dm4_inv;

  cout << "Determinant of inverse should be the inverse of the determinant: "
       << is_true(dm4_inv == inv_dm4) << endl;

  cout << "Determinant of product should be the product of determinants: "
       << is_true(matrix3d::det(m4 * m4_inv) ==
                  matrix3d::det(m4) * matrix3d::det(m4_inv))
       << endl;

  cout << id.to_string() << endl;

  point3d p1 = point3d(0, 1, 0);
  point3d p2 = point3d(1, 0, 0);
  point3d p3 = point3d(0, 0, 1);
  point3d p4 = p1 + v1;
  cout << v1 << endl;
  cout << point3d() - p4 << endl;
}