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

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

TEST(PIMathMatrix_Test, identity)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix;
    int i;
    bool b;
    matrix = origMatr.identity(3, 3);
    for(i = 0; i < 3; i++)
    {
        if(matrix[i][i] == 1.0)
        {
            b = true;
        }
        else
        {
            b = false;
            break;
        }
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, matrixRow)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix;
    PIMathVector<double> vector;
    uint i;
    bool b;
    vector.resize(3, 3.0);
    vector.fill(5.0);
    matrix = origMatr.matrixRow(vector);
    for(i = 0; i < vector.size(); i++)
    {
        if(matrix[0][i] == 5.0)
        {
            b = true;
        }
        else
        {
            b = false;
            break;
        }
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, matrixCol)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix;
    PIMathVector<double> vector;
    uint i;
    bool b;
    vector.resize(3, 3.0);
    vector.fill(5.0);
    matrix = origMatr.matrixCol(vector);
    for(i = 0; i < vector.size(); i++)
    {
        if(matrix[i][0] == 5.0)
        {
            b = true;
        }
        else
        {
            b = false;
            break;
        }
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, setCol)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix;
    PIMathVector<double> vector;
    uint i;
    bool b;
    vector.resize(3, 3.0);
    vector.fill(5.0);
    matrix = origMatr.matrixCol(vector);
    vector.fill(10.0);
    matrix.setCol(0, vector);
    for(i = 0; i < vector.size(); i++)
    {
        if(matrix[i][0] == 10.0)
        {
            b = true;
        }
        else
        {
            b = false;
            break;
        }
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, setRow)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix;
    PIMathVector<double> vector;
    uint i;
    bool b;
    vector.resize(3, 3.0);
    vector.fill(5.0);
    matrix = origMatr.matrixRow(vector);
    vector.fill(10.0);
    matrix.setRow(0, vector);
    for(i = 0; i < vector.size(); i++)
    {
        if(matrix[0][i] == 10.0)
        {
            b = true;
        }
        else
        {
            b = false;
            break;
        }
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, swapCols)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathVector<double> vector;
    uint i1 = 0; uint i2 = 1;
    double a1[3], a2[3], a3[3];
    double b1[3], b2[3], b3[3];
    bool b;
    vector.resize(3, 3.0);
    vector.at(0) = 3.0;
    vector.at(1) = 6.0;
    vector.at(2) = 8.0;
    matrix1 = origMatr.identity(3, 3);
    matrix1.setCol(0, vector);
    vector.at(0) = 2.0;
    vector.at(1) = 1.0;
    vector.at(2) = 4.0;
    matrix1.setCol(1, vector);
    vector.at(0) = 6.0;
    vector.at(1) = 2.0;
    vector.at(2) = 5.0;
    matrix1.setCol(2, vector);
    for(int i = 0; i < 3; i++)
    {
        a1[i] = matrix1.element(i, 0);
        a2[i] = matrix1.element(i, 1);
        a3[i] = matrix1.element(i, 2);
    }
    matrix1.swapCols(i1, i2);
    for(int i = 0; i < 3; i++)
    {
        b1[i] = matrix1.element(i, 0);
        b2[i] = matrix1.element(i, 1);
        b3[i] = matrix1.element(i, 2);
    }
    if((memcmp(a1, b2, sizeof(b1)) == 0) && (memcmp(a2, b1, sizeof(b1)) == 0) && (memcmp(a3, b3, sizeof(b1)) == 0))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, swapRows)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathVector<double> vector;
    uint i1 = 0; uint i2 = 1;
    double a1[3], a2[3], a3[3];
    double b1[3], b2[3], b3[3];
    bool b;
    vector.resize(3, 3.0);
    vector.at(0) = 3.0;
    vector.at(1) = 6.0;
    vector.at(2) = 8.0;
    matrix1 = origMatr.identity(3, 3);
    matrix1.setCol(0, vector);
    vector.at(0) = 2.0;
    vector.at(1) = 1.0;
    vector.at(2) = 4.0;
    matrix1.setCol(1, vector);
    vector.at(0) = 6.0;
    vector.at(1) = 2.0;
    vector.at(2) = 5.0;
    matrix1.setCol(2, vector);
    for(int i = 0; i < 3; i++)
    {
        a1[i] = matrix1.element(0, i);
        a2[i] = matrix1.element(1, i);
        a3[i] = matrix1.element(2, i);
    }
    matrix1.swapRows(i1, i2);
    for(int i = 0; i < 3; i++)
    {
        b1[i] = matrix1.element(0, i);
        b2[i] = matrix1.element(1, i);
        b3[i] = matrix1.element(2, i);
    }
    if((memcmp(a1, b2, sizeof(b1)) == 0) && (memcmp(a2, b1, sizeof(b1)) == 0) && (memcmp(a3, b3, sizeof(b1)) == 0))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}
