#include <stdio.h>
#include "matrivec.h"

const int X = 0, Y = 1, Z = 2, W = 3;

class quaternion
{
	public:
	
		quaternion()
		{};
		quaternion(double a, double b, double c, double d)
		{vals[X] = a; vals[Y] = b; vals[Z] = c; vals[W] = d;};
		~quaternion()
		{};
		
		double norm();
		double& operator [] (int index);

		quaternion operator * (quaternion q);
		void operator = (quaternion q);
		void normalize();
		
		matrix createRotationMatrix();
		void makeRotationQuaternion(double x, double y, double z, double angle);
		void createFromRotationMatrix(matrix& r);
		
		void print();
		
		double vals[4];
};

quaternion quaternion::operator * (quaternion q)
{ 
	quaternion qq; 
	qq.vals[W] = vals[W] * q.vals[W] - vals[X] * q.vals[X] - vals[Y] * q.vals[Y] - vals[Z] * q.vals[Z];
	qq.vals[X] = vals[W] * q.vals[X] + vals[X] * q.vals[W] + vals[Y] * q.vals[Z] - vals[Z] * q.vals[Y];
	qq.vals[Y] = vals[W] * q.vals[Y] + vals[Y] * q.vals[W] + vals[Z] * q.vals[X] - vals[X] * q.vals[Z];
	qq.vals[Z] = vals[W] * q.vals[Z] + vals[Z] * q.vals[W] + vals[X] * q.vals[Y] - vals[Y] * q.vals[X];
	return qq;
} 

void quaternion::operator = (quaternion q)
{
	for(int i = 0; i < 4; i++)
		vals[i] = q.vals[i];
}

double& quaternion::operator [] (int index)
{
	return vals[index];
}

double quaternion::norm()
{
	return vals[X] * vals[X] + vals[Y] * vals[Y] + vals[Z] * vals[Z] + vals[W] * vals[W];
}

/*****************************************************************************
 * 
    q_make:  make a quaternion given an axis and an angle (in radians)
         
        notes:
            - rotation is counter-clockwise when rotation axis vector is 
                pointing at you
            - if angle or vector are 0, the identity quaternion is returned.
            
    q_type destQuat :    quat to be made  
    double x, y, z  :    axis of rotation 
    double angle    :    angle of rotation about axis in radians 
*
*****************************************************************************/
void quaternion::makeRotationQuaternion(double x, double y, double z, double angle)
{
  	double length, sinA;
  	/* normalize vector */
  	length = sqrt(x * x + y * y + z * z);
  	/* if zero vector passed in, just return identity quaternion    */
  	if(length < 1e-12)
  	{
		vals[X] = vals[Y] = vals[Z] = 0;
      vals[W] = 1;
      return;
   }
  	sinA = sin(angle / 2);
  	vals[W] = cos(angle / 2);
  	vals[X] = sinA * x / length;
  	vals[Y] = sinA * y / length;
  	vals[Z] = sinA * z / length;
}

/*****************************************************************************
 * q_normalize-  normalize quaternion.  src and dest can be same
 *****************************************************************************/
void quaternion::normalize()
{
	if(norm() < 1e-12)
	{
		cout << "Error in quaternion::norm: quaternion has norm 0!\n";
		exit(0);
	}
	double normFactor = 1 / sqrt(vals[X] * vals[X] + vals[Y] * vals[Y] + vals[Z] * vals[Z] + vals[W] * vals[W]);
	for(int i = 0; i < 4; i++)
	  	vals[i] *= normFactor;
}

/* Construct rotation matrix from (possibly non-unit) quaternion. 
* Assumes matrix is used to multiply column vector on the left: 
* vnew = mat vold. Works correctly for right-handed coordinate system 
* and right-handed rotations. */ 
//R = [  1 - 2(y^2 + z^2)  2(xy - wz)        2(xz + wy)         ] * a scalar
//		[  2(xy + wz)        1 - 2(x^2 + z^2)  2(yz - wx)         ]
//		[  2(xz - wy)        2(yz + wx)        1 - 2(x^2 + y^2)   ]
// (treat matrix as though it were homogeneous and 4 x 4)
matrix quaternion::createRotationMatrix()
{ 
	normalize();
	matrix r(3, 3);
	r[X][X] = 1 - 2 * (vals[Y] * vals[Y] + vals[Z] * vals[Z]);
	r[X][Y] = 2 * (vals[X] * vals[Y] + vals[W] * vals[Z]);
	r[X][Z] = 2 * (vals[X] * vals[Z] - vals[W] * vals[Y]);
	r[Y][X] = 2 * (vals[X] * vals[Y] - vals[W] * vals[Z]);
	r[Y][Y] = 1 - 2 * (vals[X] * vals[X] + vals[Z] * vals[Z]);
	r[Y][Z] = 2 * (vals[Y] * vals[Z] + vals[W] * vals[X]);
	r[Z][X] = 2 * (vals[X] * vals[Z] + vals[W] * vals[Y]);
	r[Z][Y] = 2 * (vals[Y] * vals[Z] - vals[W] * vals[X]);
	r[Z][Z] = 1 - 2 * (vals[X] * vals[X] + vals[Y] * vals[Y]);
	return r;
}

/* Construct a unit quaternion from rotation matrix. Assumes matrix is 
* used to multiply column vector on the left: v_new = r * v_old. Works 
* correctly for right-handed coordinate system and right-handed rotations. 
* Translation and perspective components ignored. */ 
//R = [  1 - 2(y^2 + z^2)  2(xy - wz)        2(xz + wy)         ] * a scalar
//		[  2(xy + wz)        1 - 2(x^2 + z^2)  2(yz - wx)         ]
//		[  2(xz - wy)        2(yz + wx)        1 - 2(x^2 + y^2)   ]
// (treat matrix as though it were homogeneous and 4 x 4)

// r[0][1] + r[1][0] = 4xy                 know xy
// r[0][2] + r[2][0] = 4xz                 know xz => y/z = xy/xz
// r[1][2] + r[2][1] = 4yz                 know yz => y = sqrt(yz * y/z) => know x, z
// r[0][1] = 2xy - 2wz                     know w

void quaternion::createFromRotationMatrix(matrix& r)
{ 												//calculate x, y, z, w of a rotation quaternion from the corresponding rotation matrix
	if(r.numRows != 3 || r.numCols != 3)
	{
		cout << "Error in quaternion::create from rotation matrix: matrix wrong size (" 
			  << r.numRows << " x " << r.numCols << ")!\n";
		exit(0);
	}
	double xy = (r[0][1] + r[1][0]) / 4, xz = (r[0][2] + r[2][0]) / 4, ydz = xy / xz, yz = (r[1][2] + r[2][1]) / 4;
	vals[Y] = sqrt(yz * ydz);
	vals[X] = xy / vals[Y];
	vals[Z] = yz / vals[Y];
	vals[W] = (r[0][1] - 2 * xy) / (2 * vals[Z]);
}

void quaternion::print()
{
	printf("{ %.8lf, %.8lf, %.8lf, %.8lf }", vals[X], vals[Y], vals[Z], vals[W]);
}