
--
-- Should never really happen.
--
motionsensor = motionsensor or {}



--
-- These bones are used to draw the debug 
-- kinect skeleton. You just need to loop through
-- and draw a line between each of these bones.
--
motionsensor.DebugBones = 
{
	-- Torso
	{ SENSORBONE.HEAD,				SENSORBONE.SHOULDER },
	{ SENSORBONE.SHOULDER,			SENSORBONE.SHOULDER_LEFT },
	{ SENSORBONE.SHOULDER,			SENSORBONE.SHOULDER_RIGHT },
	{ SENSORBONE.SHOULDER,			SENSORBONE.SPINE },
	{ SENSORBONE.SHOULDER,			SENSORBONE.HIP },
	{ SENSORBONE.HIP,				SENSORBONE.HIP_LEFT },
	{ SENSORBONE.HIP,				SENSORBONE.HIP_RIGHT },

	-- Left Arm
	{ SENSORBONE.SHOULDER_LEFT,		SENSORBONE.ELBOW_LEFT },
	{ SENSORBONE.ELBOW_LEFT,		SENSORBONE.WRIST_LEFT },
	{ SENSORBONE.WRIST_LEFT,		SENSORBONE.HAND_LEFT },

	-- Right Arm
	{ SENSORBONE.SHOULDER_RIGHT,	SENSORBONE.ELBOW_RIGHT },
	{ SENSORBONE.ELBOW_RIGHT,		SENSORBONE.WRIST_RIGHT },
	{ SENSORBONE.WRIST_RIGHT,		SENSORBONE.HAND_RIGHT },

	-- left leg
	{ SENSORBONE.HIP_LEFT,			SENSORBONE.KNEE_LEFT },
	{ SENSORBONE.KNEE_LEFT,			SENSORBONE.ANKLE_LEFT },
	{ SENSORBONE.ANKLE_LEFT,		SENSORBONE.FOOT_LEFT },

	-- right leg
	{ SENSORBONE.HIP_RIGHT,			SENSORBONE.KNEE_RIGHT },
	{ SENSORBONE.KNEE_RIGHT,		SENSORBONE.ANKLE_RIGHT },
	{ SENSORBONE.ANKLE_RIGHT,		SENSORBONE.FOOT_RIGHT },
}

motionsensor.ChooseBuilderFromEntity = function( ent )
	
	local builders = list.Get( "SkeletonConvertor" )

	for k, v in pairs( builders ) do

		if ( v:IsApplicable( ent ) ) then return k end

	end

	return 'ValveBiped'

end

motionsensor.ProcessAngle = function( translator, sensor, pos, ang, special_vectors, boneid, v )

	local a = nil
	local b = nil
	local up = special_vectors['up']

	--
	-- Using a vector from another angle.
	-- If the angle isn't processed yet return 
	-- we will be added to the list to process again.
	--
	if ( v.up_up ) then	
		if ( !ang[ v.up_up ] ) then return end			
		up = ang[ v.up_up ]:Up()			
	end

	if ( v.up_dn ) then	
		if ( !ang[ v.up_dn ] ) then return end			
		up = ang[ v.up_dn ]:Up() * -1			
	end

	if ( v.up_fwd ) then		
		if ( !ang[ v.up_fwd ] ) then return end		
		up = ang[ v.up_fwd ]:Forward()		
	end

	if ( v.up_lft ) then			
		if ( !ang[ v.up_lft ] ) then return end	
		up = ang[ v.up_lft ]:Right() * -1	
	end

	if ( v.up_rgt ) then	
		if ( !ang[ v.up_rgt ] ) then return end			
		up = ang[ v.up_rgt ]:Right()		
	end

	--
	-- From -> To vectors
	--
	if ( v.from_sensor ) then	a = sensor[ v.from_sensor ]		end
	if ( v.to_sensor ) then		b = sensor[ v.to_sensor ]		end
	if ( v.from ) then			a = pos[ v.from ]				end
	if ( v.to ) then			b = pos[ v.to ]					end

	--
	-- We can offer special vectors to define 'up'
	--
	if ( isstring( v.up ) ) then	
		up = special_vectors[ v.up ]		
	end

	if ( a == nil || b == nil || up == nil ) then return end

	ang[ boneid ]	= (a-b):GetNormal():AngleEx( up:GetNormal() );

	if ( v.adjust ) then
		ang[ boneid ] = ang[ boneid ] + v.adjust
	end

	return true

end

