/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                   http://mrpt.sourceforge.net/                            |
   |                                                                           |
   |   Copyright (C) 2005-2008  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Perception and Robotics               |
   |      research group, University of Malaga (Spain).                        |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */

#ifndef CQuaternion_H
#define CQuaternion_H

#include <MRPT/UTILS/utils_defs.h>
#include <MRPT/UTILS/CMatrixTemplateNumeric.h>
#include <MRPT/UTILS/CVectorTemplate.h>
#include <math.h>

namespace UTILS
{
	namespace MATH
	{
		template <class T>
		class CQuaternion:public CVectorTemplate<T>//public std::vector<T>//
		{
			/** General functions for quaternion. This quaternion class represents a 3D rotation as a complex vector
			 * with 3 imaginary parts x, y, z and 1 real part r. Where q = r + ix + jy + kz.
			 */
		public:

	/* @{ Constructors
	 */

		/**	Default constructor. Construct an empty quaternion
		*
		*/
			CQuaternion()
			{
				(*this).resize(4);
			}

		/**	Construct a quaternion based on his parameters 'qr', 'qx', 'qy', 'qz'
		*
		*/
			CQuaternion(const T &r,const T &x,const T &y,const T &z)
			{
				(*this).resize(4);
				(*this)[0] = r;
				(*this)[1] = x;
				(*this)[2] = y;
				(*this)[3] = z;
			}

		/** Construct a quaternion as copy of an input quaternion
		*
		*/
			CQuaternion(const CQuaternion &qin)
			{
				(*this).resize(4);
				(*this)=qin;
			}

		/** Construct a quaternion from a 3D input vector
		*
		*/
			CQuaternion(const CVectorTemplate<T> &in)
			{

				if (in.size()!=3)
					THROW_EXCEPTION("Wrong Dimension in input vector for quaternion Constructor");

				(*this).resize(4);
				const T x = in[0];
				const T y = in[1];
				const T z = in[2];
				if ((x==0)&&(y==0)&&(z==0))
				{
					(*this)[0] = 1;
					(*this)[1] = 0;
					(*this)[2] = 0;
					(*this)[3] = 0;
				}
				else
				{
					const T angle = sqrt(x*x+y*y+z*z);
					const T s = (sin(angle/2))/angle;
					const T c = cos(angle/2);
					(*this)[0] = c;
					(*this)[1] = x * s;
					(*this)[2] = y * s;
					(*this)[3] = z * s;
				}
			}

		/* @}
		 */

		/**	Return r coordinate of the quaternion
		*
		*/
			T  r()const {return (*this)[0];}
		/**	Return x coordinate of the quaternion
		*
		*/
			T  x()const {return (*this)[1];}
		/**	Return y coordinate of the quaternion
		*
		*/
			T  y()const {return (*this)[2];}
		/**	Return z coordinate of the quaternion
		*
		*/
			T  z()const {return (*this)[3];}
		/**	Set r coordinate of the quaternion
		*
		*/
			void  r(const T &r) {(*this)[0]=r;}
		/**	Set x coordinate of the quaternion
		*
		*/
			void  x(const T &x) {(*this)[1]=x;}
		/**	Set y coordinate of the quaternion
		*
		*/
			void  y(const T &y) {(*this)[2]=y;}
		/**	Set z coordinate of the quaternion
		*
		*/
			void  z(const T &z) {(*this)[3]=z;}

		/**	Calculate the quaternion associated to a vector
		*
		*/
			void  q_conv(const CVectorTemplate<T> &in)
			{
				ASSERT_(in.size()==3);
				const T x = in[0];
                const T y = in[1];
				const T z = in[2];
				const T angle = sqrt(x*x+y*y+z*z);
				const T s = (sin(angle/2))/angle;
				const T c = cos(angle/2);
				(*this)[0] = c;
				(*this)[1] = x * s;
                (*this)[2] = y * s;
				(*this)[3] = z * s;
			}

