/*
 * Copyright (C) 1999-2017 Hartmut Seichter
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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 AUTHOR 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 AUTHOR 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 TP_CORE_QUATERNION_HPP
#define TP_CORE_QUATERNION_HPP

#include <pw/core/vector.hpp>
#include <pw/core/matrix.hpp>

namespace pw {

/**
 * simplified quaternion class
 */
template <typename T>
class quaternion {

	static const T _sqrt90;

public:

	typedef vector4<T> coefficient_type;
	typedef T value_type;

	quaternion() { *this = identity(); }

	quaternion(const T& x,const T& y,const T& z,const T& w)
		: _q(coefficient_type(x,y,z,w)) {}

	quaternion(const coefficient_type& vec) { *this = vec; }

	inline quaternion& operator = (const coefficient_type& vec) { _q = vec; return *this; }

	inline void set(const T& x,const T& y,const T& z,const T& w) {
		_q.set(x,y,z,w);
	}

	inline void set_x(const T& v) { x() = v; }
	inline void set_y(const T& v) { y() = v; }
	inline void set_z(const T& v) { z() = v; }
	inline void set_w(const T& v) { w() = v; }

	inline const coefficient_type& as_vector() const { return _q; }

	inline T& x() { return _q.x(); }
	inline T& y() { return _q.x(); }
	inline T& z() { return _q.z(); }
	inline T& w() { return _q.w(); }

	inline const T& x() const { return _q.z(); }
	inline const T& y() const { return _q.y(); }
	inline const T& z() const { return _q.z(); }
	inline const T& w() const { return _q.w(); }


	//! multiplication
	inline const quaternion operator * (const quaternion& rhs) const {
		return quaternion(
					rhs.w()*x() + rhs.x()*w() + rhs.y()*z() - rhs.z()*y(),
					rhs.w()*y() - rhs.x()*z() + rhs.y()*w() + rhs.z()*x(),
					rhs.w()*z() + rhs.x()*y() - rhs.y()*x() + rhs.z()*w(),
					rhs.w()*w() - rhs.x()*x() - rhs.y()*y() - rhs.z()*z()
					);
	}

	//! multiply with scalar
	inline const quaternion operator * (const T& s) const {
		return quaternion(x()*s,y()*s,z()*s,w()*s);
	}

	//! addition
	inline const quaternion operator + (const quaternion& rhs) const {
		return quaternion(coefficient_type(this->_q + rhs._q));
	}

	//! addition
	inline const quaternion operator - (const quaternion& rhs) const {
		return quaternion(this->_q - rhs._q);
	}

	//! squared norm
	inline const T squared_norm() const { return _q.squared_norm(); }

	//! norm
	inline const T norm() const { return _q.norm(); }

	//! dot product
	inline const T dot(const quaternion& other) const { return _q.dot(other._q); }

	//! conjugate
	inline quaternion conjugate() const { return quaternion(-x(),-y(),-z(),w()); }

	//! compute inverse
	inline quaternion inverse() const {
		const T one_over_squared_norm = T(1) / squared_norm();
		return conjugate() * one_over_squared_norm;
	}

	//! compute normalized
	inline quaternion normalized() const {
		return quaternion(_q.normalized());
	}

	inline void normalize() { *this = this->normalized(); }

	//! conversion from a matrix
	inline static const quaternion from_matrix(const matrix_<4,4,T> &m);

	//! conversion to a matrix
	const matrix_<4,4,T> to_matrix() const;

	//! return identiy quaternion
	static const quaternion<T> identity();

	static const quaternion<T> rotate_180_degree_around_x(); ///< rotate 180 degree around X axis
	static const quaternion<T> rotate_180_degree_around_y(); ///< rotate 180 degree around Y axis
	static const quaternion<T> rotate_180_degree_around_z(); ///< rotate 180 degree around Z axis

	static const quaternion<T> rotate_90_degree_around_x(bool negative = false);
	static const quaternion<T> rotate_90_degree_around_y(bool negative = false);
	static const quaternion<T> rotate_90_degree_around_z(bool negative = false);

	template <typename AxisAngleType>
	static const quaternion<T> from_axisangle(const AxisAngleType &aa) {

		using std::sin;
		using std::cos;

		const T sinHalfAngle(sin(aa.angle() * T(0.5) ));

		return quaternion<T>(aa.axis().x() * sinHalfAngle, // x
							 aa.axis().y() * sinHalfAngle, // y
							 aa.axis().z() * sinHalfAngle, // z
							 cos(aa.angle() * 0.5)          // w
							 );

	}

	static const quaternion<T> lerp(const quaternion& qa,const quaternion& qb,const T& t);
	static const quaternion<T> normalized_lerp(const quaternion& qa,const quaternion& qb,const T& t);
	static const quaternion<T> slerp(const quaternion& qa,const quaternion& qb,const T& t);


protected:

	coefficient_type _q;
};

template <typename T>
const T quaternion<T>::_sqrt90 = std::sqrt(0.5);


