#include <string>
#include <math.h>
#include "vector.hh"
#include "diagonal_matrix.hh"

std::string bool_to_str(const bool& x) {
  std::string s = "FAIL";
  if (x) 
    s = "PASS";
  return s;
}

/* Check if two vectors are identical */
template <class V>
bool compare_vectors(const V& v,
                     const V& w) {
  std::vector<double> v_vec=v.as_vector();
  std::vector<double> w_vec=w.as_vector();
  double tolerance = 1.E-12;
  bool identical=true;
  for (int i=0;i<v.size();++i) {
    identical = identical and (fabs(v_vec[i]-w_vec[i])< tolerance);
  }
  return identical;
}

/* Check if two diagonal matrices are identical */
template <class M>
bool compare_diagonal_matrices(const M& m1,
                               const M& m2) {
  std::vector<double> m1_vec=m1.as_vector();
  std::vector<double> m2_vec=m2.as_vector();
  double tolerance = 1.E-12;
  bool identical=true;
  for (int i=0;i<m1_vec.size();++i) {
    identical = identical and (fabs(m1_vec[i]-m2_vec[i])< tolerance);
  }
  return identical;
}

/* Test += operator */
bool test_plus_equals() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,1.0});
  Vector<2> v3({4.0,3.0});
  v1+=v2;
  return compare_vectors(v1,v3);
}

/* Test vector + vector operator */
bool test_plus() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,1.0});
  Vector<2> v3({4.0,3.0});
  Vector<2> v4 = v1+v2;
  return compare_vectors(v3,v4);
}

/* Test vector + double operator */
bool test_vector_plus_double() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,4.0});
  Vector<2> v3 = v1+2.0;
  return compare_vectors(v2,v3);
}

/* Test double + vector operator */
bool test_double_plus_vector() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,4.0});
  Vector<2> v3 = 2.0+v1;
  return compare_vectors(v2,v3);
}

/* Test -= operator */
bool test_minus_equals() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,1.0});
  Vector<2> v3({-2.0,1.0});
  v1-=v2;
  return compare_vectors(v1,v3);
}

/* Test vector - vector operator */
bool test_minus() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,1.0});
  Vector<2> v3({-2.0,1.0});
  Vector<2> v4 = v1-v2;
  return compare_vectors(v3,v4);
}

/* Test vector - double operator */
bool test_vector_minus_double() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({-1.0,0.0});
  Vector<2> v3 = v1-2.0;
  return compare_vectors(v2,v3);
}

/* Test double - vector operator */
bool test_double_minus_vector() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({1.0,0.0});
  Vector<2> v3 = 2.0-v1;
  return compare_vectors(v2,v3);
}

/* Test *= operator */
bool test_multiply_equals() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,6.0});
  v1*=3.0;
  return compare_vectors(v1,v2);
}

/* Test double*vector operator */
bool test_double_multiply_vector() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,6.0});
  Vector<2> v3 = 3.0*v1;
  return compare_vectors(v2,v3);
}

/* Test vector*double operator */
bool test_vector_multiply_double() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,6.0});
  Vector<2> v3 = v1*3.0;
  return compare_vectors(v2,v3);
}

/* Test /= operator*/
bool test_divide_equals() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({0.5,1.0});
  v1/=2.0;
  return compare_vectors(v1,v2);
}

/* Test vector/double operator */
bool test_vector_divide_double() {
  Vector<2> v1({1.0,4.0});
  Vector<2> v2({0.5,2.0});
  Vector<2> v3=v1/2.0;
  return compare_vectors(v2,v3);
}

/* Test = operator */
bool test_equals() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,1.0});
  v1=v2;
  return compare_vectors(v1,v2);
}

/* Test dot product */
bool test_dot_product() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,1.0});
  double tolerance = 1.E-12;
  double dot_v1v2 = 5.0;
  return (fabs(dot_v1v2-v1.dot(v2)) < tolerance);
}

/* Test two norm */
bool test_two_norm() {
  Vector<2> v1({1.0,2.0});
  double tolerance = 1.E-12;
  double two_norm_v1 = sqrt(5.0);
  return (fabs(two_norm_v1-v1.two_norm()) < tolerance);
}

/* Test linear combination */
bool test_linear() {
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({3.0,1.0});
  Vector<2> v3({9.0,8.0});
  Vector<2> v4 = 3.0*v1 + 2.0*v2;
  return compare_vectors(v3,v4);
}

