g_asteroids_vars = {}

g_height = 8  -- offset from origin to centre for crate

function asteroids_init(e)
	Hide(e)
	HideTerrain()
    HideWater()
end

function asteroids_HandleControls(vars)

	local rotx = -(g_MouseY - vars.my) / 200
	local rotz = -(g_MouseX - vars.mx) / 200

	if g_KeyPressE == 1 then
		vars.velocity = vars.velocity + 0.01
	end
	if g_KeyPressD == 1 then
		vars.velocity = vars.velocity - 0.01
	end
	
	-- update turning quaternion 
	vars.turning_qat = EulerToQuat(math.rad(rotx),
								   math.rad(vars.yaw),
								   math.rad(rotz));
end
	
function asteroids_main(e)

	vars = g_asteroids_vars[e]
	
	if vars == nil then
	
		local Ent = g_Entity[e]
	
		if Ent ~= nil then
		
			g_asteroids_vars[e] = 
				-- initialise quaternions
			   {entity_qat  = EulerToQuat(0, 0, 0),
				turning_qat = EulerToQuat(0, 0, 0),
			
				-- initialise position variable
				x = Ent.x, y = Ent.y, z = Ent.z, yaw = 0};
			
			ActivateMouse()
			
			g_asteroids_vars[e].mx = 50
			g_asteroids_vars[e].my = 50
			
			g_asteroids_vars[e].velocity = 0
		end
		return
	else	

		asteroids_HandleControls(vars)
		
		-- this bit rotates the entity
		vars.entity_qat = QuatMul(vars.entity_qat, vars.turning_qat)
												 
		-- get the new Euler angles
		local xr, yr, zr = QuatToEuler(vars.entity_qat)
		
		SetFreezePosition(vars.x, vars.y, vars.z)
		SetFreezeAngle(math.deg(xr), math.deg(yr), math.deg(zr))
		TransportToFreezePosition()
		
		local VX,VY,VZ = Rotate3D(0,0,vars.velocity,xr,yr,zr) 
		
		asteroid_update({x = VX, y = VY, z = VZ})
	end
end

function asteroids_exit (e)
	g_asteroids_vars[e] = nil
end
	
function Rotate3D (x, y, z, xrot, yrot, zrot) 

	function RotatePoint2D (x, y, Ang)  -- Ang in radians
		local Sa, Ca = math.sin(Ang), math.cos(Ang)
		return x*Ca - y*Sa, x*Sa + y*Ca
	end

 
	local NX, NY, NZ = x, y, z

	-- X
	NZ, NY = RotatePoint2D (NZ, NY, -xrot)
	
	-- Y
	NX, NZ = RotatePoint2D (NX, NZ, -yrot)
	
	-- Z
	NY, NX = RotatePoint2D (NY, NX, -zrot)

	return NX, NY, NZ
end

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 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