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

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

bool cmpMatrixWithValue(PIMathMatrixT<rows, cols, double> matrix, double val)
{
    int i = 0;
    int j = 0;
    int k = 0;
    bool b;
    while(i < 9)
    {
        if(k < 3)
        {
            if(matrix.at(j,k) == val)
            {
                b = true;
            }
            else
            {
                b = false;
                break;
            }
            k++;
            if(k == 3)
            {
                j++;
                k = 0;
            }
        }
        i++;
    }
    return b;
}


TEST(PIMathMatrixT_Test, identity)
{
    PIMathMatrixT<rows, cols, double> matr;
    PIMathMatrixT<rows, cols, double> matrix;
    double d;
    double i = 1.0;
    bool a;
    bool output;
    matrix = matr.identity();
    d = matrix.determinant();
    uint j;
    for(j = 0; j < cols; j++)
    {
        if(matrix.at(i, i) == 1.0) a = true;
        else
        {
            a = false;
            break;
        }
    }
    if((i == d) && (a == true)){
        output = true;
    }
    else{
        output = false;
    }
    ASSERT_TRUE(output);
}

TEST(PIMathMatrixT_Test, at)
{
    uint i;
    bool b;
    PIMathMatrixT<rows, cols, double> matr;
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    matrix1 = matr.identity();
    matrix2 = matr.identity();
    for(i = 0; i < rows; i++)
    {
        if(matrix1.at(i,i) == 1.0)
        {
            b = true;
        }
        else
        {
            b = false;
            break;
        }
    }
    ASSERT_TRUE(b);
}
TEST(PIMathMatrixT_Test, filled)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    PIMathMatrixT<rows, cols, double> matr;
    PIMathVectorT<rows, double> vect;
    double g = 1.0;
    matrix2 = matr.fill(g);
    uint j = 0, i = 0;
    for(i = 0; i < cols; i++)
    {
        for(j = 0; j < rows; j++)
        {
            matrix1.at(j,i) = g;
        }
    }
    ASSERT_TRUE(matrix2 == matrix1);
}

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 i;
    uint g = 2;
    bool b;
    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(i = 0; i < matr.cols(); i++)
    {
        if(matr.at(i,g) == vect.at(i))
        {
            b = true;
        }
        else
        {
            b = false;
            break;
        }
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, row)
{
    PIMathMatrixT<rows, cols, double> matr;
    PIMathVectorT<rows, double> vect;
    uint i;
    uint g = 2;
    bool b;
    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(i = 0; i < matr.rows(); i++)
    {
        if(matr.at(g,i) == vect.at(i))
        {
            b = true;
        }
        else
        {
            b = false;
            break;
        }
    }
    ASSERT_TRUE(b);
}

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;
    uint i = 0;
    bool b;
    matr.setCol(g, vect);
    for(i = 0; i < vect.size(); i++)
    {
        if(matr.at(i,g) == vect.at(i))
        {
            b = true;
        }
        else
        {
            b = false;
            break;
        }
    }
    ASSERT_TRUE(b);
}

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;
    uint i = 0;
    bool b;
    matr.setRow(g, vect);
    for(i = 0; i < vect.size(); i++)
    {
        if(matr.at(g,i) == vect.at(i))
        {
            b = true;
        }
        else
        {
            b = false;
            break;
        }
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, swapCols)
{
    PIMathMatrixT<rows, cols, double> matr;
    PIMathVectorT<rows, double> before_Vect1;
    PIMathVectorT<rows, double> before_Vect2;
    PIMathVectorT<rows, double> after_Vect1;
    PIMathVectorT<rows, double> after_Vect2;
    int g1 = 1, g2 = 2;
    bool b;
    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;
    before_Vect1 = matr.col(g1);
    before_Vect2 = matr.col(g2);
    matr.swapCols(g1,g2);
    after_Vect1 = matr.col(g1);
    after_Vect2 = matr.col(g2);
    if((before_Vect1 == after_Vect2) && (before_Vect2 == after_Vect1))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, swapRows)
{
    PIMathMatrixT<rows, cols, double> matr;
    PIMathVectorT<rows, double> before_Vect1;
    PIMathVectorT<rows, double> before_Vect2;
    PIMathVectorT<rows, double> after_Vect1;
    PIMathVectorT<rows, double> after_Vect2;
    int g1 = 1, g2 = 2;
    bool b;
    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;
    before_Vect1 = matr.row(g1);
    before_Vect2 = matr.row(g2);
    matr.swapRows(g1,g2);
    after_Vect1 = matr.row(g1);
    after_Vect2 = matr.row(g2);
    if((before_Vect1 == after_Vect2) && (before_Vect2 == after_Vect1))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

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

TEST(PIMathMatrixT_Test, isSquare)
{
    PIMathMatrixT<rows, cols, double> matr;
    PIMathMatrixT<rows, cols, double> matrix1;
    const uint new_Cols = 4;
    PIMathMatrixT<rows, new_Cols, double> matrix2;
    bool b;
    if((matrix1.isSquare() == true) && (matrix2.isSquare() == false))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, isIdentity)
{
    PIMathMatrixT<rows, cols, double> matr;
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    bool b;
    matrix1 = matr.identity();
    matrix2 = matr.filled(3.67);
    if((matrix1.isIdentity() == true) && (matrix2.isIdentity() == false))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, isNull)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    PIMathMatrixT<rows, cols, double> matr;
    bool b;
    matrix2 = matr.filled(3.67);
    if((matrix1.isNull() == true) && (matrix2.isNull() == false))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, operator_Assignment)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    PIMathMatrixT<rows, cols, double> matr;
    bool b;
    matrix2.fill(6.72);
    matrix1 = matrix2;
    b = cmpMatrixWithValue(matrix1, 6.72);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, operator_Equal)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    PIMathMatrixT<rows, cols, double> matr;
    bool b;
    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;
    matrix1 = matr;
    matrix2 = matr;
    matrix2.at(2, 2) = 232;
    if(((matr == matrix1) == true) && ((matr == matrix2) == false))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, operator_Not_Equal)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    bool b;
    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;
    matrix1 = matr;
    matrix2 = matr;
    matrix2.at(2, 2) = 232;
    if(((matr != matrix1) == false) && ((matr != matrix2) == true))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, operator_Addition_Aassignment)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    bool b;
    matrix2.fill(6.72);
    matrix1.fill(1.0);
    matrix1 += matrix2;
    b = cmpMatrixWithValue(matrix1, 7.72);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, operator_Subtraction_Assignment)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    bool b;
    matrix2.fill(6.72);
    matrix1.fill(1.0);
    matrix1 -= matrix2;
    b = cmpMatrixWithValue(matrix1, -5.72);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, operator_Multiplication_Assignment)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    bool b;
    matrix1.fill(6.72);
    matrix1 *= 2.0;
    b = cmpMatrixWithValue(matrix1, 13.44);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, operator_Division_Assignment)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    bool b;
    matrix1.fill(6.72);
    matrix1 /= 2.0;
    b = cmpMatrixWithValue(matrix1, 3.36);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, operator_Addition)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    bool b;
    matrix1.fill(6.72);
    matrix2.fill(8.28);
    b = cmpMatrixWithValue(matrix1 + matrix2, 15.0);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, operator_Subtraction)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    bool b;
    matrix1.fill(6.0);
    matrix2.fill(5.0);
    b = cmpMatrixWithValue(matrix1 - matrix2, 1.0);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, operator_Multiplication)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    bool b;
    matrix1.fill(6.72);
    matrix2 = matrix1*4.0;
    b = cmpMatrixWithValue(matrix2, 26.88);
    ASSERT_TRUE(b);
}
TEST(PIMathMatrixT_Test, operator_Division)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    bool b;
    matrix1.fill(6.72);
    matrix2 = matrix1/4.0;
    b = cmpMatrixWithValue(matrix2, 1.68);
    ASSERT_TRUE(b);
}