/* Test matrix addition */
bool test_matrix_addition() {
  DiagonalMatrix<2> m1({1.0,1.0});
  DiagonalMatrix<2> m2({1.0,2.0});
  DiagonalMatrix<2> m3({2.0,3.0});
  m1+=m2;
  return compare_diagonal_matrices(m1,m3);
}

/* Test matrix + matrix operator */
bool test_matrix_plus() {
  DiagonalMatrix<2> m1({1.0,1.0});
  DiagonalMatrix<2> m2({1.0,2.0});
  DiagonalMatrix<2> m3({2.0,3.0});
  DiagonalMatrix<2> m4=m1+m2;
  return compare_diagonal_matrices(m3,m4);
}

/* Test matrix + double operator */
bool test_matrix_plus_double() {
  DiagonalMatrix<2> m1({1.0,2.0});
  DiagonalMatrix<2> m2({4.5,5.5});
  DiagonalMatrix<2> m3=m1+3.5;
  return compare_diagonal_matrices(m2,m3);
}

/* Test double + matrix operator */
bool test_double_plus_matrix() {
  DiagonalMatrix<2> m1({1.0,2.0});
  DiagonalMatrix<2> m2({4.5,5.5});
  DiagonalMatrix<2> m3=3.5+m1;
  return compare_diagonal_matrices(m2,m3);
}

/* Test matrix subtraction */
bool test_matrix_subtraction() {
  DiagonalMatrix<2> m1({1.0,1.0});
  DiagonalMatrix<2> m2({1.0,2.0});
  DiagonalMatrix<2> m3({0.0,-1.0});
  m1-=m2;
  return compare_diagonal_matrices(m1,m3);
}

/* Test matrix - matrix operator */
bool test_matrix_minus() {
  DiagonalMatrix<2> m1({1.0,1.0});
  DiagonalMatrix<2> m2({1.0,2.0});
  DiagonalMatrix<2> m3({0.0,-1.0});
  DiagonalMatrix<2> m4=m1-m2;
  return compare_diagonal_matrices(m3,m4);
}

/* Test matrix - double operator */
bool test_matrix_minus_double() {
  DiagonalMatrix<2> m1({1.0,2.0});
  DiagonalMatrix<2> m2({-2.5,-1.5});
  DiagonalMatrix<2> m3=m1-3.5;
  return compare_diagonal_matrices(m2,m3);
}

/* Test double - matrix operator */
bool test_double_minus_matrix() {
  DiagonalMatrix<2> m1({1.0,2.0});
  DiagonalMatrix<2> m2({2.5,1.5});
  DiagonalMatrix<2> m3=3.5-m1;
  return compare_diagonal_matrices(m2,m3);
}

/* Test matrix*vector operator */
bool test_matrix_vector_multiplication() {
  DiagonalMatrix<2> m1({1.0,1.0});
  Vector<2> v1({1.0,2.0});
  Vector<2> v2({1.0,2.0});
  Vector<2> v3=m1*v1;  
  return compare_vectors(v2,v3);
}

/* Test matrix *= operator*/
bool test_matrix_multiply_equals() {
  DiagonalMatrix<2> m1({1.0,2.0});
  DiagonalMatrix<2> m2({2.0,4.0});
  m1*=2.0;
  return compare_diagonal_matrices(m1,m2);
}

/* Test double*matrix operator */
bool test_double_multiply_matrix() {
  DiagonalMatrix<2> m1({1.0,2.0});
  DiagonalMatrix<2> m2({3.0,6.0});
  DiagonalMatrix<2> m3=3.0*m1;
  return compare_diagonal_matrices(m2,m3);
}

/* Test matrix*double operator */
bool test_matrix_multiply_double() {
  DiagonalMatrix<2> m1({1.0,2.0});
  DiagonalMatrix<2> m2({3.0,6.0});
  DiagonalMatrix<2> m3=m1*3.0;
  return compare_diagonal_matrices(m2,m3);
}

/* Test matrix /= operator*/
bool test_matrix_divide_equals() {
  DiagonalMatrix<2> m1({1.0,2.0});
  DiagonalMatrix<2> m2({0.5,1.0});
  m1/=2.0;
  return compare_diagonal_matrices(m1,m2);
}