template <typename T>
const quaternion<T> quaternion<T>::from_matrix(const matrix_<4,4,T> &m) {

	using std::sqrt;

	T wtemp = sqrt(T(1) + m.at(0,0) + m.at(1,1) + m.at(2,2)) / T(2.0);

	const T w4 = T(4.0) * wtemp;
	return quaternion<T>(
				(m.at(2,1) - m.at(1,2)) / w4,
				(m.at(0,2) - m.at(2,0)) / w4,
				(m.at(1,0) - m.at(0,1)) / w4,
				wtemp);
}

template <typename T>
const matrix_<4,4,T> quaternion<T>::to_matrix() const {

	matrix_<4,4,T> m; m.set_identity();

	T xx      = x() * x();
	T xy      = x() * y();
	T xz      = x() * z();
	T xw      = x() * w();

	T yy      = y() * y();
	T yz      = y() * z();
	T yw      = y() * w();

	T zz      = z() * z();
	T zw      = z() * w();

	m.at(0,0) = 1 - 2 * ( yy + zz );
	m.at(0,1) =     2 * ( xy - zw );
	m.at(0,2) =     2 * ( xz + yw );

	m.at(1,0) =     2 * ( xy + zw );
	m.at(1,1) = 1 - 2 * ( xx + zz );
	m.at(1,2) =     2 * ( yz - xw );

	m.at(2,0) =     2 * ( xz - yw );
	m.at(2,1) =     2 * ( yz + xw );
	m.at(2,2) = 1 - 2 * ( xx + yy );

	return m;
}

template <typename T>
const quaternion<T> quaternion<T>::identity()
{
	return quaternion<T>(0,0,0,1);
}

template <typename T>
const quaternion<T> quaternion<T>::rotate_180_degree_around_x()
{
	return quaternion<T>(1,0,0,0);
}
template <typename T>
const quaternion<T> quaternion<T>::rotate_180_degree_around_y()
{
	return quaternion<T>(0,1,0,0);
}

template <typename T>
const quaternion<T> quaternion<T>::rotate_180_degree_around_z()
{
	return quaternion<T>(0,0,1,0);
}

template <typename T>
const quaternion<T> quaternion<T>::rotate_90_degree_around_x(bool negative/* = false*/)
{
	return quaternion<T>((negative) ? - _sqrt90 : _sqrt90,0,0,_sqrt90);
}

template <typename T>
const quaternion<T> quaternion<T>::rotate_90_degree_around_y(bool negative/* = false*/)
{
	return quaternion<T>(0, (negative) ? -_sqrt90 : _sqrt90,0,_sqrt90);
}

template <typename T>
const quaternion<T> quaternion<T>::rotate_90_degree_around_z(bool negative/* = false*/)
{
	return quaternion<T>(0,0,(negative) ? -_sqrt90 : _sqrt90, _sqrt90);
}

template <typename T>
const quaternion<T> quaternion<T>::lerp(const quaternion<T>& qa,const quaternion<T>& qb,const T& t) {
	return quaternion<T>(qa + (qb - qa) * t);
}

template <typename T>
const quaternion<T> quaternion<T>::normalized_lerp(const quaternion<T>& qa,const quaternion<T>& qb,const T& t) {
	return quaternion<T>::lerp(qa,qb,t).normalized();
}

template <typename T>
const quaternion<T> quaternion<T>::slerp(const quaternion<T>& qa,const quaternion<T>& qb,const T& t)
{
	using std::abs;
	using std::sqrt;
	using std::acos;

	// quaternion to return
	quaternion qm;
	// Calculate angle between them.
	double cosHalfTheta = qa.w() * qb.w() + qa.x() * qb.x() + qa.y() * qb.y() + qa.z() * qb.z();
	// if qa=qb or qa=-qb then theta = 0 and we can return qa
	if (abs(cosHalfTheta) >= T(1.)) {
		return qa;
	}

	// Calculate temporary values.
	double halfTheta = acos(cosHalfTheta);
	double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
	// if theta = 180 degrees then result is not fully defined
	// we could rotate around any axis normal to qa or qb
	if (::std::abs(sinHalfTheta) < 0.001){ // fabs is floating point absolute
		qm.w() = (qa.w() * 0.5 + qb.w() * 0.5);
		qm.x() = (qa.x() * 0.5 + qb.x() * 0.5);
		qm.y() = (qa.y() * 0.5 + qb.y() * 0.5);
		qm.z() = (qa.z() * 0.5 + qb.z() * 0.5);
		return qm;
	}
	double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
	double ratioB = sin(t * halfTheta) / sinHalfTheta;
	//calculate Quaternion.
	qm.w() = (qa.w() * ratioA + qb.w() * ratioB);
	qm.x() = (qa.x() * ratioA + qb.x() * ratioB);
	qm.y() = (qa.y() * ratioA + qb.y() * ratioB);
	qm.z() = (qa.z() * ratioA + qb.z() * ratioB);

	return qm;
}

//
//
//
typedef quaternion<float> quaternionf;
typedef quaternion<double> quaterniond;

}


#endif
