#include "gtest/gtest.h"
#include "pimathmatrix.h"

const uint rows = 3;
const uint cols = 3;

bool cmpSquareMatrixWithValue(PIMathMatrixT<rows, cols, double> matrix, double val, int num) {
	for(int i = 0; i < num; i++) {
		for(int j = 0; j < num; j++) {
			if(matrix.at(i, j) != val) {
				return false;
			}
		}
	}
	return true;
}

TEST(PIMathMatrixT_Test, identity) {
	auto matrix = PIMathMatrixT<rows, cols, double>::identity();
	for(int i = 0; i < 3; i++){
		for(int j = 0; j < 3; j++){
			if(i != j){
				if(matrix[i][j] != 0.0){
					ASSERT_TRUE(false);
				}
			}
			else {
				if(matrix[i][i] != 1.0){
					ASSERT_TRUE(false);
				}
			}
		}
	}
	ASSERT_TRUE(true);
}

TEST(PIMathMatrixT_Test, at) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>::identity();
	for(uint i = 0; i < rows; i++) {
		if(matrix1.at(i,i) != 1.0) {
			ASSERT_TRUE(false);
		}
	}
	ASSERT_TRUE(true);
}
TEST(PIMathMatrixT_Test, filled) {
	auto matr = PIMathMatrixT<rows, cols, double>(1.0);
	ASSERT_TRUE(cmpSquareMatrixWithValue(matr, 1.0, rows));
}

TEST(PIMathMatrixT_Test, cols) {
	PIMathMatrixT<rows, cols, double> matr;
	ASSERT_EQ(cols,matr.cols());
}

TEST(PIMathMatrixT_Test, rows) {
	PIMathMatrixT<rows, cols, double> matr;
	ASSERT_EQ(rows,matr.rows());
}

TEST(PIMathMatrixT_Test, col) {
	PIMathMatrixT<rows, cols, double> matr;
	PIMathVectorT<rows, double> vect;
	uint g = 2;
	matr.element(0,0) = 3;
	matr.element(0,1) = 6;
	matr.element(0,2) = 8;
	matr.element(1,0) = 2;
	matr.element(1,1) = 1;
	matr.element(1,2) = 4;
	matr.element(2,0) = 6;
	matr.element(2,1) = 2;
	matr.element(2,2) = 5;
	vect = matr.col(g);
	for(uint i = 0; i < matr.cols(); i++) {
		if(matr.element(i, g) != vect[i]) {
			ASSERT_TRUE(false);
		}
	}
	ASSERT_TRUE(true);
}

TEST(PIMathMatrixT_Test, row) {
	PIMathMatrixT<rows, cols, double> matr;
	PIMathVectorT<rows, double> vect;
	uint g = 2;
	matr.element(0,0) = 3;
	matr.element(0,1) = 6;
	matr.element(0,2) = 8;
	matr.element(1,0) = 2;
	matr.element(1,1) = 1;
	matr.element(1,2) = 4;
	matr.element(2,0) = 6;
	matr.element(2,1) = 2;
	matr.element(2,2) = 5;
	vect = matr.row(g);
	for(uint i = 0; i < matr.rows(); i++) {
		if(matr.element(g, i) != vect[i]) {
			ASSERT_TRUE(false);
		}
	}
	ASSERT_TRUE(true);
}

TEST(PIMathMatrixT_Test, setCol) {
	PIMathMatrixT<rows, cols, double> matr;
	PIMathVectorT<rows, double> vect;
	vect[0] = 1.0;
	vect[1] = 3.0;
	vect[2] = 5.0;
	uint g = 1;
	matr.setCol(g, vect);
	for(uint i = 0; i < vect.size(); i++) {
		if(matr.element(i, g) != vect[i]) {
			ASSERT_TRUE(false);
		}
	}
	ASSERT_TRUE(true);
}

TEST(PIMathMatrixT_Test, setRow) {
	PIMathMatrixT<rows, cols, double> matr;
	PIMathVectorT<rows, double> vect;
	vect[0] = 1.0;
	vect[1] = 3.0;
	vect[2] = 5.0;
	uint g = 1;
	matr.setRow(g, vect);
	for(uint i = 0; i < vect.size(); i++) {
		if(matr.element(g,i) != vect[i]) {
			ASSERT_TRUE(false);
		}
	}
	ASSERT_TRUE(true);
}

