-- guat = {w, x, y, z}
-- roll  (bank)     : rotation around Z axiz  (radians!!)
-- pitch (attitude) : rotation around X axis  (radians!!)
-- yaw   (heading)  : rotation around Y axis  (radians!!)
local quat_lib = true

function EulerToQuat(pitch, yaw, roll)    -- radians

	local cr = math.cos(roll/2)
	local cp = math.cos(pitch/2)
	local cy = math.cos(yaw/2)
	local sr = math.sin(roll/2)
	local sp = math.sin(pitch/2)
	local sy = math.sin(yaw/2)
		
	local cycp = cy * cp
	local sysp = sy * sp
	local cysp = cy * sp
	local sycp = sy * cp
	
	return {w = (cr * cycp) + (sr * sysp),
			x = (cr * cysp) - (sr * sycp),
			y = (cr * sycp) + (sr * cysp),
			z = (sr * cycp) - (cr * sysp)}

end

function QuatToEuler(q)
 
	local sqw = q.w*q.w
	local sqx = q.x*q.x
	local sqy = q.y*q.y
	local sqz = q.z*q.z
	
	local h = -2.0 * (q.x*q.z - q.y*q.w)
	
	if math.abs(h) < 0.99999 then
	
		return  math.atan2(2.0 * (q.y*q.z + q.x*q.w),(-sqx - sqy + sqz + sqw)),  -- x ang
				math.asin(-2.0 * (q.x*q.z - q.y*q.w)),                           -- y ang
				math.atan2(2.0 * (q.x*q.y + q.z*q.w),(sqx - sqy - sqz + sqw))    -- z ang
				
	else
		return  math.atan2(2.0 * (q.y*q.z + q.x*q.w),(-sqx - sqy + sqz + sqw)),  -- x ang
				(math.pi / 2) * h,                                               -- y ang
				math.atan2(2.0 * (q.x*q.y + q.z*q.w),(sqx - sqy - sqz + sqw))    -- z ang				
	end
end

function QuatConjugate(q)
	return {w = q.w, x = -q.x, y = -q.y, z = -q.z}
end
 
function QuatNormalise(q)
	local n = math.sqrt(x*x + y*y + z*z + w*w)

	if n == 0 then
		return {w = 1, x = 0, y = 0, z = 0}
	else
		return {w = q.w / n, x = q.x / n, y = q.y / n, z = q.z / n}
	end
end

function QuatAdd(q1, q2)
	return {w = q1.w + q2.w, x = q1.x + q2.x, y = q1.y + q2.y, z = q1.z + q2.z}
end
		
 function QuatMul(q1, q2)

	local A = (q1.w + q1.x)*(q2.w + q2.x)
	local B = (q1.z - q1.y)*(q2.y - q2.z)
	local C = (q1.w - q1.x)*(q2.y + q2.z)
	local D = (q1.y + q1.z)*(q2.w - q2.x)
	local E = (q1.x + q1.z)*(q2.x + q2.y)
	local F = (q1.x - q1.z)*(q2.x - q2.y)
	local G = (q1.w + q1.y)*(q2.w - q2.z)
	local H = (q1.w - q1.y)*(q2.w + q2.z)

	return {w = B + (-E - F + G + H)/2,
		    x = A - ( E + F + G + H)/2,
			y = C + ( E - F + G - H)/2,
			z = D + ( E - F - G + H)/2}
end

return quat_lib;