//===== Copyright © 1996-2005, Valve Corporation, All rights reserved. ======//
//
// Purpose: 
//
// $NoKeywords: $
//===========================================================================//

#include "jigglebones.h"

//-----------------------------------------------------------------------------
JiggleData * CJiggleBones::GetJiggleData( int bone, float currenttime, const Vector &initBasePos, const Vector &initTipPos )
{
	FOR_EACH_LL( m_jiggleBoneState, it )
	{
		if ( m_jiggleBoneState[it].bone == bone )
		{
			return &m_jiggleBoneState[it];
		}
	}

	JiggleData data;
	data.Init( bone, currenttime, initBasePos, initTipPos );

	int idx = m_jiggleBoneState.AddToHead( data );
	if ( idx == m_jiggleBoneState.InvalidIndex() )
		return NULL;

	return &m_jiggleBoneState[idx];
}


//-----------------------------------------------------------------------------
/**
 * Do spring physics calculations and update "jiggle bone" matrix
 * (Michael Booth, Turtle Rock Studios)
 */
void CJiggleBones::BuildJiggleTransformations( int boneIndex, float currenttime, const mstudiojigglebone_t *jiggleInfo, const matrix3x4 &goalMX, matrix3x4 &boneMX )
{
	Vector goalBasePosition = goalMX[3];

	Vector goalForward = goalMX[2];
	Vector goalUp = goalMX[1];
	Vector goalLeft = goalMX[0];

	// compute goal tip position
	Vector goalTip = goalBasePosition + jiggleInfo->length * goalForward;

	JiggleData *data = GetJiggleData( boneIndex, currenttime, goalBasePosition, goalTip );
	if( !data ) return;

	if( currenttime - data->lastUpdate > 0.5f )
	{
		data->Init( boneIndex, currenttime, goalBasePosition, goalTip );
	}

	// limit maximum deltaT to avoid simulation blowups
	// if framerate gets very low, jiggle will run in slow motion
	const float thirtyHZ = 0.0333f;
	const float thousandHZ = 0.001f;
	float deltaT = bound( thousandHZ, currenttime - data->lastUpdate, thirtyHZ );
	data->lastUpdate = currenttime;

	//
	// Bone tip flex
	//
	if (jiggleInfo->flags & (JIGGLE_IS_FLEXIBLE | JIGGLE_IS_RIGID))
	{
		// apply gravity in global space
		data->tipAccel.z -= jiggleInfo->tipMass;

		if (jiggleInfo->flags & JIGGLE_IS_FLEXIBLE)
		{
			// decompose into local coordinates
			Vector error = goalTip - data->tipPos;

			Vector localError;
			localError.x = DotProduct( goalLeft, error );
			localError.y = DotProduct( goalUp, error );
			localError.z = DotProduct( goalForward, error );

			Vector localVel;
			localVel.x = DotProduct( goalLeft, data->tipVel );
			localVel.y = DotProduct( goalUp, data->tipVel );

			// yaw spring
			float yawAccel = jiggleInfo->yawStiffness * localError.x - jiggleInfo->yawDamping * localVel.x;

			// pitch spring
			float pitchAccel = jiggleInfo->pitchStiffness * localError.y - jiggleInfo->pitchDamping * localVel.y;

			if (jiggleInfo->flags & JIGGLE_HAS_LENGTH_CONSTRAINT)
			{
				// drive tip towards goal tip position	
				data->tipAccel += yawAccel * goalLeft + pitchAccel * goalUp;
			}
			else
			{
				// allow flex along length of spring
				localVel.z = DotProduct( goalForward, data->tipVel );

				// along spring
				float alongAccel = jiggleInfo->alongStiffness * localError.z - jiggleInfo->alongDamping * localVel.z;

				// drive tip towards goal tip position	
				data->tipAccel += yawAccel * goalLeft + pitchAccel * goalUp + alongAccel * goalForward;
			}
		}


		// simple euler integration		
		data->tipVel += data->tipAccel * deltaT;
		data->tipPos += data->tipVel * deltaT;

		// clear this timestep's accumulated accelerations
		data->tipAccel = g_vecZero;		

		//
		// Apply optional constraints
		//
		if (jiggleInfo->flags & (JIGGLE_HAS_YAW_CONSTRAINT | JIGGLE_HAS_PITCH_CONSTRAINT))
		{
			// find components of spring vector in local coordinate system
			Vector along = data->tipPos - goalBasePosition;
			Vector localAlong;
			localAlong.x = DotProduct( goalLeft, along );
			localAlong.y = DotProduct( goalUp, along );
			localAlong.z = DotProduct( goalForward, along );

			Vector localVel;
			localVel.x = DotProduct( goalLeft, data->tipVel );
			localVel.y = DotProduct( goalUp, data->tipVel );
			localVel.z = DotProduct( goalForward, data->tipVel );

			if (jiggleInfo->flags & JIGGLE_HAS_YAW_CONSTRAINT)
			{
				// enforce yaw constraints in local XZ plane
				float yawError = atan2( localAlong.x, localAlong.z );

				bool isAtLimit = false;
				float yaw = 0.0f;

				if (yawError < jiggleInfo->minYaw)
				{
					// at angular limit
					isAtLimit = true;
					yaw = jiggleInfo->minYaw;
				}
				else if (yawError > jiggleInfo->maxYaw)
				{
					// at angular limit
					isAtLimit = true;
					yaw = jiggleInfo->maxYaw;
				}

				if (isAtLimit)
				{
					float sy, cy;
					SinCos( yaw, &sy, &cy );

					// yaw matrix
					matrix3x4 yawMatrix;

					yawMatrix.SetForward( Vector( cy, 0, -sy ));
					yawMatrix.SetRight( Vector( 0.0f, 1.0f, 0.0f ));
					yawMatrix.SetUp( Vector( sy, 0.0f, cy ));
					yawMatrix.SetOrigin( g_vecZero );

					// global coordinates of limit
					matrix3x4 limitMatrix = goalMX.ConcatTransforms( yawMatrix );

					Vector limitLeft( limitMatrix[0] );
					Vector limitUp( limitMatrix[1] );
					Vector limitForward( limitMatrix[2] );
					Vector limitAlong( DotProduct( limitLeft, along ), DotProduct( limitUp, along ), DotProduct( limitForward, along ));

					// clip to limit plane
					data->tipPos = goalBasePosition + limitAlong.y * limitUp + limitAlong.z * limitForward;

					// yaw friction - rubbing along limit plane
					Vector limitVel;
					limitVel.y = DotProduct( limitUp, data->tipVel );
					limitVel.z = DotProduct( limitForward, data->tipVel );

					data->tipAccel -= jiggleInfo->yawFriction * (limitVel.y * limitUp + limitVel.z * limitForward);

					// update velocity reaction to hitting constraint
					data->tipVel = -jiggleInfo->yawBounce * limitVel.x * limitLeft + limitVel.y * limitUp + limitVel.z * limitForward;

					// update along vectors for use by pitch constraint
					along = data->tipPos - goalBasePosition;
					localAlong.x = DotProduct( goalLeft, along );
					localAlong.y = DotProduct( goalUp, along );
					localAlong.z = DotProduct( goalForward, along );

					localVel.x = DotProduct( goalLeft, data->tipVel );
					localVel.y = DotProduct( goalUp, data->tipVel );
					localVel.z = DotProduct( goalForward, data->tipVel );
				}
			}


			if (jiggleInfo->flags & JIGGLE_HAS_PITCH_CONSTRAINT)
			{
				// enforce pitch constraints in local YZ plane
				float pitchError = atan2( localAlong.y, localAlong.z );

				bool isAtLimit = false;
				float pitch = 0.0f;

				if (pitchError < jiggleInfo->minPitch)
				{
					// at angular limit
					isAtLimit = true;
					pitch = jiggleInfo->minPitch;
				}
				else if (pitchError > jiggleInfo->maxPitch)
				{
					// at angular limit
					isAtLimit = true;
					pitch = jiggleInfo->maxPitch;
				}

				if (isAtLimit)
				{
					float sp, cp;
					SinCos( pitch, &sp, &cp );

					// pitch matrix
					matrix3x4 pitchMatrix;

					pitchMatrix.SetForward( Vector( 1.0f, 0.0, 0.0f ));
					pitchMatrix.SetRight( Vector( 0, cp, -sp ));
					pitchMatrix.SetUp( Vector( 0, sp, cp ));
					pitchMatrix.SetOrigin( g_vecZero );

					// global coordinates of limit
					matrix3x4 limitMatrix = goalMX.ConcatTransforms( pitchMatrix );

					Vector limitLeft( limitMatrix[0] );
					Vector limitUp( limitMatrix[1] );
					Vector limitForward( limitMatrix[2] );

					Vector limitAlong( DotProduct( limitLeft, along ), DotProduct( limitUp, along ), DotProduct( limitForward, along ));

					// clip to limit plane
					data->tipPos = goalBasePosition + limitAlong.x * limitLeft + limitAlong.z * limitForward;

					// pitch friction - rubbing along limit plane
					Vector limitVel;
					limitVel.y = DotProduct( limitUp, data->tipVel );
					limitVel.z = DotProduct( limitForward, data->tipVel );

					data->tipAccel -= jiggleInfo->pitchFriction * (limitVel.x * limitLeft + limitVel.z * limitForward);

					// update velocity reaction to hitting constraint
					data->tipVel = limitVel.x * limitLeft - jiggleInfo->pitchBounce * limitVel.y * limitUp + limitVel.z * limitForward;
				}
			}
		}

		// needed for matrix assembly below
		Vector forward = (data->tipPos - goalBasePosition).Normalize();

		if (jiggleInfo->flags & JIGGLE_HAS_ANGLE_CONSTRAINT)
		{
			// enforce max angular error
			Vector error = goalTip - data->tipPos;
			float dot = DotProduct( forward, goalForward );
			float angleBetween = acos( dot );
			if (dot < 0.0f)
			{
				angleBetween = 2.0f * M_PI - angleBetween;
			}

			if (angleBetween > jiggleInfo->angleLimit)
			{
				// at angular limit
				float maxBetween = jiggleInfo->length * sin( jiggleInfo->angleLimit );

				Vector delta = (goalTip - data->tipPos).Normalize();

				data->tipPos = goalTip - maxBetween * delta;

				forward = (data->tipPos - goalBasePosition).Normalize();
			}
		}

		if (jiggleInfo->flags & JIGGLE_HAS_LENGTH_CONSTRAINT)
		{
			// enforce spring length
			data->tipPos = goalBasePosition + jiggleInfo->length * forward;

			// zero velocity along forward bone axis
			data->tipVel -= DotProduct( data->tipVel, forward ) * forward;
		}

		//
		// Build bone matrix to align along current tip direction
		//
		Vector left = CrossProduct( goalUp, forward ).Normalize();

		Vector up = CrossProduct( forward, left );

		boneMX.SetForward( left );
		boneMX.SetRight( up );
		boneMX.SetUp( forward );
		boneMX.SetOrigin( goalBasePosition );
	}

	//
	// Bone base flex
	//
	if (jiggleInfo->flags & JIGGLE_HAS_BASE_SPRING)
	{
		// gravity
		data->baseAccel.z -= jiggleInfo->baseMass;

		// simple spring
		Vector error = goalBasePosition - data->basePos;
		data->baseAccel += jiggleInfo->baseStiffness * error - jiggleInfo->baseDamping * data->baseVel;

		data->baseVel += data->baseAccel * deltaT;
		data->basePos += data->baseVel * deltaT;

		// clear this timestep's accumulated accelerations
		data->baseAccel = g_vecZero;		

		// constrain to limits
		error = data->basePos - goalBasePosition;
		Vector localError;
		localError.x = DotProduct( goalLeft, error );
		localError.y = DotProduct( goalUp, error );
		localError.z = DotProduct( goalForward, error );

		Vector localVel;
		localVel.x = DotProduct( goalLeft, data->baseVel );
		localVel.y = DotProduct( goalUp, data->baseVel );
		localVel.z = DotProduct( goalForward, data->baseVel );

		// horizontal constraint
		if (localError.x < jiggleInfo->baseMinLeft)
		{
			localError.x = jiggleInfo->baseMinLeft;

			// friction
			data->baseAccel -= jiggleInfo->baseLeftFriction * (localVel.y * goalUp + localVel.z * goalForward);
		}
		else if (localError.x > jiggleInfo->baseMaxLeft)
		{
			localError.x = jiggleInfo->baseMaxLeft;

			// friction
			data->baseAccel -= jiggleInfo->baseLeftFriction * (localVel.y * goalUp + localVel.z * goalForward);
		}

		if (localError.y < jiggleInfo->baseMinUp)
		{
			localError.y = jiggleInfo->baseMinUp;

			// friction
			data->baseAccel -= jiggleInfo->baseUpFriction * (localVel.x * goalLeft + localVel.z * goalForward);
		}
		else if (localError.y > jiggleInfo->baseMaxUp)
		{
			localError.y = jiggleInfo->baseMaxUp;

			// friction
			data->baseAccel -= jiggleInfo->baseUpFriction * (localVel.x * goalLeft + localVel.z * goalForward);
		}

		if (localError.z < jiggleInfo->baseMinForward)
		{
			localError.z = jiggleInfo->baseMinForward;

			// friction
			data->baseAccel -= jiggleInfo->baseForwardFriction * (localVel.x * goalLeft + localVel.y * goalUp);
		}
		else if (localError.z > jiggleInfo->baseMaxForward)
		{
			localError.z = jiggleInfo->baseMaxForward;

			// friction
			data->baseAccel -= jiggleInfo->baseForwardFriction * (localVel.x * goalLeft + localVel.y * goalUp);
		}

		data->basePos = goalBasePosition + localError.x * goalLeft + localError.y * goalUp + localError.z * goalForward;


		// fix up velocity
		data->baseVel = (data->basePos - data->baseLastPos) / deltaT;
		data->baseLastPos = data->basePos;


		if (!(jiggleInfo->flags & (JIGGLE_IS_FLEXIBLE | JIGGLE_IS_RIGID)))
		{
			// no tip flex - use bone's goal orientation
			boneMX = goalMX;							
		}

		// update bone position
		boneMX.SetOrigin( data->basePos );
	}
	else if ( jiggleInfo->flags & JIGGLE_IS_BOING )
	{
		// estimate velocity
		Vector vel = goalBasePosition - data->lastBoingPos;

		data->lastBoingPos = goalBasePosition;

		float speed = vel.Length();
		vel = vel.Normalize();

		if( speed < 0.00001f )
		{
			vel = Vector( 0.0f, 0.0f, 1.0f );
			speed = 0.0f;
		}
		else
		{
			speed /= deltaT;
		}

		data->boingTime += deltaT;

		// if velocity changed a lot, we impacted and should *boing*
		const float minSpeed = 5.0f; // 15.0f;
		const float minReBoingTime = 0.5f;

		if(( speed > minSpeed || data->boingSpeed > minSpeed ) && data->boingTime > minReBoingTime )
		{
			if( fabs( data->boingSpeed - speed ) > jiggleInfo->boingImpactSpeed || DotProduct( vel, data->boingVelDir ) < jiggleInfo->boingImpactAngle )
			{
				data->boingTime = 0.0f;
				data->boingDir = -vel;
			}
		}

		data->boingVelDir = vel;
		data->boingSpeed = speed;

		float damping = 1.0f - ( jiggleInfo->boingDampingRate * data->boingTime );
		if ( damping < 0.01f )
		{
			// boing has entirely damped out
			boneMX = goalMX;
		}
		else
		{
			damping *= damping;
			damping *= damping;

			float flex = jiggleInfo->boingAmplitude * cos( jiggleInfo->boingFrequency * data->boingTime ) * damping;
 			float squash = 1.0f + flex;
 			float stretch = 1.0f - flex;

			boneMX.SetForward( goalLeft );
			boneMX.SetRight( goalUp );
			boneMX.SetUp( goalForward );
			boneMX.SetOrigin( g_vecZero );

			// build transform into "boing space", where Z is along primary boing axis
			Vector boingSide;
			if( fabs( data->boingDir.x ) < 0.9f )
			{
				boingSide = CrossProduct( data->boingDir, Vector( 1.0f, 0.0f, 0.0f )).Normalize();
			}
			else
			{
				boingSide = CrossProduct( data->boingDir, Vector( 0.0f, 0.0f, 1.0f )).Normalize();
			}

			Vector boingOtherSide = CrossProduct( data->boingDir, boingSide );

			matrix3x4 xfrmToBoingCoordsMX, xfrmFromBoingCoordsMX;

			xfrmToBoingCoordsMX.SetForward( boingSide );
			xfrmToBoingCoordsMX.SetRight( boingOtherSide );
			xfrmToBoingCoordsMX.SetUp( data->boingDir );
			xfrmToBoingCoordsMX.SetOrigin( g_vecZero );

			// transform back from boing space (inverse is transpose since orthogonal)
			xfrmFromBoingCoordsMX = xfrmToBoingCoordsMX;
			xfrmToBoingCoordsMX = xfrmToBoingCoordsMX.Transpose();

			// build squash and stretch transform in "boing space"
			matrix3x4 boingMX;

			boingMX.SetForward( Vector( squash, 0.0f, 0.0f ));
			boingMX.SetRight( Vector( 0.0f, squash, 0.0f ));
			boingMX.SetUp( Vector( 0.0f, 0.0f, stretch ));
			boingMX.SetOrigin( g_vecZero );

			// put it all together
			matrix3x4 xfrmMX;
			xfrmMX = xfrmToBoingCoordsMX.ConcatTransforms( boingMX );
			xfrmMX = xfrmMX.ConcatTransforms( xfrmFromBoingCoordsMX );
			boneMX = boneMX.ConcatTransforms( xfrmMX );
			boneMX.SetOrigin( goalBasePosition );
		}
	}
	else if (!(jiggleInfo->flags & (JIGGLE_IS_FLEXIBLE | JIGGLE_IS_RIGID)))
	{
		// no flex at all - just use goal matrix
		boneMX = goalMX;
	}
}