#if !defined(QUATERNION)
        #define QUATERNION
        #include "quaternion.h"
#endif

// ----------------------------------------------------
// ---------        quaternion(void)     --------------

 quaternion::quaternion(void)
{
	// by default we set an identity rotation around the vector e1
	theta=0.0;
	alpha=kpi/2.0;
	beta=0.0;
}

// ----------------------------------------------------
// ---------    quaternion(v, theta)     --------------

 quaternion::quaternion(vector3d v, double t)
{

	double dpi=2.0*kpi;

	try
	{
		theta=modulo(t,kpi);
		if(v.norm()<ktol)
			throw "quaternion::quaternion(v,t), (W) v must not be a null vector.";
		vector3d u=v/v.norm();
		// first we need to normalise the vector
	
		alpha=acos(u[3]);
		if((alpha<ktol) || (fabs(alpha-kpi)<ktol))
		{
			// cerr << "quaternion::quaternion(v,t), (I) vector along e3.\n";
			beta=0.0;
		}
		else
		{
			double cp1=u[1]/sin(alpha);
			if(cp1>1.0)
				cp1=1.0;
			if(cp1<-1.0)
				cp1=-1.0;
			beta=acos(cp1);
			if(fabs(u[2]-sin(alpha)*sin(dpi-beta))<ktol)
				beta=dpi-beta;	
		}
	}
	catch(const char* message)
	{
		cerr << message << "\n";
		// use defaults values
		alpha=kpi/2.0;
		beta=0.0;
	}
}


// ----------------------------------------------------
// ---------  quaternion(theta, alpha, beta) ----------

 quaternion::quaternion(double t, double a, double b)
{
	double dpi=2.0*kpi;

	theta=modulo(t,kpi);
	alpha=modulo(a,kpi);
	beta=modulo(b,dpi);	
}

// ----------------------------------------------------
// ---------           operator=             ----------

 quaternion quaternion::operator=(const quaternion& q)
{
	if(this==&q)
		return *this;
	else
	{
		theta=q.theta;
		alpha=q.alpha;
		beta=q.beta;
		return *this;
	}
}

// ----------------------------------------------------
// ---------           operator~             ----------

 quaternion quaternion::operator~(void)
{
	quaternion tmp;

	tmp.theta=theta;
	tmp.alpha=kpi-alpha;
	tmp.beta=modulo(kpi+beta,2.0*kpi);

	return tmp;
}

// ----------------------------------------------------
// ---------           operator[]            ----------

double& quaternion::operator[](const int i)
{
	try
	{
		if((i<1) || (i>3))
			throw "quaternion::operator[], (C) index out of range.";
		switch(i)
		{
			case 1:
				return theta;
				break;

			case 2:
				return alpha;
				break;

			case 3:
				return beta;
				break;
		}
	}
	catch(const char* message){cerr << message << "\n";}
	return dummy;
}

// ----------------------------------------------------
// ---------               vect()            ----------

vector3d quaternion::vect(void)
{
	double sina=sin(alpha);
//cerr << "q.vect():" << alpha << ", " << beta << endl;
	return vector3d(sina*cos(beta), sina*sin(beta), cos(alpha));
}

// ----------------------------------------------------
// ---------           operator*             ----------

 quaternion operator*(const quaternion& a, const quaternion& b)
{
	double a0,a1,a2,a3,b0,b1,b2,b3,p0,p1,p2,p3;		// a and b coordinates
	double beta1, beta2;
	double sinat=sin(a.theta);
	double sinbt=sin(b.theta);
	double sinaa=sin(a.alpha);
	double sinba=sin(b.alpha);
	quaternion tmp;
	double dpi=2.0*kpi;


	try
	{
			// compute a and b cartesian coord. from the angles
		a0=cos(a.theta);
		a1=sinat*sinaa*cos(a.beta);
		a2=sinat*sinaa*sin(a.beta);
		a3=sinat*cos(a.alpha);		

		b0=cos(b.theta);	
		b1=sinbt*sinba*cos(b.beta);
		b2=sinbt*sinba*sin(b.beta);
		b3=sinbt*cos(b.alpha);	


			// compute the product coordinates
		p0=a0*b0-(a1*b1+a2*b2+a3*b3);
		p1=a0*b1+a1*b0+a2*b3-a3*b2;
		p2=a0*b2+a2*b0-a1*b3+a3*b1;
		p3=a0*b3+a3*b0+a1*b2-a2*b1;

			// and now; compute the angle back	 		

		// There is a short problem because a rotation of theta around the vector U can not
		// be separate from a rotation of 2pi-theta around -U. These two rotations are represented
		// by the same quaternion. An it is true that they are equivalent. In the way we determine
		// theta in this part, we have theta between 0 and pi. A rotation of more than pi around a
		// a given direction U  will
		// therefore become a rotation of 2pi-theta around -U. Physically speaking, they can not be distinguish
		// because they will give the same image of vector. 

		double p=sqrt(p0*p0+p1*p1+p2*p2+p3*p3);
		if (p<ktol)
			throw  "quaternion::operator*, (C) null quaternion as result is not a unit quaternion.\n";
			// here we make a small renomalisation in order to be sure that each coordinates are less than zero
		p0=p0/p;
		p1=p1/p;
		p2=p2/p;
		p3=p3/p;
 
 		double cp1=p0;
	
			// the foolowing lines should not be necessary as a renomalisation is made before
			// but we never now ...
		if(cp1>1.0)
			cp1=1.0;
		if(cp1<-1.0)
			cp1=-1.0;
	
		tmp.theta=acos(cp1);
//cerr << "theta " << tmp.theta << endl;
		if((tmp.theta<ktol) || (fabs(tmp.theta-kpi)<ktol))
		{
			// it is a trivial rotation. by default we set it to be of 0  around the e1 direction
			tmp.beta=0.0;
			tmp.alpha=kpi/2.0;
		}
		else
		{
			double sint=sin(tmp.theta);
			double cp3=p3/sint;
			if(cp3>1.0)
				cp3=1.0;
			if(cp3<-1.0)
				cp3=-1.0;
			tmp.alpha=acos(cp3);
//cerr << "alpha " << tmp.alpha << endl;

				
			if((tmp.alpha<ktol) || (fabs(tmp.alpha-kpi)<ktol))
				// it is a rotation around the e3 direction so we set beta=0
				tmp.beta=0.0;
			else
			{
				double cp1=p1/(sint*sin(tmp.alpha));
				if(cp1>1.0)
					cp1=1.0;
				if(cp1<-1.0)
					cp1=-1.0;
				tmp.beta=acos(cp1);
//cerr << "beta " << tmp.beta << endl;

				// here we check because theta is between 0 and 2*pi				
				if(fabs(p2-sint*sin(tmp.alpha)*sin(dpi-tmp.beta))<ktol)
					tmp.beta=dpi-tmp.beta;		
				
			}
		}
		return tmp;
	}
	catch(const char* message)
	{
		cerr << message << "\n";
		return quaternion(knan,knan,knan);
	}
}

