#include "CppUTest/TestHarness.h"
#include "CppUTest/TestRegistry.h"
#include "CppUTest/TestOutput.h"
#include "CppUTest/TestTestingFixture.h"

#include "mat.h"

#define TESTSIZE 4
int testMat [TESTSIZE][TESTSIZE];
int testVec [TESTSIZE];
float tolerance = 0.00001f;

TEST_GROUP(Matrix)
{
    void setup()
    {
        int i = 1;
        int j = 1;
        for (int x = 0; x <TESTSIZE ; x++)
        {
            for (int y = 0; y <TESTSIZE ; y++)
            {
                testMat[x][y] = i++;
            }
            testVec[x] = j++;
        }
    }

    void teardown()
    {
    }
};

TEST(Matrix, vectorInvRotX)
{
    Mat<float, TESTSIZE> a;
    for(int x = 0 ; x < TESTSIZE ; ++x)
    {
        for (int y = 0; y <TESTSIZE ; ++y)
        {
            float val = 0.0f;
            if (x == y )
                val = 1.0f;
            a(x,x) = val;
        }
    }
    
    a(0,0) = 1.0f;
    a(1,1) = cos(45.0f *M_PI/180.0f);
    a(1,2) = -sin(45.0f *M_PI/180.0f);
    a(2,2) = cos(45.0f *M_PI/180.0f);
    a(2,1) = sin(45.0f *M_PI/180.0f);
    


    float deg = 45.0f*M_PI/180.0f;
    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
    DOUBLES_EQUAL(cos(deg),a(1,1), tolerance);
    DOUBLES_EQUAL(-sin(deg),a(1,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
    DOUBLES_EQUAL(sin(deg),a(2,1), tolerance);
    DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
    
    a = a.inv();
    
    deg = -45.0f*M_PI/180.0f;
    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
    DOUBLES_EQUAL(cos(deg),a(1,1), tolerance);
    DOUBLES_EQUAL(-sin(deg),a(1,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
    DOUBLES_EQUAL(sin(deg),a(2,1), tolerance);
    DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
}

TEST(Matrix, vectorInvRotY)
{
    Mat<float, TESTSIZE> a(0.0f);
    
    float deg = 45.0f*M_PI/180.0f;
    a(1,1) = 1.0f;
    a(0,0) = cos(deg);
    a(0,2) = sin(deg);
    a(2,2) = cos(deg);
    a(2,0) = -sin(deg);
    a(3,3) = 1.0f;
    
    std::cout << a;

    a = a.inv();
    
    deg = -45.0f*M_PI/180.0f;
    DOUBLES_EQUAL(cos(deg),a(0,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
    DOUBLES_EQUAL(sin(deg),a(0,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
    DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
    
    DOUBLES_EQUAL(-sin(deg),a(2,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
    DOUBLES_EQUAL(cos(deg),a(2,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
}

TEST(Matrix, matrixInvEye)
{
    Mat<float, TESTSIZE> a(0.0f , 1.0f);
    
    // do the invert
    a = a.inv();
    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
    DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
    DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
}
TEST(Matrix, matrixEye)
{
    Mat<float, TESTSIZE> a(0.0f , 1.0f);
    
    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
    DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
    DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
}
TEST(Matrix, matrixDeterminantEye)
{
    //eye matrix
    Mat<float, TESTSIZE> a(0.0f, 1.0f);

    DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
}
TEST(Matrix, matrixDeterminantEye3)
{
    //eye matrix
    Mat<float, 3> a(0.0f, 1.0f);

    DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
}
TEST(Matrix, matrixDeterminantSimple)
{
    Mat<float, 1> a;
    a(0,0) = 50.0f;

    DOUBLES_EQUAL(50.0f,a.determinant(), tolerance);
}
TEST(Matrix, matrixDeterminantSimple2)
{
    Mat<float, 2> a;
    a(0,0) = 10.0f;
    a(1,1) = 5.0f;
    DOUBLES_EQUAL(50.0f,a.determinant(), tolerance);
}

TEST(Matrix, matrixDeterminantSimple3)
{
    Mat<float, 2> a;
    a(0,0) = 10.0f;
    a(0,1) = 5.0f;
    a(1,0) = 10.0f;
    a(1,1) = 5.0f;
    DOUBLES_EQUAL(0.0f,a.determinant(), tolerance);
}

TEST(Matrix, matrixDeterminantSimple4)
{
    Mat<float, 2> a;
    a(0,0) = 10.0f;
    a(0,1) = 5.0f;
    a(1,0) = 5.0f;
    a(1,1) = 10.0f;
    DOUBLES_EQUAL(75.0f,a.determinant(), tolerance);
}

TEST(Matrix, matrixTransposeEye)
{
    Mat<float, TESTSIZE> a;
    for(int x = 0 ; x < TESTSIZE ; ++x)
    {
        for (int y = 0; y <TESTSIZE ; ++y)
        {
            float val = 0.0f;
            if (x == y )
                val = 1.0f;
            a(x,y) = val;
        }
    }
    
    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
    DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
    DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);

    a = a.inv();

    // should stille be the same
    DOUBLES_EQUAL(1.0f,a(0,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(0,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(1,0), tolerance);
    DOUBLES_EQUAL(1.0f,a(1,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(1,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(2,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,1), tolerance);
    DOUBLES_EQUAL(1.0f,a(2,2), tolerance);
    DOUBLES_EQUAL(0.0f,a(2,3), tolerance);
    
    DOUBLES_EQUAL(0.0f,a(3,0), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,1), tolerance);
    DOUBLES_EQUAL(0.0f,a(3,2), tolerance);
    DOUBLES_EQUAL(1.0f,a(3,3), tolerance);
}
TEST(Matrix, vectorMultiply)
{
    Mat<int, TESTSIZE> a = testMat;
    Vec<int, TESTSIZE> v = testVec;

    //Vec<int, TESTSIZE> b = a * v;
    for (int x = 0; x <TESTSIZE ; x++)
    {
        int val = 0;
        for (int y = 0; y <TESTSIZE ; y++)
        {
            val += testMat[x][y] * testVec[x];
        }
        //CHECK_EQUAL(val,b(x));
    }
}

TEST(Matrix, scalarDivide)
{
    Mat<int, TESTSIZE> a = testMat;

    a = a / 5;
    for (int x = 0; x <TESTSIZE ; x++)
    {
        for (int y = 0; y <TESTSIZE ; y++)
        {
            int val = testMat[x][y] / 5;
            CHECK_EQUAL(val,a(x,y));
        }
    }
}

TEST(Matrix, scalarSubstract)
{
    Mat<int, TESTSIZE> a = testMat;

    a = a - 5;
    for (int x = 0; x <TESTSIZE ; x++)
    {
        for (int y = 0; y <TESTSIZE ; y++)
        {
            int val = testMat[x][y] - 5;
            CHECK_EQUAL(val,a(x,y));
        }
    }
}

TEST(Matrix, scalarAdd)
{
    Mat<int, TESTSIZE> a = testMat;

    a = a + 5;
    for (int x = 0; x <TESTSIZE ; x++)
    {
        for (int y = 0; y <TESTSIZE ; y++)
        {
            int val = testMat[x][y] + 5;
            CHECK_EQUAL(val,a(x,y));
        }
    }
}

TEST(Matrix, scalarMultiply)
{
    Mat<int, TESTSIZE> a = testMat;

    a = a + 5;
    for (int x = 0; x <TESTSIZE ; x++)
    {
        for (int y = 0; y <TESTSIZE ; y++)
        {
            int val = testMat[x][y] + 5;
            CHECK_EQUAL(val,a(x,y));
        }
    }
}

TEST(Matrix, setIndex)
{
    Mat<int,TESTSIZE> a ; 
    Mat<int,TESTSIZE> b ; 

    int val = 1;
    for (int x = 0; x <TESTSIZE ; x++)
    {
        for (int y = 0; y <TESTSIZE ; y++)
        {
            a(x,y) = val;
            CHECK_EQUAL(val,a(x,y));
            val++;
        }
    }

    for (int x = 0; x <TESTSIZE ; x++)
    {
        for (int y = 0; y <TESTSIZE ; y++)
        {
            CHECK_EQUAL(0,b(x,y));
        }
    }
    b = a;
    val = 1;
    for (int x = 0; x <TESTSIZE ; x++)
    {
        for (int y = 0; y <TESTSIZE ; y++)
        {
            a(x,y) = val;
            CHECK_EQUAL(val,b(x,y));
            val++;
        }
    }
}

TEST(Matrix, initAndSet)
{
    Mat<int,TESTSIZE> a ; 

    for (int x = 0; x <TESTSIZE ; x++)
    {
        for (int y = 0; y <TESTSIZE ; y++)
        {
            int val = (x+1)*TESTSIZE+y;
            a(x,y) = val;
        }
    }
    for (int x = 0; x <TESTSIZE ; x++)
    {
        for (int y = 0; y <TESTSIZE ; y++)
        {
            int val = (x+1)*TESTSIZE+y;
            CHECK_EQUAL(val,a(x,y));
        }
    }
}

TEST(Matrix, initZeroed)
{
    Mat<int,TESTSIZE> a ; 
    for (int x = 0; x <TESTSIZE ; x++)
    {
        for (int y = 0; y <TESTSIZE ; y++)
        {
            CHECK_EQUAL(0 , a(x,y) );
        }
    }
}