#ifndef B3_QUAT_H
#define B3_QUAT_H

#include "Bullet3Common/shared/b3PlatformDefinitions.h"
#include "Bullet3Common/shared/b3Float4.h"

#ifdef __cplusplus
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Transform.h"

#define b3Quat b3Quaternion
#define b3QuatConstArg const b3Quaternion&
inline b3Quat b3QuatInverse(b3QuatConstArg orn)
{
	return orn.inverse();
}

inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
{
	b3Transform tr;
	tr.setOrigin(translation);
	tr.setRotation(orientation);
	return tr(point);
}

#else
typedef float4 b3Quat;
#define b3QuatConstArg const b3Quat

inline float4 b3FastNormalize4(float4 v)
{
	v = (float4)(v.xyz, 0.f);
	return fast_normalize(v);
}

inline b3Quat b3QuatMul(b3Quat a, b3Quat b);
inline b3Quat b3QuatNormalized(b3QuatConstArg in);
inline b3Quat b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec);
inline b3Quat b3QuatInvert(b3QuatConstArg q);
inline b3Quat b3QuatInverse(b3QuatConstArg q);

inline b3Quat b3QuatMul(b3QuatConstArg a, b3QuatConstArg b)
{
	b3Quat ans;
	ans = b3Cross3(a, b);
	ans += a.w * b + b.w * a;
	//	ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
	ans.w = a.w * b.w - b3Dot3F4(a, b);
	return ans;
}

inline b3Quat b3QuatNormalized(b3QuatConstArg in)
{
	b3Quat q;
	q = in;
	//return b3FastNormalize4(in);
	float len = native_sqrt(dot(q, q));
	if (len > 0.f)
	{
		q *= 1.f / len;
	}
	else
	{
		q.x = q.y = q.z = 0.f;
		q.w = 1.f;
	}
	return q;
}
inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)
{
	b3Quat qInv = b3QuatInvert(q);
	float4 vcpy = vec;
	vcpy.w = 0.f;
	float4 out = b3QuatMul(b3QuatMul(q, vcpy), qInv);
	return out;
}

inline b3Quat b3QuatInverse(b3QuatConstArg q)
{
	return (b3Quat)(-q.xyz, q.w);
}

inline b3Quat b3QuatInvert(b3QuatConstArg q)
{
	return (b3Quat)(-q.xyz, q.w);
}

inline float4 b3QuatInvRotate(b3QuatConstArg q, b3QuatConstArg vec)
{
	return b3QuatRotate(b3QuatInvert(q), vec);
}

inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
{
	return b3QuatRotate(orientation, point) + (translation);
}

#endif

#endif  //B3_QUAT_H