#pragma once

#include <moggle/math/matrix.hpp>
#include <boost/math/quaternion.hpp>
#include <limits>
#include <stdexcept>

// Will create a R3 rotation if the quaternion is normalized
// Typical use is: mat4 matrix = quaternion_to_R3_rotation(normalize(foo));
template<typename T>
moggle::matrix4<T> quaternion_to_R3_rotation(boost::math::quaternion<T> const & q){
	using ::std::numeric_limits;

	T a = q.R_component_1();
	T b = q.R_component_2();
	T c = q.R_component_3();
	T d = q.R_component_4();

	T aa = a*a;
	T ab = a*b;
	T ac = a*c;
	T ad = a*d;
	T bb = b*b;
	T bc = b*c;
	T bd = b*d;
	T cc = c*c;
	T cd = c*d;
	T dd = d*d;

	return {
		(aa+bb-cc-dd), 2*(-ad+bc),    2*(ac+bd),     0,
		2*(ad+bc),     (aa-bb+cc-dd), 2*(-ab+cd),    0,
		2*(-ac+bd),    2*(ab+cd),     (aa-bb-cc+dd), 0,
		0,             0,             0,             1
	};
}

namespace boost {
	namespace math {
		template <typename T>
		quaternion<T> normalize(quaternion<T> q){
			return q /= abs(q);
		}
	}
}