// test hmatrix librar
#include <iostream>

#ifdef _WIN32
#include <windows.h>
#endif
#include <math.h>
#include <GL/glut.h>
#ifndef M_PI
#define M_PI            3.14159265358979323846
#endif 
#include "cvec2t.h"
#include "cvec3t.h"
#include "cvec4t.h"
#include "hmatrix.h"

typedef CVec2T<float> Vec2f;
typedef CVec3T<float> Vec3f;
typedef CVec4T<float> Vec4f;
typedef HMatrix<float> HMatrixf;

namespace WindowParams {
  static int WindowWidth = 800;
  static int WindowHeight = 600;
  static int MainWindow; 
};


void testHMatrix() { 
	// test default constructor
    HMatrixf M,M1;
	float m[16];

    // compare to OpenGL matrices
	glMatrixMode(GL_MODELVIEW);

    // 	test setIdentity, Identity 
	M.setIdentity();
	glLoadIdentity();
	glGetFloatv(GL_MODELVIEW_MATRIX,m);
	// test norm, operator -,  explicit conversion from array
    cout << "identity matrix difference " << (M - HMatrixf(m)).frobnorm() <<endl;
	// test setTranslation, Translation
	M.setTranslation( Vec3f(1,2,3));
	glTranslatef(1,2,3);
	glGetFloatv(GL_MODELVIEW_MATRIX,m);
    cout << "translation matrix difference " << (M - HMatrixf(m)).frobnorm() <<endl;
	// test setRotation, Rotation
	M.setRotation( 45*M_PI/180.0, Vec3f(1,2,3));
	glLoadIdentity();
	glRotatef(45,1,2,3);
	glGetFloatv(GL_MODELVIEW_MATRIX,m);
    cout << "rotation matrix difference " << (M - HMatrixf(m)).frobnorm() <<endl;
    // test setScale,Scale
	M.setScale(Vec3f(1,2,3));
    glLoadIdentity();
	glScalef(1,2,3);
	glGetFloatv(GL_MODELVIEW_MATRIX,m);
    cout << "scale matrix difference " << (M - HMatrixf(m)).frobnorm() <<endl;
	M.setRotation( 45*M_PI/180.0,Vec3f(1,2,3));
    M1.setTranslation(Vec3f(1,2,3));
	// test operator * for matrices, operator = 
	M = M*M1;
	glLoadIdentity();
	glRotatef(45,1,2,3);
	glTranslatef(1,2,3);
	glGetFloatv(GL_MODELVIEW_MATRIX,m);
    cout << "matrix multiply difference " << (M - HMatrixf(m)).frobnorm() <<endl;
	// test constructor from elements
    HMatrixf M2(1,2,3,4, 5,6,7,8, 9,10,11,12, 13,14,15,16);
    cout << "matrix from elements " << M2 << endl;
    // test copy constructor
	HMatrixf M3(M2);
	cout << "matrix copy         " << M3 << endl;
    // test conversion to float*
	float* m1 = M2;
	int i;
	cout << "conversion to float* " << endl;
	for( i = 0; i < 16; i++)
		cout << m1[i] << " ";
	cout << endl;
	// test column
	cout << "columns" << endl;
    for(i = 0; i < 4; i++) 
		cout << M2.col(i) << endl;
	// test row
	cout << "rows" << endl;
    for(i = 0; i < 4; i++) 
		cout << M2.row(i) << endl;
	HMatrixf M4,M5;
    // test transpose
	M4.setTranspose(M2);
	// test inverse
	cout << "test transpose " << M4 << endl;
    // build an invertible matrix with nonzero entries
	M5.setRotation(30*M_PI/180.0,Vec3f(1,2,3));
	M5 = M5 + HMatrixf::Translation(Vec3f(1,2,3));
	M5 = M5.transpose() + M5;
	M4.setInverse(M5);
	cout << "test inverse result " << (M4*M5 - HMatrixf::Identity()).frobnorm() << endl;
 	// test scale 
	M4 = M2*(-0.5f);
	cout << "scale test " << M4 << endl;
    // test addition
	cout << "addition test " << M2 + M4*2.0f << endl;
    // test matrix vector mutliply
	Vec4f v(1,2,3,4);
	cout << "matrix vector multiply test" << M2*v << endl; 

	Vec2f v2d(1,2);
	cout << "matrix 2d vector multiply test" << M2*Vec4f(v2d,0,1) << endl; 

	Vec3f v3d(1,2,3);
	cout << "matrix 3d vector multiply test" << M2*Vec4f(v3d,1) << endl; 

}


int main(int argc, char* argv[]) {

  glutInitDisplayMode(GLUT_RGBA|GLUT_DOUBLE|GLUT_DEPTH);

  glutInitWindowSize(WindowParams::WindowWidth,WindowParams::WindowHeight);

  WindowParams::MainWindow = glutCreateWindow("Dummy");

  testHMatrix();
  return 0;
}