/*
 * Copyright (c) 1999-2019 Hartmut Seichter
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */
#ifndef PW_CORE_QUATERNION_HPP
#define PW_CORE_QUATERNION_HPP

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

namespace pw {

/**
 * simplified quaternion class
 */
template <typename T>
struct quaternion_ : vector4_<T> {

    typedef vector4_<T> base_type;

	using typename base_type::value_type;
    using base_type::base_type;
    using base_type::x;
    using base_type::y;
    using base_type::z;
    using base_type::w;
    using base_type::lerp;
	using base_type::xyz;
//    using base_type::operator*;
//    using base_type::operator/;


	quaternion_(const base_type& other) : base_type(other) {}

	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()
					);
	}

	inline auto operator / (const T& rhs) const {
		return quaternion_( { x() / rhs, y() / rhs, z() / rhs, w() / rhs, } );
	}

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

	//! compute inverse
	inline auto inverse() const {
		return conjugate() / this->norm();
	}

	inline static auto identity() {
		return quaternion_({0,0,0,1});
	}

	const matrix4x4_<T> to_matrix() const {

		matrix4x4_<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(0,0) = 1 - 2 * ( yy + zz );
		m(0,1) =     2 * ( xy - zw );
		m(0,2) =     2 * ( xz + yw );

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

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

		return m;
	}

	static auto from_matrix(const matrix_<4,4,T> &m) {
		using std::sqrt;
		auto wtemp = sqrt(T(1) + m(0,0) + m(1,1) + m(2,2)) / T(2);
		auto w4 = T(4.0) * wtemp;
		return quaternion_<T>(
					(m(2,1) - m(1,2)) / w4,
					(m(0,2) - m(2,0)) / w4,
					(m(1,0) - m(0,1)) / w4,
					wtemp);
	}

	static auto normalized_lerp(const quaternion_ &a,const quaternion_ &b,const T &t) {
		return quaternion_(lerp(a,b,t).normalized());
	}

	static auto slerp(const quaternion_& qa,const quaternion_& qb,const T& t)
	{
		using std::abs;
		using std::sqrt;
		using std::acos;

		// quaternion to return
		quaternion_ qm;
		// Calculate angle between them.
		T 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.
		const T halfTheta = acos(cosHalfTheta);
		const T 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 (abs(sinHalfTheta) < 0.001){ // fabs is floating point absolute
			qm.w() = (qa.w() * T(0.5) + qb.w() * T(0.5));
			qm.x() = (qa.x() * T(0.5) + qb.x() * T(0.5));
			qm.y() = (qa.y() * T(0.5) + qb.y() * T(0.5));
			qm.z() = (qa.z() * T(0.5) + qb.z() * T(0.5));
			return qm;
		}
		const T ratioA = sin((value_type(1) - t) * halfTheta) / sinHalfTheta;
		const T 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;
	}


	static auto from_axisangle(const axisangle_<T> &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 * T(0.5))          // w
							   }
							   );

	}

};
//
//
//
using quaternion = quaternion_<real_t>;
using quaternionf = quaternion_<float>;
using quaterniond = quaternion_<double>;



}




#if 0
/**
 * 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 dot(_q,other._q); }




	//! 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_<T,4,4> &m);

	//! conversion to a matrix
	const matrix_<T,4,4> 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>::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>::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;
}



#endif


#endif