TEST(PIMathMatrixT_Test, swapCols) {
	PIMathMatrixT<rows, cols, double> matr;
	int g1 = 1, g2 = 2;
	matr.element(0,0) = 3;
	matr.element(0,1) = 6;
	matr.element(0,2) = 8;
	matr.element(1,0) = 2;
	matr.element(1,1) = 1;
	matr.element(1,2) = 4;
	matr.element(2,0) = 6;
	matr.element(2,1) = 2;
	matr.element(2,2) = 5;
	const PIMathVectorT<rows, double> before_Vect1 = matr.col(g1);
	const PIMathVectorT<rows, double> before_Vect2 = matr.col(g2);
	matr.swapCols(g1, g2);
	const PIMathVectorT<rows, double> after_Vect1 = matr.col(g1);
	const PIMathVectorT<rows, double> after_Vect2 = matr.col(g2);
	if((before_Vect1 == after_Vect2) && (before_Vect2 == after_Vect1)) {
		ASSERT_TRUE(true);
	}
	else {
		ASSERT_TRUE(false);
	}
}

TEST(PIMathMatrixT_Test, swapRows) {
	PIMathMatrixT<rows, cols, double> matr;
	int g1 = 1, g2 = 2;
	matr.element(0,0) = 3;
	matr.element(0,1) = 6;
	matr.element(0,2) = 8;
	matr.element(1,0) = 2;
	matr.element(1,1) = 1;
	matr.element(1,2) = 4;
	matr.element(2,0) = 6;
	matr.element(2,1) = 2;
	matr.element(2,2) = 5;
	const PIMathVectorT<rows, double> before_Vect1 = matr.row(g1);
	const PIMathVectorT<rows, double> before_Vect2 = matr.row(g2);
	matr.swapRows(g1, g2);
	const PIMathVectorT<rows, double> after_Vect1 = matr.row(g1);
	const PIMathVectorT<rows, double> after_Vect2 = matr.row(g2);
	if((before_Vect1 == after_Vect2) && (before_Vect2 == after_Vect1)) {
		ASSERT_TRUE(true);
	}
	else {
		ASSERT_TRUE(false);
	}
}

TEST(PIMathMatrixT_Test, fill) {
	PIMathMatrixT<rows, cols, double> matr;
	PIMathMatrixT<rows, cols, double> matrix1;
	double g = 1.0;
	matr.fill(g);
	for(uint i = 0; i < cols; i++) {
		for(uint j = 0; j < rows; j++) {
			matrix1.element(j,i) = g;
		}
	}
	ASSERT_TRUE(matr == matrix1);
}

TEST(PIMathMatrixT_Test, isSquareTrue) {
	PIMathMatrixT<rows, cols, double> matrix1;
	ASSERT_TRUE(matrix1.isSquare());
}

TEST(PIMathMatrixT_Test, isSquareFalse) {
	const uint new_Cols = 4;
	PIMathMatrixT<rows, new_Cols, double> matrix2;
	ASSERT_FALSE(matrix2.isSquare());
}

TEST(PIMathMatrixT_Test, isIdentityTrue) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>::identity();
	ASSERT_TRUE(matrix1.isIdentity());
}

TEST(PIMathMatrixT_Test, isIdentityFalse) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(2.5);
	ASSERT_FALSE(matrix1.isIdentity());
}

TEST(PIMathMatrixT_Test, isNullTrue) {
	PIMathMatrixT<rows, cols, double> matrix1;
	ASSERT_TRUE(matrix1.isNull());
}

TEST(PIMathMatrixT_Test, isNullFalse) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>::identity();
	ASSERT_FALSE(matrix1.isNull());
}

TEST(PIMathMatrixT_Test, operator_Assignment) {
	PIMathMatrixT<rows, cols, double> matrix1;
	auto matrix2 = PIMathMatrixT<rows, cols, double>(6.72);
	matrix1 = matrix2;
	ASSERT_TRUE(cmpSquareMatrixWithValue(matrix1, 6.72, rows));
}

TEST(PIMathMatrixT_Test, operator_EqualTrue) {
	PIMathMatrixT<rows, cols, double> matrix1;
	PIMathMatrixT<rows, cols, double> matrix2;
	matrix1.element(0, 0) = 5.1;
	matrix1.element(0, 1) = 1.21;
	matrix1.element(1, 1) = 0.671;
	matrix1.element(1, 0) = 2.623;
	matrix2.element(0, 0) = 5.1;
	matrix2.element(0, 1) = 1.21;
	matrix2.element(1, 1) = 0.671;
	matrix2.element(1, 0) = 2.623;
	ASSERT_TRUE(matrix1 == matrix2);
}

TEST(PIMathMatrixT_Test, operator_EqualFalse) {
	PIMathMatrixT<rows, cols, double> matrix1;
	PIMathMatrixT<rows, cols, double> matrix2;
	matrix1.element(0, 0) = 5.1;
	matrix1.element(0, 1) = 1.21;
	matrix1.element(1, 1) = 0.671;
	matrix1.element(1, 0) = 2.623;
	matrix2.element(0, 0) = 5.1;
	matrix2.element(0, 1) = 1.21;
	matrix2.element(1, 1) = 665.671;
	matrix2.element(1, 0) = 2.623;
	ASSERT_FALSE(matrix1 == matrix2);
}

