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

 Copyright (c) 2009, Charles McGarvey
 All rights reserved.
 
 Redistribution   and   use  in  source  and  binary  forms,  with  or  without
 modification, are permitted provided that the following conditions are met:
 
   * Redistributions  of  source  code  must retain the above copyright notice,
     this list of conditions and the following disclaimer.
   * Redistributions  in binary form must reproduce the above copyright notice,
     this  list of conditions and the following disclaimer in the documentation
     and/or other materials provided with the distribution.
 
 THIS  SOFTWARE  IS  PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND  ANY  EXPRESS  OR  IMPLIED  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 DISCLAIMED.  IN  NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 FOR  ANY  DIRECT,  INDIRECT,  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 DAMAGES  (INCLUDING,  BUT  NOT  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 SERVICES;  LOSS  OF  USE,  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 CAUSED  AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 OR  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

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

#ifndef _QUATERNION_HH_
#define _QUATERNION_HH_

#include <cassert>

#include "math.hh"
#include "matrix.hh"
#include "vector.hh"


//
// Quaternion -> 3x3 Matrix
//
// | w^2 + x^2 - y^2 - z^2         2xy - 2wz               2xz + 2yw       |
// |       2xy + 2wz         w^2 - x^2 + y^2 - z^2         2yz - 2wx       |
// |       2xz - 2wy               2yz - 2wx         w^2 - x^2 - y^2 + z^2 |
//

namespace dc {


class quaternion
{
public:
	// Constructors.
	quaternion() {}
	quaternion(scalar X, scalar Y, scalar Z, scalar W) {
		vec.x = X; vec.y = Y; vec.z = Z; w = W;
	}
	quaternion(vector3 v, scalar W = 1.0) {
		vec = v; w = W;
	}
	quaternion(scalar q[4]) {
		vec.x = q[0]; vec.y = q[1]; vec.z = q[2]; w = q[3];
	}

	// Accessible by index.
	const scalar& operator [] (int i) const {
		assert(i >= 0 && i <= 3 && "Index into quaternion out of bounds.");
		return *((&vec.x) + i);
	}
	scalar& operator [] (int i) {
		assert(i >= 0 && i <= 3 && "Index into quaternion out of bounds.");
		//return vec[i];
		return *((&vec.x) + i);
	}

	// Basic maths.
	quaternion operator + (const quaternion& q) const {
		return quaternion(vec + q.vec, w + q.w);
	}
	quaternion operator - (const quaternion& q) const {
		return quaternion(vec - q.vec, w - q.w);
	}
	quaternion operator * (const quaternion& q) const {
		return quaternion(q.w * vec + w * q.vec + q.vec.cross(vec),
						  w * q.w - q.vec.dot(vec));
	}
	quaternion operator * (scalar s) const {
		return quaternion(vec * s, w * s);
	}
	quaternion operator / (scalar s) const {
		return quaternion(vec / s, w / s);
	}

	quaternion& operator += (const quaternion& q) {
		vec += q.vec; w += q.w;
		return *this;
	}
	quaternion& operator -= (const quaternion& q) {
		vec -= q.vec; w -= q.w;
		return *this;
	}
	quaternion& operator *= (const quaternion& q) {
		scalar W = w;
		w = W * q.w - q.vec.dot(vec);
		vec = q.w * vec + W * q.vec + q.vec.cross(vec);
		return *this;
	}
	quaternion& operator *= (scalar s) {
		vec *= s; w *= s;
		return *this;
	}
	quaternion& operator /= (scalar s) {
		vec /= s; w /= s;
		return *this;
	}

	// Unary operators.
	quaternion operator - () const {
		return quaternion(-vec, -w);
	}

	// Quaternion operations.
	quaternion conjugate() const {
		return quaternion(-vec, w);
	}
	void normalize(const vector3& v)
	{
		scalar len = length();
		if (len != 0.0) { *this /= len; }
	}
	
	scalar length2() const {
		return vec.x * vec.x + vec.y * vec.y + vec.z * vec.z + w * w;
	}
	scalar length() const {
		return std::sqrt(length2());
	}

	// Converting to other types.
	matrix3 matrix() const {
		scalar x2 = vec.x * vec.x;
		scalar y2 = vec.y * vec.y;
		scalar z2 = vec.z * vec.z;
		scalar w2 = w * w;
		scalar xy = vec.x * vec.y;
		scalar xz = vec.x * vec.z;
		scalar yzwx = 2 * (vec.y * vec.z - w * vec.x);
		scalar wy = w * vec.y;
		scalar wz = w * vec.z;
		return matrix3(x2 - y2 - z2 + w2, 2 * (xy - wz), 2 * (xz + wy),
					   2 * (xy + wz), w2 - x2 + y2 - z2, yzwx,
					   2 * (xz - wy), yzwx, w2 - x2 - y2 + z2);
	}
	vector3 axis(scalar& angle) { // Axis of rotation, w/ angle of rotation.
		vector3 axisVec = axis();
		angle = 2 * std::acos(w);
		return axisVec;
	}
	vector3 axis() {
		scalar sa = std::sqrt(1 - w * w);
		return vec / sa;
	}

	// Checking equality.
	bool operator == (const quaternion& q) const {
		return vec == q.vec && equals(w,q.w);
	}
	bool operator != (const quaternion& q) const {
		return !(*this == q);
	}

	// Data.
	vector3 vec;
	scalar w;
};

inline quaternion operator * (scalar s, const quaternion& q) {
	return q * s;
}
inline quaternion operator / (scalar s, const quaternion& q) {
	return quaternion(s / q.vec, s / q.w);
}

} // namespace dc


#endif // _QUATERNION_HH_