TEST(PIMathMatrix_Test, fill)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix;
    bool b;
    matrix = origMatr.identity(3, 3);
    matrix.fill(5.0);
    b = cmpMatrixWithValue(matrix, 5.0);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, isSquare)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix2 = origMatr.identity(4,3);
    if((matrix1.isSquare() == true) && (matrix2.isSquare() == false))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, isIdentity)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix2 = origMatr.identity(3,3);
    matrix2.fill(3.932);
    if((matrix1.isIdentity() == true) && (matrix2.isIdentity() == false))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, isNull)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix2 = origMatr.identity(3,3);
    matrix1.fill(0.0);
    matrix2.fill(3.932);
    if((matrix1.isNull() == true) && (matrix2.isNull() == false))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, isValid)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    bool b;
    matrix1 = origMatr.identity(3,3);
    if((matrix1.isValid() == true) && (matrix2.isValid() == false))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, operator_Assignment)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix2 = origMatr.identity(3,3);
    matrix2.fill(6.72);
    matrix1 = matrix2;
    b = cmpMatrixWithValue(matrix1, 6.72);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, operator_Equal)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    PIMathMatrix<double> matrix3;
    PIMathVector<double> vector;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix2 = origMatr.identity(3,3);
    matrix3 = origMatr.identity(3,3);
    vector.resize(3, 3.0);
    vector.at(0) = 3.0;
    vector.at(1) = 6.0;
    vector.at(2) = 8.0;
    matrix1.setCol(0, vector);
    matrix2.setCol(0, vector);
    matrix3.setCol(0, vector);
    vector.at(0) = 2.0;
    vector.at(1) = 1.0;
    vector.at(2) = 4.0;
    matrix1.setCol(1, vector);
    matrix2.setCol(1, vector);
    matrix3.setCol(1, vector);
    vector.at(0) = 6.0;
    vector.at(1) = 2.0;
    vector.at(2) = 5.0;
    matrix1.setCol(2, vector);
    matrix2.setCol(2, vector);
    vector.at(0) = 566.0;
    vector.at(1) = 564.0;
    vector.at(2) = 543.0;
    matrix3.setCol(2, vector);
    if(((matrix1 == matrix2) == true) && ((matrix1 == matrix3) == false))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, operator_Not_Equal)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    PIMathMatrix<double> matrix3;
    PIMathVector<double> vector;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix2 = origMatr.identity(3,3);
    matrix3 = origMatr.identity(3,3);
    vector.resize(3, 3.0);
    vector.at(0) = 3.0;
    vector.at(1) = 6.0;
    vector.at(2) = 8.0;
    matrix1.setCol(0, vector);
    matrix2.setCol(0, vector);
    matrix3.setCol(0, vector);
    vector.at(0) = 2.0;
    vector.at(1) = 1.0;
    vector.at(2) = 4.0;
    matrix1.setCol(1, vector);
    matrix2.setCol(1, vector);
    matrix3.setCol(1, vector);
    vector.at(0) = 6.0;
    vector.at(1) = 2.0;
    vector.at(2) = 5.0;
    matrix1.setCol(2, vector);
    matrix2.setCol(2, vector);
    vector.at(0) = 566.0;
    vector.at(1) = 564.0;
    vector.at(2) = 543.0;
    matrix3.setCol(2, vector);
    if(((matrix1 != matrix2) == false) && ((matrix1 != matrix3) == true))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, operator_Addition_Aassignment)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix2 = origMatr.identity(3,3);
    matrix2.fill(6.72);
    matrix1.fill(1.0);
    matrix1 += matrix2;
    b = cmpMatrixWithValue(matrix1, 7.72);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, operator_Subtraction_Assignment)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix2 = origMatr.identity(3,3);
    matrix2.fill(6.72);
    matrix1.fill(1.0);
    matrix1 -= matrix2;
    b = cmpMatrixWithValue(matrix1, -5.72);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, operator_Multiplication_Assignment)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix1.fill(6.72);
    matrix1 *= 2.0;
    b = cmpMatrixWithValue(matrix1, 13.44);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, operator_Division_Assignment)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix1.fill(6.72);
    matrix1 /= 2.0;
    b = cmpMatrixWithValue(matrix1, 3.36);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, operator_Addition)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix2 = origMatr.identity(3,3);
    matrix1.fill(6.72);
    matrix2.fill(8.28);
    b = cmpMatrixWithValue(matrix1 + matrix2, 15.0);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, operator_Subtraction)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    bool b;
    matrix1 = origMatr.identity(3, 3);
    matrix2 = origMatr.identity(3, 3);
    origMatr.fill(8.0);
    matrix1.fill(8.28);
    origMatr = origMatr - matrix1;
    matrix2.fill(-0.28);
    if(origMatr == matrix2)
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, operator_Multiplication)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix2 = origMatr.identity(3,3);
    matrix1.fill(6.72);
    matrix2 = matrix1*4.0;
    b = cmpMatrixWithValue(matrix2, 26.88);
    ASSERT_TRUE(b);
}
TEST(PIMathMatrix_Test, operator_Division)
{
    PIMathMatrix<double> origMatr;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    bool b;
    matrix1 = origMatr.identity(3,3);
    matrix2 = origMatr.identity(3,3);
    matrix1.fill(6.72);
    matrix2 = matrix1/4.0;
    b = cmpMatrixWithValue(matrix2, 1.68);
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, determinant)
{
    PIMathMatrix<double> origMatr;
    double d;
    double i = 59.0;
    PIMathMatrix<double> matrix;
    PIMathVector<double> vector;
    vector.resize(3, 3.0);
    vector.at(0) = 3.0;
    vector.at(1) = 6.0;
    vector.at(2) = 8.0;
    matrix = origMatr.identity(3, 3);
    matrix.setCol(0, vector);
    vector.at(0) = 2.0;
    vector.at(1) = 1.0;
    vector.at(2) = 4.0;
    matrix.setCol(1, vector);
    vector.at(0) = 6.0;
    vector.at(1) = 2.0;
    vector.at(2) = 5.0;
    matrix.setCol(2, vector);
    d = matrix.determinant();
    ASSERT_DOUBLE_EQ(d, i);
}

