g_tutorial_vars = {}

g_tutorial_ts = {pitch = 0, yaw = 0, bank = 0}

g_turn_increment = 0.01

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

function tutorial_init(e)
	FreezePlayer()
end

function tutorial_HandleKeys(vars)
	if g_KeyPressA == 1 then
		g_tutorial_ts.bank = g_tutorial_ts.bank + g_turn_increment
	end
	if g_KeyPressD == 1 then
		g_tutorial_ts.bank = g_tutorial_ts.bank - g_turn_increment
	end
	if g_KeyPressW == 1 then
		g_tutorial_ts.pitch = g_tutorial_ts.pitch + g_turn_increment
	end
	if g_KeyPressS == 1 then
		g_tutorial_ts.pitch = g_tutorial_ts.pitch - g_turn_increment
	end
	if g_KeyPressR == 1 then
		g_tutorial_ts.yaw = g_tutorial_ts.yaw + g_turn_increment
	end
	if g_KeyPressE == 1 then
		g_tutorial_ts.yaw = g_tutorial_ts.yaw - g_turn_increment
	end
	
	-- update turning quaternion 
	vars.turning_qat = EulerToQuat(math.rad(g_tutorial_ts.pitch),
								   math.rad(g_tutorial_ts.yaw),
								   math.rad(g_tutorial_ts.bank));
end
	
function tutorial_main(e)

	vars = g_tutorial_vars[e]
	
	if vars == nil then
	
		local Ent = g_Entity[e]
	
		if Ent ~= nil then
		
			g_tutorial_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 + 60, z = Ent.z}
		end
		return
	else	

		tutorial_HandleKeys(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)
		
		-- This bit works out the new offsets to centre the entity
		local XO, YO, ZO = Rotate3D(0, g_height, 0, xr, yr, zr)
		
		-- reposition entity
		SendMessageI("collisionoff",e)		   	
		ResetPosition(e, vars.x - XO, vars.y - YO, vars.z - ZO)
		ResetRotation(e, math.deg(xr), math.deg(yr), math.deg(zr))
		SendMessageI("collisionon",e)
		
	end
end

function tutorial_exit (e)
	g_tutorial_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