// ----------------------------------------------------
// ---------           operator<<            ----------

ostream& operator<<(ostream& out, quaternion& q)
{
	vector3d u;
	double sina=sin(q.alpha);
	
	u[1]=sina*cos(q.beta);
	u[2]=sina*sin(q.beta);
	u[3]=cos(q.alpha);
	
	out << q.theta << " " << u;
	
	return out;

}

// ----------------------------------------------------
// ---------             display             ----------
ostream& quaternion::display(ostream& out)
{
	double sina=sin(alpha);
	double sint=sin(theta);
	
	out << cos(theta) << " " << sint*sina*cos(beta) << " " << sint*sina*sin(beta) << " " << sint*cos(alpha) << "\n";
	
	return out;
}

// ----------------------------------------------------
// ---------           operator>>            ----------

istream& operator>>(istream& in, quaternion &q)
{
	double tmp[4];
	double qn;
	double dpi=2.0*kpi;
	
	
	try
	{
		for(int i=0;i<=3;i++)
			in >> tmp[i];
		
		q.theta=modulo(tmp[0],kpi);
		qn=sqrt(sqr(tmp[1])+sqr(tmp[2])+sqr(tmp[3]));
		if(qn<ktol)
			throw "quaternion::operator>>, (C) null quaternion is not a unit quaternion";
		q.alpha=acos(tmp[3]/qn);
		if((q.alpha<ktol) || (fabs(q.alpha-kpi)<ktol))
		{
			// cerr <<  "quaternion::operator>>, (I) vector along e3\n";
			q.beta=0.0;
		}
		else
		{
			double cp1=tmp[1]/(qn*sin(q.alpha));
			if(cp1>1.0)
				cp1=1.0;
			if(cp1<-1.0)
				cp1=-1.0;
			q.beta=acos(cp1);
			
			if(fabs(tmp[2]-sin(q.alpha)*sin(dpi-q.beta))<ktol)
				q.beta=dpi-q.beta;	
		}
	}
	catch(const char* message)
	{
		cerr << message << "\n";
		q.alpha=q.beta=q.theta=knan;
	}
	return in;
}

/*// ----------------------------------------------------
// ---------        move(dt,da,db)           ----------

 void quaternion::move(double dt, double da, double db)
{
	double dpi=2.0*kpi;

	theta = modulo(theta+dt,kpi);	// move theta
	beta = modulo(beta+db,dpi);	// move beta
		// moving alpha require more attention
	if(da>=0.0)
		if((kpi-alpha)<=da)	// check if alpha+da will acceed pi
		{
			alpha = alpha + da -kpi; 
			beta = modulo(beta+kpi,dpi);	// beta has also to change
		}
		else
			alpha+=da;
	else
	{
		da=-da;		// take the absolute value of da	
		if(alpha<da)// check if alpha-|da| will be less than 0
		{
			alpha = da - alpha;
			beta = modulo(beta+kpi,dpi);
		}
	}
}*/

// ----------------------------------------------------
// ---------        move(dt,da,db)           ----------

void quaternion::move(double dt, double da, double db)
{
	double dpi=2.0*kpi;

	theta = modulo(theta+dt,kpi);	// move theta
	beta = modulo(beta+db,dpi);	// move beta
	alpha = modulo(alpha+da,kpi);	// move alpha
}
/*
// ----------------------------------------------------
// ---------        move(dt,da,db)           ----------

void quaternion::move(double dt, double da, double db)
{
	double dpi=2.0*kpi;
		// La on est tres brutal !!!!
	theta += dt;	// move theta
	beta += db;	// move beta
	alpha += da;	// move alpha
} */


// ----------------------------------------------------
// ---------            rotate(v)            ----------

vector3d quaternion::rotate(vector3d u)
{
	quaternion q=(*this);
	quaternion qp=~q;
	
	return (q*u*qp).vect();		
}