TEST(PIMathMatrixT_Test, operator_Not_EqualTrue) {
	PIMathMatrixT<rows, cols, double> matrix1;
	PIMathMatrixT<rows, cols, double> matrix2;
	matrix1.element(0, 0) = 5.1;
	matrix1.element(0, 1) = 1.21;
	matrix1.element(1, 1) = 0.671;
	matrix1.element(1, 0) = 2.623;
	matrix2.element(0, 0) = 5.1;
	matrix2.element(0, 1) = 1.21;
	matrix2.element(1, 1) = 665.671;
	matrix2.element(1, 0) = 2.623;
	ASSERT_TRUE(matrix1 != matrix2);
}

TEST(PIMathMatrixT_Test, operator_Not_EqualFalse) {
	PIMathMatrixT<rows, cols, double> matrix1;
	PIMathMatrixT<rows, cols, double> matrix2;
	matrix1.element(0, 0) = 5.1;
	matrix1.element(0, 1) = 1.21;
	matrix1.element(1, 1) = 0.671;
	matrix1.element(1, 0) = 2.623;
	matrix2.element(0, 0) = 5.1;
	matrix2.element(0, 1) = 1.21;
	matrix2.element(1, 1) = 0.671;
	matrix2.element(1, 0) = 2.623;
	ASSERT_FALSE(matrix1 != matrix2);
}

TEST(PIMathMatrixT_Test, operator_Addition_Assignment) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(6.72) ;
	auto matrix2 = PIMathMatrixT<rows, cols, double>(1.0) ;
	matrix1 += matrix2;
	ASSERT_TRUE(cmpSquareMatrixWithValue(matrix1, 7.72, rows));
}

TEST(PIMathMatrixT_Test, operator_Subtraction_Assignment) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(1.0);
	auto matrix2 = PIMathMatrixT<rows, cols, double>(6.72);
	matrix1 -= matrix2;
	ASSERT_TRUE(cmpSquareMatrixWithValue(matrix1, -5.72, rows));
}

TEST(PIMathMatrixT_Test, operator_Multiplication_Assignment) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(6.72);
	matrix1 *= 2.0;
	ASSERT_TRUE(cmpSquareMatrixWithValue(matrix1, 13.44, rows));
}

TEST(PIMathMatrixT_Test, operator_Division_Assignment) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(6.72);
	matrix1 /= 2.0;
	ASSERT_TRUE(cmpSquareMatrixWithValue(matrix1, 3.36, rows));
}

TEST(PIMathMatrixT_Test, operator_Addition) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(6.72);
	auto matrix2 = PIMathMatrixT<rows, cols, double>(8.28);
	ASSERT_TRUE(cmpSquareMatrixWithValue(matrix1 + matrix2, 15.0, rows));
}

TEST(PIMathMatrixT_Test, operator_Subtraction) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(6.0);
	auto matrix2 = PIMathMatrixT<rows, cols, double>(5.0);
	ASSERT_TRUE(cmpSquareMatrixWithValue(matrix1 - matrix2, 1.0, rows));
}

TEST(PIMathMatrixT_Test, operator_Multiplication) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(6.72);
	ASSERT_TRUE(cmpSquareMatrixWithValue(matrix1 * 4.0, 26.88, rows));
}
TEST(PIMathMatrixT_Test, operator_Division) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(6.72);
	ASSERT_TRUE(cmpSquareMatrixWithValue(matrix1 / 4.0, 1.68, rows));
}

TEST(PIMathMatrixT_Test, determinantIfSquare) {
	double d;
	double i = 59.0;
	PIMathMatrixT<rows, cols, double> matr;
	matr.element(0,0) = 3;
	matr.element(0,1) = 6;
	matr.element(0,2) = 8;
	matr.element(1,0) = 2;
	matr.element(1,1) = 1;
	matr.element(1,2) = 4;
	matr.element(2,0) = 6;
	matr.element(2,1) = 2;
	matr.element(2,2) = 5;
	d = matr.determinant();
	ASSERT_DOUBLE_EQ(i, d);
}

TEST(PIMathMatrixT_Test, invert) {
	PIMathMatrixT<rows, cols, double> matrix1;
	PIMathMatrixT<rows, cols, double> matrix2;
	PIMathMatrixT<rows, cols, double> matrix3;
	PIMathMatrixT<rows, cols, double> matr;
	double d1, d2;
	matr.element(0,0) = 3;
	matr.element(0,1) = 6;
	matr.element(0,2) = 8;
	matr.element(1,0) = 2;
	matr.element(1,1) = 1;
	matr.element(1,2) = 4;
	matr.element(2,0) = 6;
	matr.element(2,1) = 2;
	matr.element(2,2) = 5;
	matrix2 = matr;
	matr.invert();
	d1 = matr.determinant();
	d2 = matrix2.determinant();
	matrix3 = matrix1;
	matrix1.invert();
	ASSERT_TRUE((matrix1 == matrix3) && piCompare(d1, 1/d2));
}