		/**	Calculate the cross product of two quaternion.
		*	The result is an other quaternion
		*/
			void  q_prod(const CQuaternion &qin1, const CQuaternion &qin2)
			{
				T q1r = qin1.r();
				T q1x = qin1.x();
				T q1y = qin1.y();
				T q1z = qin1.z();

				T q2r = qin2.r();
				T q2x = qin2.x();
				T q2y = qin2.y();
				T q2z = qin2.z();

				(*this)[0] = q1r*q2r - q1x*q2x - q1y*q2y - q1z*q2z;
				(*this)[1] = q1r*q2x + q2r*q1x + q1y*q2z - q2y*q1z;
                (*this)[2] = q1r*q2y + q2r*q1y + q1z*q2x - q2z*q1x;
				(*this)[3] = q1r*q2z + q2r*q1z + q1x*q2y - q2x*q1y;

				//Normalised
				T qq = sqrt((*this)[0]*(*this)[0]+(*this)[1]*(*this)[1]+(*this)[2]*(*this)[2]+(*this)[3]*(*this)[3]);

				for (int i=0;i<4;i++)
                    (*this)[i]=(*this)[i] / qq;
			}

		/**	Normalised quaternion
		*
		*/
			void  q_normalise()
			{
				T qq = sqrt((*this)[0]*(*this)[0]+(*this)[1]*(*this)[1]+(*this)[2]*(*this)[2]+(*this)[3]*(*this)[3]);

				for (int i=0;i<4;i++)
                    (*this)[i]=(*this)[i] / qq;
			}

		/** Calculate the normalized Jacobian of a quaternion
		*
		*/
			void  q_normJac(CMatrixTemplateNumeric<T> &J)
			{
				T r_=(*this)[0];
				T x_=(*this)[1];
				T y_=(*this)[2];
				T z_=(*this)[3];
				T n = r_*r_ + x_*x_ + y_*y_ + z_*z_;
				n = 1 / (n*sqrt(n));

				J.setSize(4,4);
				J(0,0)=x_*x_+y_*y_+z_*z_;	J(0,1)=-r_*x_;				J(0,2)=-r_*y_;				J(0,3)=-r_*z_;
				J(1,0)=-x_*r_;				J(1,1)=r_*r_+y_*y_+z_*z_;	J(1,2)=-x_*y_;				J(1,3)=-x_*z_;
				J(2,0)=-y_*r_;				J(2,1)=-y_*x_;				J(2,2)=r_*r_+x_*x_+z_*z_;	J(2,3)=-y_*z_;
				J(3,0)=-z_*r_;				J(3,1)=-z_*x_;				J(3,2)=-z_*y_;				J(3,3)=r_*r_+x_*x_+y_*y_;
				J *=n;

			}

		/** Calculate de rotation matrix associated to a quaternion
		*
		*/
            void  q_rotation_matrix(CMatrixTemplateNumeric<T> &M)
			{
				M.setSize(3,3);
				T r_=(*this)[0];
				T x_=(*this)[1];
				T y_=(*this)[2];
				T z_=(*this)[3];
				M.setSize(3,3);
				M(0,0)=r_*r_+x_*x_-y_*y_-z_*z_;		M(0,1)=2*(x_*y_ -r_*z_);			M(0,2)=2*(z_*x_+r_*y_);
				M(1,0)=2*(x_*y_+r_*z_);				M(1,1)=r_*r_-x_*x_+y_*y_-z_*z_;		M(1,2)=2*(y_*z_-r_*x_);
				M(2,0)=2*(z_*x_-r_*y_);				M(2,1)=2*(y_*z_+r_*x_);				M(2,2)=r_*r_-x_*x_-y_*y_+z_*z_;

			}

		/**	Change a quaternion by his conjugate
		*
		*/
			CQuaternion  q_conj()
			{
				CQuaternion q_aux;
				q_aux[0]=(*this)[0];
				q_aux[1]=-(*this)[1];
				q_aux[2]=-(*this)[2];
				q_aux[3]=-(*this)[3];
				return q_aux;
			}

/*********************************************************************************************************************/
		/**	Return the roll, pitch and yaw angle associated to quaternion
		*
		*/
			void  rpy(T &r, T &p, T &y)
			{
				T r_=(*this)[0];
				T x_=(*this)[1];
				T y_=(*this)[2];
				T z_=(*this)[3];
				r=atan2(2*(y_*z_+r_*x_),r_*r_-x_*x_-y_*y_+z_*z_);
				p=asin(-2*(x_*z_-r_*y_));
				y=atan2(2*(x_*y_+r_*z_),r_*r_+x_*x_-y_*y_-z_*z_);
			}

/*********************************************************************************************************************/


			CQuaternion  operator * (const T &factor)
			{
				return (*this)*factor;
			}


		};	// end class

	}	// end namespace MATH

} // end namespace UTILS

#endif //__CQuaternion.h