TEST(PIMathMatrix_Test, trace)
{
    PIMathMatrix<double> origMatr;
    double t;
    double i = 9.0;
    PIMathMatrix<double> matrix;
    PIMathVector<double> vector;
    vector.resize(3, 3.0);
    vector.at(0) = 3.0;
    vector.at(1) = 6.0;
    vector.at(2) = 8.0;
    matrix = origMatr.identity(3, 3);
    matrix.setCol(0, vector);
    vector.at(0) = 2.0;
    vector.at(1) = 1.0;
    vector.at(2) = 4.0;
    matrix.setCol(1, vector);
    vector.at(0) = 6.0;
    vector.at(1) = 2.0;
    vector.at(2) = 5.0;
    matrix.setCol(2, vector);
    t = matrix.trace();
    ASSERT_DOUBLE_EQ(t, i);
}

TEST(PIMathMatrix_Test, toUpperTriangular)
{
    PIMathMatrix<double> origMatr;
    double d1, d2 = 1;
    int i;
    PIMathMatrix<double> matrix;
    PIMathVector<double> vector;
    vector.resize(3, 3.0);
    vector.at(0) = 3.0;
    vector.at(1) = 6.0;
    vector.at(2) = 8.0;
    matrix = origMatr.identity(3, 3);
    matrix.setCol(0, vector);
    vector.at(0) = 2.0;
    vector.at(1) = 1.0;
    vector.at(2) = 4.0;
    matrix.setCol(1, vector);
    vector.at(0) = 6.0;
    vector.at(1) = 2.0;
    vector.at(2) = 5.0;
    matrix.setCol(2, vector);
    d1 = matrix.determinant();
    matrix.toUpperTriangular();
    for(i = 0; i < 3; i++)
    {
        d2 = d2 * matrix.element(i, i);
    }
    ASSERT_DOUBLE_EQ(d1, d2);
}