TEST(PIMathMatrixT_Test, inverted) {
	PIMathMatrixT<rows, cols, double> matrix1;
	PIMathMatrixT<rows, cols, double> matrix2;
	PIMathMatrixT<rows, cols, double> matrix3;
	PIMathMatrixT<rows, cols, double> matr;
	double d1, d2;
	matrix1 = matr.identity();
	matr.element(0,0) = 3;
	matr.element(0,1) = 6;
	matr.element(0,2) = 8;
	matr.element(1,0) = 2;
	matr.element(1,1) = 1;
	matr.element(1,2) = 4;
	matr.element(2,0) = 6;
	matr.element(2,1) = 2;
	matr.element(2,2) = 5;
	matrix2 = matr.inverted();
	d1 = matr.determinant();
	d2 = matrix2.determinant();
	matrix3 = matrix1.inverted();
	ASSERT_TRUE((matrix1 == matrix3) && (round((1/d1)*10000)/10000 == round(d2*10000)/10000));
}

TEST(PIMathMatrixT_Test, toUpperTriangular) {
	PIMathMatrixT<rows, cols, double> matrix;
	double d1, d2 = 1;
	PIMathMatrixT<rows, cols, double> matr;
	matr.element(0,0) = 3;
	matr.element(0,1) = 6;
	matr.element(0,2) = 8;
	matr.element(1,0) = 2;
	matr.element(1,1) = 1;
	matr.element(1,2) = 4;
	matr.element(2,0) = 6;
	matr.element(2,1) = 2;
	matr.element(2,2) = 5;
	matrix = matr.toUpperTriangular();
	d1 = matrix.determinant();
	for(uint i = 0; i < cols; i++)
	{
		d2 = d2*matrix.at(i,i);
	}
	ASSERT_DOUBLE_EQ(d1, d2);
}

TEST(PIMathMatrixT_Test, transposed) {
	PIMathMatrixT<rows, cols, double> matrix1;
	PIMathMatrixT<rows, cols, double> matrix2;
	PIMathMatrixT<rows, cols, double> matr;
	double d1, d2;
	matr.element(0,0) = 3;
	matr.element(0,1) = 6;
	matr.element(0,2) = 8;
	matr.element(1,0) = 2;
	matr.element(1,1) = 1;
	matr.element(1,2) = 4;
	matr.element(2,0) = 6;
	matr.element(2,1) = 2;
	matr.element(2,2) = 5;
	d1 = matr.determinant();
	matrix1 = matr.transposed();
	d2 = matrix1.determinant();
	matrix2 = matrix1.transposed();
	ASSERT_TRUE((d1 == d2) && (matr == matrix2));
}

TEST(PIMathMatrixT_Test, rotation_2x2) {
	double angle = 1.0;
	auto matrix = PIMathMatrixT<2u, 2u, double>::identity();
	matrix.rotate(angle);
	double c = cos(angle);
	double s = sin(angle);
	ASSERT_TRUE((c == matrix.at(1u,1u)) && (c == matrix.at(0u,0u)) && (-s == matrix.at(0u,1u)) && (s == matrix.at(1u,0u)));
}

TEST(PIMathMatrixT_Test, matrixMultiplication)
{
	auto matrix1 = PIMathMatrixT<rows, cols, double>(1.5);
	auto matrix2 = PIMathMatrixT<rows, cols, double>(2.5);
	ASSERT_TRUE(cmpSquareMatrixWithValue(matrix1 * matrix2, 11.25, 3));
}

TEST(PIMathMatrixT_Test, matrixAndVectorMultiplication) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(1.5);
	auto vector = PIMathVectorT<rows, double>(2.5);
	for(uint i = 0; i < 2; i++) {
		if((matrix1 * vector)[i] != 11.25) {
			ASSERT_TRUE(false);
		}
	}
	ASSERT_TRUE(true);
}

TEST(PIMathMatrixT_Test, vectorAndMatrixMultiplication) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(1.5);
	auto vector = PIMathVectorT<rows, double>(2.5);
	for(uint i = 0; i < 2; i++) {
		if((vector * matrix1)[i] != 11.25) {
			ASSERT_TRUE(false);
		}
	}
}

TEST(PIMathMatrixT_Test, valAndMatrixMultiplication) {
	auto matrix1 = PIMathMatrixT<rows, cols, double>(1.5);
	ASSERT_TRUE(cmpSquareMatrixWithValue(25.0*matrix1, 37.5, 3));
}