TEST(PIMathMatrixT_Test, determinant)
{
    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, invert)
{
    PIMathMatrixT<rows, cols, double> matrix1;
    PIMathMatrixT<rows, cols, double> matrix2;
    PIMathMatrixT<rows, cols, double> matrix3;
    PIMathMatrixT<rows, cols, double> matr;
    double d1, d2;
    bool b;
    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;
    matr.invert();
    d1 = matr.determinant();
    d2 = matrix2.determinant();
    matrix3 = matrix1;
    matrix1.invert();
    if((matrix1 == matrix3) && (d1 == 1/d2))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

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;
    bool b;
    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();
    if((matrix1 == matrix3) && (round((1/d1)*10000)/10000 == round(d2*10000)/10000))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, toUpperTriangular)
{
    PIMathMatrixT<rows, cols, double> matrix;
    double d1, d2 = 1;
    uint i;
    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(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;
    bool b;
    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();
    if((d1 == d2) && (matr == matrix2))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, scaleX_two)
{
    double factor = 5.64;
    bool b;
    PIMathMatrixT<2u, 2u, double> matrix = PIMathMatrixT<2u, 2u, double>::scaleX(factor);

    if((1.0 == matrix.at(1u,1u)) && (factor == matrix.at(0u,0u)))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, scaleY_two)
{
    double factor = 5.64;
    bool b;
    PIMathMatrixT<2u, 2u, double> matrix = PIMathMatrixT<2u, 2u, double>::scaleY(factor);

    if((factor == matrix.at(1u,1u)) && (1.0 == matrix.at(0u,0u)))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, rotation_2x2)
{
    double angle = 1.0;
    bool b;
    PIMathMatrixT<2u, 2u, double> matrix = PIMathMatrixT<2u, 2u, double>::rotation(angle);
    double c = cos(angle);
    double s = sin(angle);
    if((c == matrix.at(1u,1u)) && (c == matrix.at(0u,0u)) && (-s == matrix.at(0u,1u)) && (s == matrix.at(1u,0u)))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, rotation_3x3)
{
    double angle = 1.0;
    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::rotation(angle);
    ASSERT_TRUE(cmpMatrixWithValue(matrix, 0.0));
}

TEST(PIMathMatrixT_Test, rotationX)
{
    double angle = 1.0;
    bool b;
    double c = cos(angle);
    double s = sin(angle);
    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::rotationX(angle);

    if((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)))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, rotationY)
{
    double angle = 1.0;
    bool b;
    double c = cos(angle);
    double s = sin(angle);
    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::rotationY(angle);

    if((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)))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrixT_Test, rotationZ)
{
    double angle = 1.0;
    bool b;
    double c = cos(angle);
    double s = sin(angle);
    PIMathMatrixT<3u, 3u, double> matrix = PIMathMatrixT<3u, 3u, double>::rotationZ(angle);

    if((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)))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

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

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

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