#include "Test.h"

#include "Transform_matrix_3D.h"
#include "Defs.h"

#include <math.h>

namespace MM
{

class Transform_matrix_3D_test : public Test
{
    Transform_matrix_3D* link;
public:
    Transform_matrix_3D_test( const Text & suite_name ) : Test( suite_name ) { }

    void run()
    {
        test_move();
        test_rotate();
    }

    void test_move();
    void test_rotate();
};

Transform_matrix_3D_test test_transform_matrix_3D ("correctness");

void Transform_matrix_3D_test::
test_move()
{
    Transform_matrix_3D matrix;
    double x=1, y=1, z=1;

    matrix.apply( x, y, z );
    TEST("Default matrix is identity matrix", x == 1 && y == 1 && z == 1 );

    matrix.move( 1, 1, 1 );
    matrix.apply( x, y, z );
    DOUBLES_EQUAL( x, 2, 0.000001 );
    DOUBLES_EQUAL( y, 2, 0.000001 );
    DOUBLES_EQUAL( z, 2, 0.000001 );

    x=1; y=1; z=1;
    matrix.move( 0, -2, -2 );
    matrix.apply( x, y, z );
    DOUBLES_EQUAL( x, 2, 0.000001 );
    DOUBLES_EQUAL( y, 0, 0.000001 );
    DOUBLES_EQUAL( z, 0, 0.000001 );

    x=1; y=1; z=1;
    matrix.move( -2, 0, 0 );
    matrix.apply( x, y, z );
    DOUBLES_EQUAL( x, 0, 0.000001 );
    DOUBLES_EQUAL( y, 0, 0.000001 );
    DOUBLES_EQUAL( z, 0, 0.000001 );

    x=1; y=1; z=1;
    matrix.load_identity();
    matrix.apply( x, y, z );
    DOUBLES_EQUAL( x, 1, 0.000001 );
    DOUBLES_EQUAL( y, 1, 0.000001 );
    DOUBLES_EQUAL( z, 1, 0.000001 );
}

void Transform_matrix_3D_test::
test_rotate()
{
    Transform_matrix_3D matrix;
    double x=1, y=0, z=0;
    double Pi_2 = __Pi__ / 2.0;

    matrix.rotate_x( Pi_2 );
    matrix.apply( x, y, z );
    // No changes.
    DOUBLES_EQUAL( x, 1, 0.000001 );
    DOUBLES_EQUAL( y, 0, 0.000001 );
    DOUBLES_EQUAL( z, 0, 0.000001 );

    matrix.rotate_y( Pi_2 );
    matrix.apply( x, y, z );
    DOUBLES_EQUAL( x, 0, 0.000001 );
    DOUBLES_EQUAL( y, 0, 0.000001 );
    DOUBLES_EQUAL( z, -1, 0.000001 );

    x=1; y=0; z=0;
    matrix.rotate_y( -3 * Pi_2 / 2 );
    matrix.apply( x, y, z );
    DOUBLES_EQUAL( x, sin(Pi_2 / 2), 0.000001 );
    DOUBLES_EQUAL( y, 0, 0.000001 );
    DOUBLES_EQUAL( z, sin(Pi_2 / 2), 0.000001 );

    x=1; y=0; z=0;
    matrix.rotate_z( Pi_2 );
    matrix.apply( x, y, z );
    DOUBLES_EQUAL( x, 0, 0.000001 );
    DOUBLES_EQUAL( y, sin(Pi_2 / 2), 0.000001 );
    DOUBLES_EQUAL( z, sin(Pi_2 / 2), 0.000001 );
}

}//MM
