#ifndef __gxframework_quaternion_h__
	#define __gxframework_quaternion_h__

#include "shared.h"
#include "gxmath.h"

#include "matrix3.h"

namespace Gx
{
	class Quaternion
	{
	public:
		Quaternion() { }

		Quaternion(float ns, const Vector3& nv)
			: s(ns), v(nv) { }

		Quaternion(float x, float y, float z, float w)
			: v(x, y, z), s(w) { }

		Quaternion(const Quaternion& q)
			: v(q.v), s(q.s) { }

		Quaternion& operator=(const Quaternion& q) {
			v = q.v; s = q.s;
			return *this;
		}

		static const Quaternion Identity;

		// Creates from orientation matrix.
		// m Rotation matrix to extract quaternion from.
		static Quaternion createFromRotationMatrix(const Matrix3& m) {
			// Not implemented yet!
			return Quaternion::Identity;
		}

		// Creates from angle-axis representation.
		// Axis must be normalized! Angle is in radians!
		static Quaternion createFromAngleAxis(float angleRadians, const Vector3& unitAxis) {
			const float a = angleRadians * 0.5f;
			return Quaternion(cos(a), unitAxis * sin(a));
		}

		// Gets the angle (radians) between this quat and the identity quaternion.
		float getAngle() const {
			return 2.0f * acos(s);
		}

		Vector3 getUnitAxis() const {
			//JM version
			const float angle = getAngle();
			ASSERT(angle != 0); // dood, check it in runtime...
			return v / sin(angle * 0.5f);

			//PhysX version: check dot < epsilon, 
			const float ts = 1.0f / sqrt( v.dot(v) );
			// angle = atan2(dot * ts, s) * 2;
			return v * ts;
		}

		// Gets the angle (radians) between this quat and the argument
		float getAngle(const Quaternion& q) const { // CHECK IT
			return acos( this->dot(q) ) * 2.0f;
		}

		float magnitudeSquared() const {
			return v.dot(v) + s * s;
		}

		float magnitude() const {
			return std::sqrt( magnitudeSquared() );
		}

		float norm() const {
			return magnitude();
		}

		float dot(const Quaternion& q) const {
			return v.dot(q.v) + s * q.s;
		}

		Quaternion getNormalized() const {
			const float n = 1.0f / norm();
			return Quaternion(n * s, v * n);
		}

		Quaternion getConjugate() const {
			return Quaternion(s, -v); // sprzężony
			// dla unormowanego to jest kwaternion odwrotny
		}

		Quaternion getInverse() const {
			return ( getConjugate() /= magnitudeSquared() );
		}

		float normalize() {
			const float n = norm();
			if ( n )
			{
				const float inv = 1.0f / n;
				s *= inv;
				v *= inv;
			}
			return n;
		}

		void conjugate() {
			v = -v;
		}

		void inverse() {
			v = -v /= magnitudeSquared();
		}

		Quaternion operator-() const {
			return Quaternion(-s, -v);
		}

		Quaternion& operator+=(const Quaternion& q) {
			s += q.s; v += q.v;
			return *this;
		}

		Quaternion& operator-=(const Quaternion& q) {
			s += q.s; v += q.v;
			return *this;
		}

		Quaternion& operator*=(const Quaternion& q) {
			const float ts = s;
			const Vector3 tv = v;
			s = s * q.s - v.dot(q.v);
			v = v.cross(q.v) + ts * q.v + q.s * tv;
			return *this;
		}

		Quaternion& operator/=(const Quaternion& q) {
			return ( *this *= q.getInverse() );
		}

		Quaternion& operator*=(float scalar) {
			s *= scalar; v *= scalar;
			return *this;
		}

		Quaternion& operator/=(float scalar) {
			return ( *this *= (1.0f / scalar) );
		}

		const void rotate(Vector3& vec) const {
			Quaternion left;
			left *= Quaternion(0, vec);
			left *= getConjugate(); // zakładam że kwaternion znormalizowany
			vec = left.v;
		}

		float s;
		Vector3 v;
	};

	__declspec(selectany) const Quaternion Quaternion::Identity = Quaternion(1.0f, Vector3::Zero);

	static Quaternion operator+(const Quaternion& qa, const Quaternion& qb)
	{
		return ( Quaternion(qa) += qb );
		//return Quaternion(qa.s + qb.s, qa.v + qb.v);
	}

	static Quaternion operator-(const Quaternion& qa, const Quaternion& qb)
	{
		return ( Quaternion(qa) -= qb );
		//return Quaternion(qa.s - qb.s, qa.v - qb.v);
	}

	static Quaternion operator*(const Quaternion& qa, const Quaternion& qb)
	{
		return ( Quaternion(qa) *= qb );
	}

	static Quaternion operator/(const Quaternion& qa, const Quaternion& qb)
	{
		return ( Quaternion(qa) /= qb );
	}

	static Quaternion operator*(const Quaternion& q, float scalar)
	{
		return ( Quaternion(q) *= scalar );
	}

	static Quaternion operator*(float scalar, const Quaternion& q)
	{
		return ( Quaternion(q) *= scalar );
	}

	static Quaternion operator/(const Quaternion& q, float scalar)
	{
		return ( Quaternion(q) /= scalar );
	}

	static Quaternion operator/(float scalar, const Quaternion& q)
	{
		return ( Quaternion(q) /= scalar );
	}

	static std::ostream& operator<<(std::ostream& stream, const Quaternion& q)
	{
		stream << "{" << q.s << " " << q.v << "}";
		return stream;
	}
}

#endif /* ~__gxframework_quaternion_h__*/