#ifndef QUATERNION_H
#define QUATERNION_H

#include "Matrix3.h"

class Quaternion {

public:

	float w;
	float x, y, z;

	/** empty ctor */
	Quaternion() : w(1.0f), x(0.0f), y(0.0f), z(0.0f) {
		;
	}

	/** valued ctor */
	Quaternion(float w, float x, float y, float z) :  w(w), x(x), y(y), z(z) {
		;
	}

	/** construct from Euler angles in radians */
	static Quaternion fromEuler(float x, float y, float z) {
		Quaternion q;
		q.setEuler(x,y,z);
		return q;
	}

	static Quaternion fromAxisAngle(float angle, float x, float y, float z) {
		const float qx = x * std::sin(angle/2);
		const float qy = y * std::sin(angle/2);
		const float qz = z * std::sin(angle/2);
		const float qw = std::cos(angle/2);
		return Quaternion(qw,	qx,qy,qz);
	}


	void normalize() {
		*this *= 1.0 / std::sqrt( x*x + y*y + z*z + w*w );
	}

	inline float dotProduct(const Quaternion& q2) const {
		return (x * q2.x) + (y * q2.y) + (z * q2.z) + (w * q2.w);
	}


	/** linear interpolation between q1 and q2 */
	static Quaternion lerp( Quaternion q1, Quaternion q2, float time) {
		const float scale = 1.0f - time;
		return (q1*scale) + (q2*time);
	}

	/** spherical interpolation between q1 and q2 */
	static Quaternion slerp( Quaternion q1, Quaternion q2, float time, float threshold = 0.05)  {

		float angle = q1.dotProduct(q2);

		// make sure we use the short rotation
		if (angle < 0.0f) {
			q1 *= -1.0f;
			angle *= -1.0f;
		}

		if (angle <= (1-threshold)) {
			// spherical interpolation
			const float theta = acosf(angle);
			const float invsintheta = 1.0/(sinf(theta));
			const float scale = sinf(theta * (1.0f-time)) * invsintheta;
			const float invscale = sinf(theta * time) * invsintheta;
			return (q1*scale) + (q2*invscale);
		} else {
			// linear interpolation
			return Quaternion::lerp(q1,q2,time);
		}

	}


	/** multiply by scalar */
	Quaternion& operator *= (float val) {
		x *= val;
		y *= val;
		z *= val;
		w *= val;
		return *this;
	}

	/** multiply by scalar */
	Quaternion operator * (float val) const {
		return Quaternion(w*val, x*val, y*val, z*val);
	}

	/** multiply by other quaterion */
	Quaternion& operator *= (const Quaternion& other) {
		return (*this = other * (*this));
	}

	/** multiply by other quaterion */
	Quaternion operator * (const Quaternion& other) const {
		Quaternion tmp;
		tmp.w = (other.w * w) - (other.x * x) - (other.y * y) - (other.z * z);
		tmp.x = (other.w * x) + (other.x * w) + (other.y * z) - (other.z * y);
		tmp.y = (other.w * y) + (other.y * w) + (other.z * x) - (other.x * z);
		tmp.z = (other.w * z) + (other.z * w) + (other.x * y) - (other.y * x);
		return tmp;
	}

	/** add two quaternions */
	Quaternion operator + (const Quaternion& b) const {
		return Quaternion(w+b.w,		x+b.x, y+b.y, z+b.z);
	}


	/** set from euler angles (in radians) */
	void setEuler(float x, float y, float z) {

		double angle;

		angle = x * 0.5;
		const double sr = std::sin(angle);
		const double cr = std::cos(angle);

		angle = y * 0.5;
		const double sp = std::sin(angle);
		const double cp = std::cos(angle);

		angle = z * 0.5;
		const double sy = std::sin(angle);
		const double cy = std::cos(angle);

		const double cpcy = cp * cy;
		const double spcy = sp * cy;
		const double cpsy = cp * sy;
		const double spsy = sp * sy;

		this->x = (float)(sr * cpcy - cr * spsy);
		this->y = (float)(cr * spcy + sr * cpsy);
		this->z = (float)(cr * cpsy - sr * spcy);
		this->w = (float)(cr * cpcy + sr * spsy);
		normalize();

	}

	/** convert to euler angles */
	Vector3 toEuler() const {

		Vector3 res;

		const double sqw = w*w;
		const double sqx = x*x;
		const double sqy = y*y;
		const double sqz = z*z;
		const double test = 2.0 * (y*w - x*z);

		if (near(test, 1.0)) {
			// heading = rotation about z-axis
			res.z = (float) (-2.0*atan2(x, w));
			// bank = rotation about x-axis
			res.x = 0;
			// attitude = rotation about y-axis
			res.y = (float) (M_PI/2.0);
		} else if (near(test, -1.0)) {
			// heading = rotation about z-axis
			res.z = (float) (2.0*std::atan2(x, w));
			// bank = rotation about x-axis
			res.x = 0;
			// attitude = rotation about y-axis
			res.y = (float) (M_PI/-2.0);
		} else {
			// heading = rotation about z-axis
			res.z = (float) std::atan2(2.0 * (x*y + z*w),(sqx - sqy - sqz + sqw));
			// bank = rotation about x-axis
			res.x = (float) std::atan2(2.0 * (y*z + x*w),(-sqx - sqy + sqz + sqw));
			// attitude = rotation about y-axis
			res.y = (float) std::asin( clamp(test, -1.0, 1.0) );
		}

		return res;

	}

private:

	static inline bool near(double d1, double d2) {
		return std::abs(d1-d2) < 0.00001;
	}

	static inline double clamp(double val, double min, double max) {
		if (val < min) {return min;}
		if (val > max) {return max;}
		return val;
	}

};

#endif // QUATERNION_H