#include "pimathmatrix.h"

#include "gtest/gtest.h"

const uint rows = 3;
const uint cols = 3;
using Type      = double;

bool cmpSquareMatrixWithValue(PIMathMatrixT<rows, cols, Type> matrix, Type 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, Type>::identity();
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			if (i != j) {
				if (matrix[i][j] != 0.0) {
					EXPECT_TRUE(false);
				}
			} else {
				if (matrix[i][i] != 1.0) {
					EXPECT_TRUE(false);
				}
			}
		}
	}
	EXPECT_TRUE(true);
}

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

TEST(PIMathMatrixT_Test, cols) {
	PIMathMatrixT<rows, cols, Type> matr;
	EXPECT_EQ(cols, matr.cols());
}

TEST(PIMathMatrixT_Test, rows) {
	PIMathMatrixT<rows, cols, Type> matr;
	EXPECT_EQ(rows, matr.rows());
}

TEST(PIMathMatrixT_Test, col) {
	PIMathMatrixT<rows, cols, Type> matr;
	PIMathVectorT<rows, Type> 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]) {
			EXPECT_TRUE(false);
		}
	}
	EXPECT_TRUE(true);
}

TEST(PIMathMatrixT_Test, row) {
	PIMathMatrixT<rows, cols, Type> matr;
	PIMathVectorT<rows, Type> 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]) {
			EXPECT_TRUE(false);
		}
	}
	EXPECT_TRUE(true);
}

TEST(PIMathMatrixT_Test, setCol) {
	PIMathMatrixT<rows, cols, Type> matr;
	PIMathVectorT<rows, Type> 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]) {
			EXPECT_TRUE(false);
		}
	}
	EXPECT_TRUE(true);
}

TEST(PIMathMatrixT_Test, setRow) {
	PIMathMatrixT<rows, cols, Type> matr;
	PIMathVectorT<rows, Type> 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]) {
			EXPECT_TRUE(false);
		}
	}
	EXPECT_TRUE(true);
}

TEST(PIMathMatrixT_Test, swapCols) {
	PIMathMatrixT<rows, cols, Type> 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, Type> before_Vect1 = matr.col(g1);
	const PIMathVectorT<rows, Type> before_Vect2 = matr.col(g2);
	matr.swapCols(g1, g2);
	const PIMathVectorT<rows, Type> after_Vect1 = matr.col(g1);
	const PIMathVectorT<rows, Type> after_Vect2 = matr.col(g2);
	if ((before_Vect1 == after_Vect2) && (before_Vect2 == after_Vect1)) {
		EXPECT_TRUE(true);
	} else {
		EXPECT_TRUE(false);
	}
}

TEST(PIMathMatrixT_Test, swapRows) {
	PIMathMatrixT<rows, cols, Type> 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, Type> before_Vect1 = matr.row(g1);
	const PIMathVectorT<rows, Type> before_Vect2 = matr.row(g2);
	matr.swapRows(g1, g2);
	const PIMathVectorT<rows, Type> after_Vect1 = matr.row(g1);
	const PIMathVectorT<rows, Type> after_Vect2 = matr.row(g2);
	if ((before_Vect1 == after_Vect2) && (before_Vect2 == after_Vect1)) {
		EXPECT_TRUE(true);
	} else {
		EXPECT_TRUE(false);
	}
}

TEST(PIMathMatrixT_Test, fill) {
	PIMathMatrixT<rows, cols, Type> matr;
	PIMathMatrixT<rows, cols, Type> matrix1;
	Type 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;
		}
	}
	EXPECT_TRUE(matr == matrix1);
}

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

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

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

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

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

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

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

TEST(PIMathMatrixT_Test, operator_EqualTrue) {
	PIMathMatrixT<rows, cols, Type> matrix1;
	PIMathMatrixT<rows, cols, Type> 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;
	EXPECT_TRUE(matrix1 == matrix2);
}

TEST(PIMathMatrixT_Test, operator_EqualFalse) {
	PIMathMatrixT<rows, cols, Type> matrix1;
	PIMathMatrixT<rows, cols, Type> 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;
	EXPECT_FALSE(matrix1 == matrix2);
}

TEST(PIMathMatrixT_Test, operator_Not_EqualTrue) {
	PIMathMatrixT<rows, cols, Type> matrix1;
	PIMathMatrixT<rows, cols, Type> 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;
	EXPECT_TRUE(matrix1 != matrix2);
}

TEST(PIMathMatrixT_Test, operator_Not_EqualFalse) {
	PIMathMatrixT<rows, cols, Type> matrix1;
	PIMathMatrixT<rows, cols, Type> 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;
	EXPECT_FALSE(matrix1 != matrix2);
}

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

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

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

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

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

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

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

TEST(PIMathMatrixT_Test, determinantIfSquare) {
	Type d;
	Type i = 59.0;
	PIMathMatrixT<rows, cols, Type> 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();
	EXPECT_NEAR(std::abs(i - d), 0., 1e-12);
}

TEST(PIMathMatrixT_Test, invert) {
	PIMathMatrixT<rows, cols, Type> matrix1;
	PIMathMatrixT<rows, cols, Type> matrix2;
	PIMathMatrixT<rows, cols, Type> matrix3;
	PIMathMatrixT<rows, cols, Type> matr;
	Type 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();
	EXPECT_EQ(matrix1, matrix3);
	EXPECT_DOUBLE_EQ(std::abs(d1 - (1. / d2)), 0.);
}

TEST(PIMathMatrixT_Test, inverted) {
	PIMathMatrixT<rows, cols, Type> matrix1;
	PIMathMatrixT<rows, cols, Type> matrix2;
	PIMathMatrixT<rows, cols, Type> matrix3;
	PIMathMatrixT<rows, cols, Type> matr;
	Type 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();
	EXPECT_EQ(matrix1, matrix3);
	EXPECT_NEAR(std::abs(d1 - (1. / d2)), 0., 1e-12);
}

TEST(PIMathMatrixT_Test, toUpperTriangular) {
	PIMathMatrixT<rows, cols, Type> matrix;
	Type d1, d2 = 1;
	PIMathMatrixT<rows, cols, Type> 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);
	}
	EXPECT_LE(std::abs(d1 - d2), PIMATHVECTOR_ZERO_CMP);
}

TEST(PIMathMatrixT_Test, transposed) {
	PIMathMatrixT<rows, cols, Type> matrix1;
	PIMathMatrixT<rows, cols, Type> matrix2;
	PIMathMatrixT<rows, cols, Type> matr;
	Type 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();
	EXPECT_TRUE((d1 == d2) && (matr == matrix2));
}

TEST(PIMathMatrixT_Test, rotation_2x2) {
	Type angle  = 1.0;
	auto matrix = PIMathMatrixT<2u, 2u, Type>::identity();
	matrix.rotate(angle);
	Type c = cos(angle);
	Type s = sin(angle);
	EXPECT_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, Type>(1.5);
	auto matrix2 = PIMathMatrixT<rows, cols, Type>(2.5);
	EXPECT_TRUE(cmpSquareMatrixWithValue(matrix1 * matrix2, 11.25, 3));
}

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

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

TEST(PIMathMatrixT_Test, valAndMatrixMultiplication) {
	auto matrix1 = PIMathMatrixT<rows, cols, Type>(1.5);
	EXPECT_TRUE(cmpSquareMatrixWithValue(Type(25.) * matrix1, 37.5, 3));
}