/* Test matrix/double operator */
bool test_matrix_divide_double() {
  DiagonalMatrix<2> m1({1.0,4.0});
  DiagonalMatrix<2> m2({0.5,2.0});
  DiagonalMatrix<2> m3=m1/2.0;
  return compare_diagonal_matrices(m2,m3);
}

/* Test matrix = operator */
bool test_matrix_equals() {
  DiagonalMatrix<2> m1({1.0,2.0});
  DiagonalMatrix<2> m2({3.0,1.0});
  m1=m2;
  return compare_diagonal_matrices(m1,m2);
}

int main(int argc, char* argv[]) {
  std::cout << "Testing operator += : "
            << bool_to_str(test_plus_equals()) 
            << std::endl;

  std::cout << "Testing operator +  (vector + vector): "
            << bool_to_str(test_plus())
            << std::endl;

  std::cout << "Testing operator +  (vector + double): "
            << bool_to_str(test_vector_plus_double())
            << std::endl;

  std::cout << "Testing operator +  (double + vector): "
            << bool_to_str(test_double_plus_vector())
            << std::endl;

  std::cout << "Testing operator -= : "
            << bool_to_str(test_minus_equals())
            << std::endl;

  std::cout << "Testing operator -  (vector - vector): "
            << bool_to_str(test_minus())
            << std::endl;

  std::cout << "Testing operator -  (vector - double): "
            << bool_to_str(test_vector_minus_double())
            << std::endl;

  std::cout << "Testing operator -  (double - vector): "
            << bool_to_str(test_double_minus_vector())
            << std::endl;

  std::cout << "Testing operator *= : "
            << bool_to_str(test_multiply_equals())
            << std::endl;

  std::cout << "Testing operator *  (double * vector): "
            << bool_to_str(test_double_multiply_vector())
            << std::endl;

  std::cout << "Testing operator *  (vector * double): "
            << bool_to_str(test_vector_multiply_double())
            << std::endl;

  std::cout << "Testing operator /= : "
            << bool_to_str(test_divide_equals())
            << std::endl;

  std::cout << "Testing operator /  (vector / double): "
            << bool_to_str(test_vector_divide_double())
            << std::endl;

  std::cout << "Testing operator  = : "
            << bool_to_str(test_equals())
            << std::endl;

  std::cout << "Testing dot product : "
            << bool_to_str(test_dot_product())
            << std::endl;

  std::cout << "Testing two norm    : "
            << bool_to_str(test_two_norm())
            << std::endl;

  std::cout << "Testing linear combination : "
            << bool_to_str(test_linear())
            << std::endl;

  std::cout << "Testing matrix += operator : "
            << bool_to_str(test_matrix_addition())
            << std::endl;

  std::cout << "Testing matrix + matrix : "
            << bool_to_str(test_matrix_plus())
            << std::endl;

  std::cout << "Testing matrix + double : "
            << bool_to_str(test_matrix_plus_double())
            << std::endl;

  std::cout << "Testing double + matrix : "
            << bool_to_str(test_double_plus_matrix())
            << std::endl;

  std::cout << "Testing matrix -= operator : "
            << bool_to_str(test_matrix_subtraction())
            << std::endl;

  std::cout << "Testing matrix - matrix : "
            << bool_to_str(test_matrix_minus())
            << std::endl;

  std::cout << "Testing matrix - double : "
            << bool_to_str(test_matrix_minus_double())
            << std::endl;

  std::cout << "Testing double - matrix : "
            << bool_to_str(test_double_minus_matrix())
            << std::endl;

  std::cout << "Testing matrix * vector operator : "
            << bool_to_str(test_matrix_vector_multiplication())
            << std::endl;

  std::cout << "Testing matrix *= operator : "
            << bool_to_str(test_matrix_multiply_equals())
            << std::endl;

  std::cout << "Testing double * matrix operator : "
            << bool_to_str(test_double_multiply_matrix())
            << std::endl;

  std::cout << "Testing matrix * double operator : "
            << bool_to_str(test_matrix_multiply_double())
            << std::endl;

  std::cout << "Testing matrix /= operator : "
            << bool_to_str(test_matrix_divide_equals())
            << std::endl;

  std::cout << "Testing matrix / double operator : "
            << bool_to_str(test_matrix_divide_double())
            << std::endl;

 std::cout << "Testing matrix operator  = : "
            << bool_to_str(test_matrix_equals())
            << std::endl;
}