--
-- Processes the AnglesTable from a skeleton constructor
--
motionsensor.ProcessAnglesTable = function( translator, sensor, pos, rotation )

	if ( !translator.AnglesTable ) then return {} end

	local ang = {}

	local special_vectors = {}
	special_vectors['right']	= rotation:Right()
	special_vectors['left']		= special_vectors['right'] * -1
	special_vectors['up']		= rotation:Up()
	special_vectors['down']		= special_vectors['up'] * -1
	special_vectors['forward']	= special_vectors['down']:Cross( special_vectors['right'] )
	special_vectors['backward']	= special_vectors['forward'] * -1

	special_vectors['hips_left']	= (sensor[SENSORBONE.HIP_RIGHT]	- sensor[SENSORBONE.HIP_LEFT]):GetNormal()
	special_vectors['hips_up']		= (sensor[SENSORBONE.HIP]		- sensor[SENSORBONE.SHOULDER]):GetNormal()
	special_vectors['hips_back']	= special_vectors['hips_up']:Cross( special_vectors['hips_left'] )
	special_vectors['hips_fwd']		= special_vectors['hips_back'] * -1

	special_vectors['chest_lft']	= (sensor[SENSORBONE.SHOULDER_RIGHT]	- sensor[SENSORBONE.SHOULDER_LEFT]):GetNormal()
	special_vectors['chest_rgt']	= special_vectors['chest_lft'] * -1
	special_vectors['chest_up']		= (sensor[SENSORBONE.SPINE]		- sensor[SENSORBONE.SHOULDER]):GetNormal()
	special_vectors['chest_dn']		= special_vectors['chest_up'] * -1
	special_vectors['chest_bck']	= special_vectors['chest_up']:Cross( special_vectors['chest_lft'] )
	special_vectors['chest_fwd']	= special_vectors['chest_bck'] * -1

	special_vectors['head_up']		= (sensor[SENSORBONE.SHOULDER]		- sensor[SENSORBONE.HEAD]):GetNormal()
	special_vectors['head_back']	= special_vectors['head_up']:Cross( special_vectors['chest_lft'] )

	local reprocess = {}

	for k, v in pairs( translator.AnglesTable ) do

		table.insert( reprocess, k )

	end

	for iPasses = 1, 5 do

		local cur_process = reprocess;
		reprocess = {}

		for k, v in pairs( cur_process ) do

			if ( !motionsensor.ProcessAngle( translator, sensor, pos, ang, special_vectors, v, translator.AnglesTable[v] ) ) then
				table.insert( reprocess, v )
			end

		end

		if ( table.Count( reprocess ) == 0 ) then
			--DebugInfo( 0, iPasses .. " Passes" )
			return ang
		end

	end

	--
	-- If we got here then we're doing something wrong.
	-- It should have out'ed before completing 5 passes.
	--
	DebugInfo( 0, "motionsensor.ProcessAnglesTable: 4+ passes!" )
	return ang

end

--
-- Processes the PositionTable from a skeleton constructor
--
motionsensor.ProcessPositionTable = function( translator, sensor )

	if ( !translator.PositionTable ) then return {} end

	local pos = {}

	for k, v in pairs( translator.PositionTable ) do

		-- A number means get value straight from the sensor
		if ( isnumber( v ) ) then pos[ k ] = sensor[ v ] end

		if ( istable( v ) ) then

			if ( v.type == "lerp" ) then pos[ k ] = LerpVector( v.value, sensor[ v.from ], sensor[ v.to ] ) end

		end

	end

	return pos

end

--
-- Called to build the skeleton
--
motionsensor.BuildSkeleton = function( translator, player, rotation )

	--
	-- The kinect animations are recorded on the skunt, so rotate towards the player
	--
	rotation:RotateAroundAxis( rotation:Up(), -90 )

	--
	-- Pre-get and rotate all the player positions
	--
	local sensor = {}
	for i=0, 19 do
		sensor[ i ] = player:MotionSensorPos( i )
		sensor[ i ]:Rotate( rotation )
	end

	if ( translator.PrePosition ) then
		translator:PrePosition( sensor )
	end

	--
	-- Fill out the position table..
	--
	local pos = motionsensor.ProcessPositionTable( translator, sensor )

	
	--
	-- Fill out the angles
	--
	local ang = motionsensor.ProcessAnglesTable( translator, sensor, pos, rotation )

	--
	-- Allow the bone builder to make any last minute changes
	--
	translator:Complete( player, sensor, rotation, pos, ang )


	return pos, ang, sensor

end

