/**
 * @addtogroup testsuite
 * @{
 * @addtogroup matrixtests
 * @{
 * @author  Philipp Schoenberger <ph.schoenberger@googlemail.com>
 * @version 1.0
 *
 * @section LICENSE
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details at
 * https://www.gnu.org/copyleft/gpl.html
 *
 * @section DESCRIPTION
 *
 * This file contains the Trajectory for a bang bang trajectory.
 * The bang bang trajectory is a trajectory with linear acceleration
 * phase followed by a direct de-acceleration phase
 *
 * The slowest joint is defining the speed of the other joints.
 * By that all joints start and stop the movement synchronously
 */
#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;

/**
 * testgroup for the matrix class
 */
TEST_GROUP(Matrix)
{
    /**
     * setup for for all test suite
     * This Function is called before every Test case
     */
    void setup()
    {
        // initializing the testmatrix 
        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++;
        }
    }

    /**
     * exit for for all test suite
     * This Function is called after every Test case
     */
    void teardown()
    {
    }
};

/**
 * Test the matrix for invert an non identity matrix 
 * with a x rotation
 */
TEST(Matrix, vectorInvRotX)
{
    // setup the test matrix to an identity matrix
    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;
        }
    }

    // set the rotation matrix to 45 degree in z
    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;

    // check the matrix after setting it up
    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);

    // invert the matrix 
    a = a.inv();

    // check for the negative angle for a z rotation
    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 the matrix for invert an non identity matrix
 * with a y rotation
 */
TEST(Matrix, vectorInvRotY)
{
    // setup the test matrix to an identity matrix
    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 the matrix for invert an identity matrix
 */
TEST(Matrix, matrixInvEye)
{
    Mat<float, TESTSIZE> a(0.0f , 1.0f);
    
    // do the invert
    a = a.inv();

    // check if the matrix is still an identity matrix
    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 if the matrix constructor for identity matrix is working
 */
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 if the determinant function is working with an identity matrix
 * This should always return an 1
 */
TEST(Matrix, matrixDeterminantEye)
{
    //eye matrix
    Mat<float, TESTSIZE> a(0.0f, 1.0f);

    DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
}

/**
 * Test if the determinant function is working with an identity matrix
 * with only 3 dimensions.
 * This should always return an 1
 */
TEST(Matrix, matrixDeterminantEye3)
{
    //eye matrix
    Mat<float, 3> a(0.0f, 1.0f);

    DOUBLES_EQUAL(1.0f,a.determinant(),tolerance);
}

/**
 * Test if the determinant function is working with an identity matrix
 * with 1 dimension should always return the first cell
 */
TEST(Matrix, matrixDeterminantSimple)
{
    Mat<float, 1> a;
    a(0,0) = 50.0f;

    DOUBLES_EQUAL(50.0f,a.determinant(), tolerance);
}

/**
 * Test if the determinant function is working with an identity matrix
 * with 2 dimension should always return the multiplication of the diagonal
 */
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 if the determinant function is working with an complete non zero matrix
 * with 2 dimension should always return the multiplication of the diagonal
 * and negative counter diagonal multiplication
 */
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 if the determinant function is working with an complete non zero matrix
 * with 2 dimension should always return the multiplication of the diagonal
 * and negative counter diagonal multiplication
 */
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 if the transpose function is working with an identity matrix 
 * with 4 dimension should always be the same again 
 */
TEST(Matrix, matrixTransposeEye)
{
    //eye matrix
    Mat<float, TESTSIZE> a(0.0f,1.0f);
    
    // check if the identity matrix is correctly created
    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);

    // invert the identity matrix
    a = a.inv();

    // should still 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 if the transpose function is working with an identity matrix 
 * with 4 dimension should always be the same again 
 */
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) );
        }
    }
}