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

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

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

TEST(PIMathMatrixT_Test, identity)
{
    auto matrix = PIMathMatrixT<rows, cols, double>::identity();
    for(int i = 0; i < 3; i++){
        if(matrix[i][i] != 1.0){
            ASSERT_TRUE(false);
        }
    }
    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);
                }
            }
        }
    }
    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>::filled(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.at(0,0) = 3;
    matr.at(0,1) = 6;
    matr.at(0,2) = 8;
    matr.at(1,0) = 2;
    matr.at(1,1) = 1;
    matr.at(1,2) = 4;
    matr.at(2,0) = 6;
    matr.at(2,1) = 2;
    matr.at(2,2) = 5;
    vect = matr.col(g);
    for(uint i = 0; i < matr.cols(); i++) {
        if(matr.at(i, g) != vect.at(i)) {
            ASSERT_TRUE(false);
        }
    }
    ASSERT_TRUE(true);
}

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

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

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

TEST(PIMathMatrixT_Test, swapCols)
{
    PIMathMatrixT<rows, cols, double> matr;
    int g1 = 1, g2 = 2;
    matr.at(0,0) = 3;
    matr.at(0,1) = 6;
    matr.at(0,2) = 8;
    matr.at(1,0) = 2;
    matr.at(1,1) = 1;
    matr.at(1,2) = 4;
    matr.at(2,0) = 6;
    matr.at(2,1) = 2;
    matr.at(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.at(0,0) = 3;
    matr.at(0,1) = 6;
    matr.at(0,2) = 8;
    matr.at(1,0) = 2;
    matr.at(1,1) = 1;
    matr.at(1,2) = 4;
    matr.at(2,0) = 6;
    matr.at(2,1) = 2;
    matr.at(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.at(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>::filled(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>::filled(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.at(0, 0) = 5.1;
    matrix1.at(0, 1) = 1.21;
    matrix1.at(1, 1) = 0.671;
    matrix1.at(1, 0) = 2.623;
    matrix2.at(0, 0) = 5.1;
    matrix2.at(0, 1) = 1.21;
    matrix2.at(1, 1) = 0.671;
    matrix2.at(1, 0) = 2.623;
    ASSERT_TRUE(matrix1 == matrix2);
}

TEST(PIMathMatrixT_Test, operator_EqualFalse)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    matrix1.at(0, 0) = 5.1;
    matrix1.at(0, 1) = 1.21;
    matrix1.at(1, 1) = 0.671;
    matrix1.at(1, 0) = 2.623;
    matrix2.at(0, 0) = 5.1;
    matrix2.at(0, 1) = 1.21;
    matrix2.at(1, 1) = 665.671;
    matrix2.at(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.at(0, 0) = 5.1;
    matrix1.at(0, 1) = 1.21;
    matrix1.at(1, 1) = 0.671;
    matrix1.at(1, 0) = 2.623;
    matrix2.at(0, 0) = 5.1;
    matrix2.at(0, 1) = 1.21;
    matrix2.at(1, 1) = 665.671;
    matrix2.at(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.at(0, 0) = 5.1;
    matrix1.at(0, 1) = 1.21;
    matrix1.at(1, 1) = 0.671;
    matrix1.at(1, 0) = 2.623;
    matrix2.at(0, 0) = 5.1;
    matrix2.at(0, 1) = 1.21;
    matrix2.at(1, 1) = 0.671;
    matrix2.at(1, 0) = 2.623;
    ASSERT_FALSE(matrix1 != matrix2);
}

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

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

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

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

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

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

TEST(PIMathMatrixT_Test, operator_Multiplication)
{
    auto matrix1 = PIMathMatrixT<rows, cols, double>::filled(6.72);
    ASSERT_TRUE(cmpSquareMatrixWithValue(matrix1 * 4.0, 26.88, rows));
}
TEST(PIMathMatrixT_Test, operator_Division)
{
    auto matrix1 = PIMathMatrixT<rows, cols, double>::filled(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.at(0,0) = 3;
    matr.at(0,1) = 6;
    matr.at(0,2) = 8;
    matr.at(1,0) = 2;
    matr.at(1,1) = 1;
    matr.at(1,2) = 4;
    matr.at(2,0) = 6;
    matr.at(2,1) = 2;
    matr.at(2,2) = 5;
    d = matr.determinant();
    ASSERT_DOUBLE_EQ(i, d);
}

TEST(PIMathMatrixT_Test, determinantIfNotSquare)
{
    PIMathMatrixT<rows, 5u, double> matr;
    matr.at(0,0) = 3;
    matr.at(0,1) = 6;
    matr.at(0,2) = 8;
    matr.at(1,0) = 2;
    matr.at(1,1) = 1;
    matr.at(1,2) = 4;
    matr.at(2,0) = 6;
    matr.at(2,1) = 2;
    matr.at(2,2) = 5;
    ASSERT_FALSE(matr.determinant());
}

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.at(0,0) = 3;
    matr.at(0,1) = 6;
    matr.at(0,2) = 8;
    matr.at(1,0) = 2;
    matr.at(1,1) = 1;
    matr.at(1,2) = 4;
    matr.at(2,0) = 6;
    matr.at(2,1) = 2;
    matr.at(2,2) = 5;
    matrix2 = matr;
    matr.invert();
    d1 = matr.determinant();
    d2 = matrix2.determinant();
    matrix3 = matrix1;
    matrix1.invert();
    ASSERT_TRUE((matrix1 == matrix3) && (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.at(0,0) = 3;
    matr.at(0,1) = 6;
    matr.at(0,2) = 8;
    matr.at(1,0) = 2;
    matr.at(1,1) = 1;
    matr.at(1,2) = 4;
    matr.at(2,0) = 6;
    matr.at(2,1) = 2;
    matr.at(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.at(0,0) = 3;
    matr.at(0,1) = 6;
    matr.at(0,2) = 8;
    matr.at(1,0) = 2;
    matr.at(1,1) = 1;
    matr.at(1,2) = 4;
    matr.at(2,0) = 6;
    matr.at(2,1) = 2;
    matr.at(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.at(0,0) = 3;
    matr.at(0,1) = 6;
    matr.at(0,2) = 8;
    matr.at(1,0) = 2;
    matr.at(1,1) = 1;
    matr.at(1,2) = 4;
    matr.at(2,0) = 6;
    matr.at(2,1) = 2;
    matr.at(2,2) = 5;
    d1 = matr.determinant();
    matrix1 = matr.transposed();
    d2 = matrix1.determinant();
    matrix2 = matrix1.transposed();
    ASSERT_TRUE((d1 == d2) && (matr == matrix2));
}

TEST(PIMathMatrixT_Test, scaleX_two)
{
    double factor = 5.64;
    PIMathMatrixT<2u, 2u, double> matrix = PIMathMatrixT<2u, 2u, double>::scaleX(factor);
    ASSERT_TRUE((1.0 == matrix.at(1u,1u)) && (factor == matrix.at(0u,0u)));
}

TEST(PIMathMatrixT_Test, scaleY_two)
{
    double factor = 5.64;
    PIMathMatrixT<2u, 2u, double> matrix = PIMathMatrixT<2u, 2u, double>::scaleY(factor);
    ASSERT_TRUE((factor == matrix.at(1u,1u)) && (1.0 == matrix.at(0u,0u)));
}

TEST(PIMathMatrixT_Test, rotation_2x2)
{
    double angle = 1.0;
    PIMathMatrixT<2u, 2u, double> matrix = PIMathMatrixT<2u, 2u, double>::rotation(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, rotation_3x3)
{
    double angle = 1.0;
    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::rotation(angle);
    ASSERT_TRUE(cmpSquareMatrixWithValue(matrix, 0.0, rows));
}

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

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

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

TEST(PIMathMatrixT_Test, scaleX_three)
{
    double factor = 23.65;
    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::scaleX(factor);
    ASSERT_TRUE((1.0 == matrix.at(2u,2u)) && (factor == matrix.at(0u,0u)) && (1.0 == matrix.at(1u,1u)));
}

TEST(PIMathMatrixT_Test, scaleY_three)
{
    double factor = 23.65;
    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::scaleY(factor);
    ASSERT_TRUE((1.0 == matrix.at(2u,2u)) && (1.0 == matrix.at(0u,0u)) && (factor == matrix.at(1u,1u)));
}

TEST(PIMathMatrixT_Test, scaleZ_three)
{
    double factor = 23.65;
    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::scaleZ(factor);
    ASSERT_TRUE((factor == matrix.at(2u,2u)) && (1.0 == matrix.at(0u,0u)) && (1.0 == matrix.at(1u,1u)));
}

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

TEST(PIMathMatrixT_Test, matrixAndVectorMultiplication)
{
    auto matrix1 = PIMathMatrixT<rows, cols, double>::filled(1.5);
    auto vector = PIMathVectorT<rows, double>::filled(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>::filled(1.5);
    auto vector = PIMathVectorT<rows, double>::filled(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>::filled(1.5);
    ASSERT_TRUE(cmpSquareMatrixWithValue(25.0*matrix1, 37.5, 3));
}