TEST(PIMathMatrix_Test, invert)
{
    PIMathMatrix<double> origMatr;
    double d1, d2;
    bool b;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    PIMathMatrix<double> matrix3;
    PIMathMatrix<double> matrix4;
    PIMathVector<double> vector;
    vector.resize(3, 3.0);
    vector.at(0) = 3.0;
    vector.at(1) = 6.0;
    vector.at(2) = 8.0;
    matrix1 = origMatr.identity(3, 3);
    matrix1.setCol(0, vector);
    vector.at(0) = 2.0;
    vector.at(1) = 1.0;
    vector.at(2) = 4.0;
    matrix1.setCol(1, vector);
    vector.at(0) = 6.0;
    vector.at(1) = 2.0;
    vector.at(2) = 5.0;
    matrix1.setCol(2, vector);
    d1 = matrix1.determinant();
    matrix2 = matrix1;
    matrix2.invert();
    d2 = matrix2.determinant();
    matrix3 = origMatr.identity(3, 3);
    matrix4 = origMatr.identity(3, 3);
    matrix4.invert();
    if((matrix3 == matrix4) && (round((1/d1)*10000)/10000 == round(d2*10000)/10000))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, inverted)
{
    PIMathMatrix<double> origMatr;
    double d1, d2;
    bool b;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    PIMathMatrix<double> matrix3;
    PIMathMatrix<double> matrix4;
    PIMathVector<double> vector;
    vector.resize(3, 3.0);
    vector.at(0) = 3.0;
    vector.at(1) = 6.0;
    vector.at(2) = 8.0;
    matrix1 = origMatr.identity(3, 3);
    matrix1.setCol(0, vector);
    vector.at(0) = 2.0;
    vector.at(1) = 1.0;
    vector.at(2) = 4.0;
    matrix1.setCol(1, vector);
    vector.at(0) = 6.0;
    vector.at(1) = 2.0;
    vector.at(2) = 5.0;
    matrix1.setCol(2, vector);
    d1 = matrix1.determinant();
    matrix2 = matrix1;
    matrix1 = matrix2.invert();
    d2 = matrix1.determinant();
    matrix3 = origMatr.identity(3, 3);
    matrix4 = origMatr.identity(3, 3);
    matrix3 = matrix4.invert();
    if((matrix3 == matrix4) && (round((1/d1)*10000)/10000 == round(d2*10000)/10000))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}

TEST(PIMathMatrix_Test, transposed)
{
    PIMathMatrix<double> origMatr;
    double d1, d2;
    bool b;
    PIMathMatrix<double> matrix1;
    PIMathMatrix<double> matrix2;
    PIMathMatrix<double> matrix3;
    PIMathVector<double> vector;
    vector.resize(3, 3.0);
    vector.at(0) = 3.0;
    vector.at(1) = 6.0;
    vector.at(2) = 8.0;
    matrix1 = origMatr.identity(3, 3);
    matrix1.setCol(0, vector);
    vector.at(0) = 2.0;
    vector.at(1) = 1.0;
    vector.at(2) = 4.0;
    matrix1.setCol(1, vector);
    vector.at(0) = 6.0;
    vector.at(1) = 2.0;
    vector.at(2) = 5.0;
    matrix1.setCol(2, vector);
    d1 = matrix1.determinant();
    matrix2 = matrix1.transposed();
    d2 = matrix2.determinant();
    matrix3 = matrix2.transposed();
    if((d1 == d2) && (matrix1 == matrix3))
    {
        b = true;
    }
    else
    {
        b = false;
    }
    ASSERT_TRUE(b);
}