/*
QtWagon: a project about 3D objects.
Science and technology promotion license applied. Third party license automatically cascaded.
Zhikai Wang/ www.heteroclinic.net 2013
You can do anything with this file or any file(s) published as part QtWagon project, given this header is kept.
*/
#ifndef __GLORIENTATION
#define __GLORIENTATION

#include "orientation.h"
#include <iostream>
#include <GL/glut.h>

//it is weired we got a o2 here??
template <class T>
class glOrientation: public orientation<T> {
private:
protected:
	////T data[16];
	//mathVector<T> up;
	//mathVector<T> left;
	//mathVector<T> front;
	mathMatrix<T> RYP;

public:
	T * getRYPPointer() const{
		return RYP.getDataPointer();
	}
	// v must be normalized and not zero (norm very small)
	static const mathVector<T> rotateAboutAVector (const mathVector<T> & v, const mathVector<T> & w, T theta) {
		T d1[] = {w.getx(),w.gety(),w.getz()};
		mathMatrix<T> mm;
		mathMatrix<T> m0;
		mathMatrix<T> m1;
		mathVector<T> r;
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		glMatrixMode(GL_MODELVIEW);
		glPushMatrix();
		glLoadIdentity();

#ifdef __USE_DOUBLE
		glGetDoublev(GL_MODELVIEW_MATRIX, m0.getDataPointer());
		glRotated(theta,v.getx(),v.gety(),v.getz());
		glGetDoublev(GL_MODELVIEW_MATRIX, m1.getDataPointer());	
#else
		glGetFloatv(GL_MODELVIEW_MATRIX, m0.getDataPointer());
		glRotatef(theta,v.getx(),v.gety(),v.getz());
		glGetFloatv(GL_MODELVIEW_MATRIX, m1.getDataPointer());
#endif
		//std::cout<<"m0"<<std::endl<<m0<<std::endl;
		//std::cout<<"m1"<<std::endl<<m1<<std::endl;

		mm.setVector(d1);
		//std::cout<<mm<<std::endl;
		glPushMatrix();
#ifdef __USE_DOUBLE
		glMultMatrixd(mm.getDataPointer());
		glGetDoublev(GL_MODELVIEW_MATRIX, mm.getDataPointer());

#else
		glMultMatrixf(mm.getDataPointer());
		glGetFloatv(GL_MODELVIEW_MATRIX, mm.getDataPointer());
#endif
		//std::cout<<mm<<std::endl;
		glPopMatrix();
		r = mm.returnVector();
		//std::cout<<"r: "<<r<<std::endl;

		glPopMatrix();
		glPopAttrib();

		return r;

	}
	virtual void orthorganalizeToWorld() {
		T d1[] = {(T)1.0,(T)0.0,(T)0.0};
		T d2[] = {(T)0.0,(T)1.0,(T)0.0};
		T d3[] = {(T)0.0,(T)0.0,(T)1.0};

		mathMatrix<T> mm;
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		glMatrixMode(GL_MODELVIEW);
		glPushMatrix();
		glLoadIdentity();

#ifdef __USE_DOUBLE
		glGetDoublev(GL_MODELVIEW_MATRIX, RYP.getDataPointer());

		mm.setVector(d1);
		glPushMatrix();
		glMultMatrixd(mm.getDataPointer());
		glGetDoublev(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		left = mm.returnVector();

		mm.setVector(d2);
		glPushMatrix();
		glMultMatrixd(mm.getDataPointer());
		glGetDoublev(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		up = mm.returnVector();

		mm.setVector(d3);
		glPushMatrix();
		glMultMatrixd(mm.getDataPointer());
		glGetDoublev(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		front = mm.returnVector();

#else
		glGetFloatv(GL_MODELVIEW_MATRIX, RYP.getDataPointer());

		mm.setVector(d1);
		glPushMatrix();
		glMultMatrixf(mm.getDataPointer());
		glGetFloatv(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		left = mm.returnVector();

		mm.setVector(d2);
		glPushMatrix();
		glMultMatrixf(mm.getDataPointer());
		glGetFloatv(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		up = mm.returnVector();

		mm.setVector(d3);
		glPushMatrix();
		glMultMatrixf(mm.getDataPointer());
		glGetFloatv(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		front = mm.returnVector();
#endif

		glPopMatrix();
		glPopAttrib();
		left.normalize();
		up.normalize();
		front.normalize();
	}
	virtual void rotateAboutMatrix( const mathVector<T> & v, T theta)  {
		T d1[] = {1.0,0.0,0.0};
		T d2[] = {0.0,1.0,0.0};
		T d3[] = {0.0,0.0,1.0};

		mathMatrix<T> mm;
		// Multiply to current matrix(MatrixYPR), and save the result to MatrixYPR
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		glMatrixMode(GL_MODELVIEW);
		glPushMatrix();
		glLoadIdentity();

#ifdef __USE_DOUBLE
		glRotated(theta,v.getx(),v.gety(),v.getz());
		glMultMatrixd(RYP.getDataPointer());
		glGetDoublev(GL_MODELVIEW_MATRIX, RYP.getDataPointer());
		mm.setVector(d1);
		glPushMatrix();
		glMultMatrixd(mm.getDataPointer());
		glGetDoublev(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		left = mm.returnVector();
		mm.setVector(d2);
		glPushMatrix();
		glMultMatrixd(mm.getDataPointer());
		glGetDoublev(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		up = mm.returnVector();
		mm.setVector(d3);
		glPushMatrix();
		glMultMatrixd(mm.getDataPointer());
		glGetDoublev(GL_MODELVIEW_MATRIX, mm.getDataPointer());
#else
		glRotatef(theta,v.getx(),v.gety(),v.getz());
		glMultMatrixf(RYP.getDataPointer());
		glGetFloatv(GL_MODELVIEW_MATRIX, RYP.getDataPointer());
		mm.setVector(d1);
		glPushMatrix();
		glMultMatrixf(mm.getDataPointer());
		glGetFloatv(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		left = mm.returnVector();
		mm.setVector(d2);
		glPushMatrix();
		glMultMatrixf(mm.getDataPointer());
		glGetFloatv(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		up = mm.returnVector();
		mm.setVector(d3);
		glPushMatrix();
		glMultMatrixf(mm.getDataPointer());
		glGetFloatv(GL_MODELVIEW_MATRIX, mm.getDataPointer());
#endif
		glPopMatrix();
		front = mm.returnVector();
		glPopMatrix();
		glPopAttrib();
		left.normalize();
		up.normalize();
		front.normalize();
	}


	//this method is too complex, but the underlying is intuitive.
	virtual void rotateAboutRYP( const mathVector<T> & v, T theta)  {
		mathVector<T> i2;
		mathVector<T> t1;
		T a1 = angleBetweenVW<T> (getUpVectorRef() , v, i2 );
		i2.normalize();
		if ( isAlmost<T>(a1,(T)0.0) || isAlmost<T>(a1,(T)180.0)) {
			if ( isAlmost<T>(a1,(T)0.0) ) 
				yaw(theta);
			if ( isAlmost<T>(a1,(T)180.0) )
				yaw((T)-1.0*theta);

		} else {
			mathVector<T> t2;
			T a2 = angleBetweenVW<T> ( getLeftVectorRef(),i2 , t2 );
			t2.normalize();
			mathVector<T> t3;
			T a3 = angleBetweenVW<T> (t2, getUpVectorRef() , t3 );
			t3.normalize();
			if ( isAlmost<T>(a3,(T)0.0) || isAlmost<T>(a3,(T)180.0)) {
				if ( isAlmost<T>(a3,(T)0.0) ) 
					yaw(a2);

				if ( isAlmost<T>(a3,(T)180.0) )
					yaw((T)-1.0*a2);
				pitch((T)-1.0*a1);
				yaw(theta);
				pitch(a1);
				if ( isAlmost<T>(a3,(T)0.0) ) 
					yaw((T)-1.0*a2);

				if ( isAlmost<T>(a3,(T)180.0) )
					yaw(a2);
			} else {
				//
				std::cout<<"In rotate about a3 is not 0 or 180, please contact the developer!"<<std::endl;
				exit(1);
			}
		}
	}

	virtual void RollYawPitch(unsigned char lastElement, T ROTATION_ANGLE)
	{
		T d1[] = {(T)1.0,(T)0.0,(T)0.0};
		T d2[] = {(T)0.0,(T)1.0,(T)0.0};
		T d3[] = {(T)0.0,(T)0.0,(T)1.0};

		mathMatrix<T> mm;
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		glMatrixMode(GL_MODELVIEW);
		glPushMatrix();
		glLoadIdentity();
#ifdef __USE_DOUBLE
		glMultMatrixd(RYP.getDataPointer());
#else
		glMultMatrixf(RYP.getDataPointer());
#endif

		switch (lastElement)
		{
		case 'Y':
		case 'y':// Yaw: rotate the object by yaw_Angle about y-axis
#ifdef __USE_DOUBLE
			glRotated(ROTATION_ANGLE, (T)0.0, (T)1.0, (T)0.0);
#else
			glRotatef(ROTATION_ANGLE, (T)0.0, (T)1.0, (T)0.0);
#endif
				
			break;
		case 'P':
		case 'p':// Pitch: rotate the object by pitch_Angle about x-axis; pull-up
#ifdef __USE_DOUBLE
			glRotated((T)-1.0 * ROTATION_ANGLE,(T) 1.0,(T) 0.0,(T) 0.0);	
#else
			glRotatef((T)-1.0 * ROTATION_ANGLE, (T)1.0,(T) 0.0,(T) 0.0);	
#endif
			
			break;
		case 'R':
		case 'r':// Roll: rotate the object by roll_Angle about z-axis
#ifdef __USE_DOUBLE
			glRotated(ROTATION_ANGLE, (T)0.0, (T)0.0,(T) 1.0);	
#else
			glRotatef(ROTATION_ANGLE,(T) 0.0,(T) 0.0,(T) 1.0);	
#endif
			
			break;
		}
#ifdef __USE_DOUBLE
		glGetDoublev(GL_MODELVIEW_MATRIX, RYP.getDataPointer());

		mm.setVector(d1);
		glPushMatrix();
		glMultMatrixd(mm.getDataPointer());
		glGetDoublev(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		left = mm.returnVector();

		mm.setVector(d2);
		glPushMatrix();
		glMultMatrixd(mm.getDataPointer());
		glGetDoublev(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		up = mm.returnVector();

		mm.setVector(d3);
		glPushMatrix();
		glMultMatrixd(mm.getDataPointer());
		glGetDoublev(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		front = mm.returnVector();

#else
		glGetFloatv(GL_MODELVIEW_MATRIX, RYP.getDataPointer());

		mm.setVector(d1);
		glPushMatrix();
		glMultMatrixf(mm.getDataPointer());
		glGetFloatv(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		left = mm.returnVector();

		mm.setVector(d2);
		glPushMatrix();
		glMultMatrixf(mm.getDataPointer());
		glGetFloatv(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		up = mm.returnVector();

		mm.setVector(d3);
		glPushMatrix();
		glMultMatrixf(mm.getDataPointer());
		glGetFloatv(GL_MODELVIEW_MATRIX, mm.getDataPointer());
		glPopMatrix();
		front = mm.returnVector();
#endif

		glPopMatrix();
		glPopAttrib();
		left.normalize();
		up.normalize();
		front.normalize();
	}
	void printRYP() { // for debug purpose ONLY
		std::cout<<"RYP:"<<std::endl<<RYP<<std::endl;
	}
	virtual void roll(T degree = (T)1.0){
		RollYawPitch('r',degree);
	};
	virtual void yaw(T degree = (T)1.0) {
		RollYawPitch('y',degree);
	};
	virtual void pitch(T degree =(T)1.0) {
		RollYawPitch('p',degree);
	};

	virtual void draw(T scale = (T)1.0) {
		scale = (T)1.0;
	}
	virtual void drawAxis(T scale = (T)1.0)  {
		scale = (T)1.0;
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		glPushMatrix();
#ifdef __USE_DOUBLE
				glMultMatrixd(RYP.getDataPointer());
#else
				glMultMatrixf(RYP.getDataPointer());
#endif
		glDisable(GL_LIGHTING);
		drawOrthorgonalAxis();
		glPopMatrix();
		glPopAttrib();
	}

	// If you don't dis-able the light, the colors won't be shown.
	void drawOrthorgonalAxis(T scale = (T)1.0) {
#ifdef __USE_DOUBLE
		glColor3d((T)1.0,(T)0.0,(T)0.0);
		glBegin(GL_LINES);
		glEnd();
		glColor4d((T)1.0,(T)0.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3d((T)0.0,(T)0.0,(T)0.0);
		glVertex3d(scale*((T)1.0),(T)0.0,(T)0.0);
		glEnd();

		glColor4d((T)0.0,(T)1.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		//glVertex3d
		glVertex3d((T)0.0,(T)0.0,(T)0.0);
		glVertex3d((T)0.0,scale*((T)1.0),(T)0.0);
		glEnd();

		glColor4d((T)0.0,(T)0.0,(T)1.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3d((T)0.0,(T)0.0,(T)0.0);
		glVertex3d((T)0.0,(T)0.0,scale*((T)1.0));
		glEnd();

#else
		glColor3f((T)1.0,(T)0.0,(T)0.0);
		glBegin(GL_LINES);
		glEnd();
		glColor4f((T)1.0,(T)0.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3f((T)0.0,(T)0.0,(T)0.0);
		glVertex3f(scale*((T)1.0),(T)0.0,(T)0.0);
		glEnd();

		glColor4f((T)0.0,(T)1.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		//glVertex3d
		glVertex3f((T)0.0,(T)0.0,(T)0.0);
		glVertex3f((T)0.0,scale*((T)1.0),(T)0.0);
		glEnd();

		glColor4f((T)0.0,(T)0.0,(T)1.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3f((T)0.0,(T)0.0,(T)0.0);
		glVertex3f((T)0.0,(T)0.0,scale*((T)1.0));
		glEnd();

#endif


	}
	//virtual void draw() {// for debug purpose
	void draw2(T scale = (T)1.0) {
#ifdef __USE_DOUBLE
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		glPushMatrix();
		glDisable(GL_LIGHTING);
		glColor3d((T)1.0,(T)0.0,(T)0.0);

		glColor4d((T)1.0,(T)0.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3d((T)0.0,(T)0.0,(T)0.0);
		glVertex3d(scale*left.getx(),scale*left.gety(),scale*left.getz());
		glEnd();

		glColor4d((T)0.0,(T)1.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3d((T)0.0,(T)0.0,(T)0.0);
		glVertex3d(scale*up.getx(),scale*up.gety(),scale*up.getz());
		glEnd();

		glColor4d((T)0.0,(T)1.0,(T)1.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3d((T)0.0,(T)0.0,(T)0.0);
		glVertex3d(scale*front.getx(),scale*front.gety(),scale*front.getz());
		glEnd();

		glPopMatrix();
		//glEnable(GL_LIGHTING);
		glPopAttrib();
#else
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		glPushMatrix();
		glDisable(GL_LIGHTING);
		glColor3f((T)1.0,(T)0.0,(T)0.0);

		glColor4f((T)1.0,(T)0.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3f((T)0.0,(T)0.0,(T)0.0);
		glVertex3f(scale*left.getx(),scale*left.gety(),scale*left.getz());
		glEnd();

		glColor4f((T)0.0,(T)1.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3f((T)0.0,(T)0.0,(T)0.0);
		glVertex3f(scale*up.getx(),scale*up.gety(),scale*up.getz());
		glEnd();

		glColor4f((T)0.0,(T)1.0,(T)1.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3f((T)0.0,(T)0.0,(T)0.0);
		glVertex3f(scale*front.getx(),scale*front.gety(),scale*front.getz());
		glEnd();

		glPopMatrix();
		//glEnable(GL_LIGHTING);
		glPopAttrib();
#endif

	}

	void draw1(T scale = (T)1.0) {

#ifdef __USE_DOUBLE
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		glPushMatrix();
		glMultMatrixd(RYP.getDataPointer());
		glDisable(GL_LIGHTING);
		glColor3d((T)1.0,(T)0.0,(T)0.0);
		glBegin(GL_LINES);
		glEnd();
		glColor4d((T)1.0,(T)0.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3d((T)0.0,(T)0.0,(T)0.0);
		glVertex3d(scale*((T)1.0),(T)0.0,(T)0.0);
		glEnd();

		glColor4d((T)0.0,(T)1.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		//glVertex3d
		glVertex3d((T)0.0,(T)0.0,(T)0.0);
		glVertex3d((T)0.0,scale*((T)1.0),(T)0.0);
		glEnd();

		glColor4d((T)0.0,(T)0.0,(T)1.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3d((T)0.0,(T)0.0,(T)0.0);
		glVertex3d((T)0.0,(T)0.0,scale*((T)1.0));
		glEnd();
		glPopMatrix();
		//glEnable(GL_LIGHTING);
		glPopAttrib();
#else
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		glPushMatrix();
		glMultMatrixf(RYP.getDataPointer());
		glDisable(GL_LIGHTING);
		glColor3f((T)1.0,(T)0.0,(T)0.0);
		glBegin(GL_LINES);
		glEnd();
		glColor4f((T)1.0,(T)0.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3f((T)0.0,(T)0.0,(T)0.0);
		glVertex3f(scale*((T)1.0),(T)0.0,(T)0.0);
		glEnd();

		glColor4f((T)0.0,(T)1.0,(T)0.0,(T)1.0);
		glBegin(GL_LINES);
		//glVertex3d
		glVertex3f((T)0.0,(T)0.0,(T)0.0);
		glVertex3f((T)0.0,scale*((T)1.0),(T)0.0);
		glEnd();

		glColor4f((T)0.0,(T)0.0,(T)1.0,(T)1.0);
		glBegin(GL_LINES);
		glVertex3f((T)0.0,(T)0.0,(T)0.0);
		glVertex3f((T)0.0,(T)0.0,scale*((T)1.0));
		glEnd();
		glPopMatrix();
		//glEnable(GL_LIGHTING);
		glPopAttrib();

#endif
	};
	virtual void apply() {
	};

	//// you have to re-write these constructors
	glOrientation(const T nup[],const T nleft[],const T nfront[]):
	orientation<T>(nup,nleft,nfront)
	{
		//normalize();
	}
	glOrientation():orientation<T>()
		//orientation({0.0,1.0,0.0},{1.0,0.0,0.0},{0.0,0.0,1.0})// up/opengl y; left/opengl x; forward opengl/z
	{
	}

	virtual ~ glOrientation() {
	}
	glOrientation(const glOrientation & nglo) {
		//setData(nmm.getDataPointer());
		up = nglo.up;
		left = nglo.left;
		front = nglo.front;
		RYP = nglo.RYP;
	}
	// assignment operator
	glOrientation & operator=(const glOrientation & nglo) {
		if (this == &nglo) return *this;  // time-saving self-test
		orientation::operator = (nglo);
		RYP = nglo.RYP;
		return *this;                    // for daisy-chaining
	}

	// not like the vector/point, vector can use pint's in/out
	template <class U> 
	friend std::ostream & operator << (std::ostream & os,const glOrientation<U> & mm);
	template <class U> 
	friend std::istream & operator >> (std::istream & is, glOrientation<U> & mm);

};
template <class U> 
std::ostream & operator << (std::ostream & os,const glOrientation<U> & or) {
	os<<or.RYP;
	os<<(orientation<U>&)or;
	return os;

};
template <class U> 
std::istream & operator >> (std::istream & is, glOrientation<U> & or ) {
	is>>or.RYP;
	is>>(orientation<U>&)or;
	return is;
};

#endif