/*
===========================================================================

Doom 3 BFG Edition GPL Source Code
Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company.

This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").

Doom 3 BFG Edition Source Code is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

Doom 3 BFG Edition Source Code is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with Doom 3 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.

In addition, the Doom 3 BFG Edition Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 BFG Edition Source Code.  If not, please request a copy in writing from id Software at the address below.

If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.

===========================================================================
*/

#include "sys/platform.h"
#include "idlib/geometry/JointTransform.h"

#include "gamesys/SysCvar.h"
#include "Mover.h"

#include "IK.h"
#include "Vr.h"
#include "Player.h"

idCVar rollfix("rollfix", "20", CVAR_FLOAT, "");
idCVar rfx("rfx", "0", CVAR_FLOAT, "");
idCVar rfy("rfy", "0", CVAR_FLOAT, "");
idCVar rfz("rfz", "0", CVAR_FLOAT, "");

// idCVar rhx( "rhx", "-1", CVAR_FLOAT, "" );
// idCVar rhy( "rhy", "0", CVAR_FLOAT, "" );
// idCVar rhz( "rhz", "0", CVAR_FLOAT, "" );

/*
===============================================================================

  idIK

===============================================================================
*/

/*
================
idIK::idIK
================
*/
idIK::idIK()
{
	ik_activate = false;
	initialized = false;
	self = NULL;
	animator = NULL;
	modifiedAnim = 0;
	modelOffset.Zero();
}

/*
================
idIK::~idIK
================
*/
idIK::~idIK()
{
}

/*
================
idIK::Save
================
*/
void idIK::Save( idSaveGame* savefile ) const
{
	savefile->WriteBool( initialized );
	savefile->WriteBool( ik_activate );
	savefile->WriteObject( self );
	savefile->WriteString( animator != NULL && animator->GetAnim( modifiedAnim ) ? animator->GetAnim( modifiedAnim )->Name() : "" );
	savefile->WriteVec3( modelOffset );
}

/*
================
idIK::Restore
================
*/
void idIK::Restore( idRestoreGame* savefile )
{
	idStr anim;

	savefile->ReadBool( initialized );
	savefile->ReadBool( ik_activate );
	savefile->ReadObject( reinterpret_cast<idClass*&>( self ) );
	savefile->ReadString( anim );
	savefile->ReadVec3( modelOffset );

	if( self )
	{
		animator = self->GetAnimator();
		if( animator == NULL || animator->ModelDef() == NULL )
		{
			gameLocal.Warning( "idIK::Restore: IK for entity '%s' at (%s) has no model set.",
							   self->name.c_str(), self->GetPhysics()->GetOrigin().ToString( 0 ) );
			return;
		}
		modifiedAnim = animator->GetAnim( anim );
		if( modifiedAnim == 0 )
		{
			gameLocal.Warning( "idIK::Restore: IK for entity '%s' at (%s) has no modified animation.",
							   self->name.c_str(), self->GetPhysics()->GetOrigin().ToString( 0 ) );
		}
	}
	else
	{
		animator = NULL;
		modifiedAnim = 0;
	}
}

/*
================
idIK::IsInitialized
================
*/
bool idIK::IsInitialized() const
{
	return initialized && ik_enable.GetBool();
}

/*
================
idIK::Init
================
*/
bool idIK::Init( idEntity* self, const char* anim, const idVec3& modelOffset )
{
	idRenderModel* model;

	if( self == NULL )
	{
		return false;
	}

	this->self = self;

	animator = self->GetAnimator();
	if( animator == NULL || animator->ModelDef() == NULL )
	{
		gameLocal.Warning( "idIK::Init: IK for entity '%s' at (%s) has no model set.",
						   self->name.c_str(), self->GetPhysics()->GetOrigin().ToString( 0 ) );
		return false;
	}
	if( animator->ModelDef()->ModelHandle() == NULL )
	{
		gameLocal.Warning( "idIK::Init: IK for entity '%s' at (%s) uses default model.",
						   self->name.c_str(), self->GetPhysics()->GetOrigin().ToString( 0 ) );
		return false;
	}
	model = animator->ModelHandle();
	if( model == NULL )
	{
		gameLocal.Warning( "idIK::Init: IK for entity '%s' at (%s) has no model set.",
						   self->name.c_str(), self->GetPhysics()->GetOrigin().ToString( 0 ) );
		return false;
	}
	modifiedAnim = animator->GetAnim( anim );
	if( modifiedAnim == 0 )
	{
		gameLocal.Warning( "idIK::Init: IK for entity '%s' at (%s) has no modified animation.",
						   self->name.c_str(), self->GetPhysics()->GetOrigin().ToString( 0 ) );
		return false;
	}

	this->modelOffset = modelOffset;

	return true;
}

/*
================
idIK::Evaluate
================
*/
void idIK::Evaluate()
{
}

/*
================
idIK::ClearJointMods
================
*/
void idIK::ClearJointMods()
{
	ik_activate = false;
}

/*
================
idIK::SolveTwoBones
================
*/
bool idIK::SolveTwoBones( const idVec3& startPos, const idVec3& endPos, const idVec3& dir, float len0, float len1, idVec3& jointPos )
{
	float length, lengthSqr, lengthInv, x, y;
	idVec3 vec0, vec1;

	vec0 = endPos - startPos;
	lengthSqr = vec0.LengthSqr();
	lengthInv = idMath::InvSqrt( lengthSqr );
	length = lengthInv * lengthSqr;

	// if the start and end position are too far out or too close to each other
	if( length > len0 + len1 || length < idMath::Fabs( len0 - len1 ) )
	{
		jointPos = startPos + 0.5f * vec0;
		return false;
	}

	vec0 *= lengthInv;
	vec1 = dir - vec0 * dir * vec0;
	vec1.Normalize();

	x = ( length * length + len0 * len0 - len1 * len1 ) * ( 0.5f * lengthInv );
	y = idMath::Sqrt( len0 * len0 - x * x );

	jointPos = startPos + x * vec0 + y * vec1;

	return true;
}

/*
================
// Koz
// Moves the elbow towards the wrist (stretches the upper arm) if player reach exceeds model arm length when using motion controls
// idIK::SolveTwoArmBones
================
*/
bool idIK::SolveTwoArmBones( idVec3& startPos, idVec3& endPos, const idVec3& dir, float len0, float len1, idVec3& jointPos )
{
	float length, lengthSqr, lengthInv, x, y, maxLen, lenDif;
	idVec3 vec0, vec1, vec2, vec3;

	vec0 = endPos - startPos;
	lengthSqr = vec0.LengthSqr();
	lengthInv = idMath::InvSqrt( lengthSqr );
	length = lengthInv * lengthSqr;


	// Koz - if using motion controls and displaying the body, and a controller is reporting a impossible position, restrain the arm length.

	if ( 0 && game->isVR && vr_playerBodyMode.GetInteger() == 0 )
	{
		maxLen = (len0 + len1) * 1.40f;
		if ( length > maxLen )
		{

			lenDif = (maxLen - length) / 2.0f;
			vec0.Normalize();
			endPos = startPos + maxLen * vec0;

			jointPos = endPos - (len1 - lenDif) * vec0;
			startPos = startPos + lenDif * vec0;
			return false;
		}
	}
	// if the start and end position are too far out or too close to each other
	if ( length > len0 + len1 )
	{
		vec0.Normalize();
		jointPos = endPos - len1 * vec0;
		return false;
	}
	else if ( length < idMath::Fabs( len0 - len1 ) )
	{
		jointPos = startPos + (0.5f * vec0);
		return false;
	}

	vec0 *= lengthInv;
	vec1 = dir - vec0 * dir * vec0;
	vec1.Normalize();

	x = (length * length + len0 * len0 - len1 * len1) * (0.5f * lengthInv);
	y = idMath::Sqrt( len0 * len0 - x * x );

	jointPos = startPos + x * vec0 + y * vec1;



	float dist1 = idMath::Sqrt( (endPos.x - startPos.x) * (endPos.x - startPos.x) + (endPos.y - startPos.y) * (endPos.y - startPos.y) + (endPos.z - startPos.z) * (endPos.z - startPos.z) );
	float dist2 = idMath::Sqrt( (len0 * len0) + (len1 * len1) );

	if ( dist1 <= dist2 + 2.0 ) return true;
	return false;
	/*
	float length, lengthSqr, lengthInv, x, y;
	idVec3 vec0, vec1;

	vec0 = endPos - startPos;
	lengthSqr = vec0.LengthSqr();
	lengthInv = idMath::InvSqrt( lengthSqr );
	length = lengthInv * lengthSqr;

	// if the start and end position are too far out or too close to each other
	if ( length > len0 + len1 || length < idMath::Fabs( len0 - len1 ) ) {
		jointPos = startPos + 0.5f * vec0;
		return false;
	}

	vec0 *= lengthInv;
	vec1 = dir - vec0 * dir * vec0;
	vec1.Normalize();

	x = (length * length + len0 * len0 - len1 * len1) * (0.5f * lengthInv);
	y = idMath::Sqrt( len0 * len0 - x * x );

	jointPos = startPos + x * vec0 + y * vec1;

	if ( length <= len0 * 1.05 )
	{
		return true;
	}

	return false;
	*/
}
/*
================
idIK::GetBoneAxis
================
*/
float idIK::GetBoneAxis( const idVec3& startPos, const idVec3& endPos, const idVec3& dir, idMat3& axis, bool debug  )
{
	float length;
	axis[0] = endPos - startPos;
	length = axis[0].Normalize();

	axis[1] = dir - axis[0] * dir * axis[0];
	axis[1].Normalize();
	axis[2].Cross( axis[1], axis[0] );

	if ( debug )
	{
		common->Printf( "\nGBA [0] x y z %f %f %f ", axis[0].x, axis[0].y, axis[0].z );
		common->Printf( "[1]  %f %f %f ", axis[1].x, axis[1].y, axis[1].z );
		common->Printf( "[2]  %f %f %f ", axis[2].x, axis[2].y, axis[2].z );
	}
	return length;
}

/*
================
idIK::GetBoneAxis2
================
*/
float idIK::GetBoneAxis2( const idVec3& startPos, const idVec3& endPos, const idVec3& dir, idMat3& axis )
{


	idRotation bonRot;
	bonRot.SetVec( endPos - startPos );
	bonRot.SetAngle( 0.0f );
	axis = bonRot.ToMat3();

	float length;
	//axis[0] = endPos - startPos;
	length = axis[0].Normalize();
	//axis[1] = dir - axis[0] * dir * axis[0];
	//axis[1].Normalize();
	//axis[2].Cross( axis[1], axis[0] );
	return length;
}


/*
===============================================================================

  idIK_Walk

===============================================================================
*/

/*
================
idIK_Walk::idIK_Walk
================
*/
idIK_Walk::idIK_Walk()
{
	int i;

	initialized = false;
	footModel = NULL;
	numLegs = 0;
	enabledLegs = 0;
	for( i = 0; i < MAX_LEGS; i++ )
	{
		footJoints[i] = INVALID_JOINT;
		ankleJoints[i] = INVALID_JOINT;
		kneeJoints[i] = INVALID_JOINT;
		hipJoints[i] = INVALID_JOINT;
		dirJoints[i] = INVALID_JOINT;
		hipForward[i].Zero();
		kneeForward[i].Zero();
		upperLegLength[i] = 0.0f;
		lowerLegLength[i] = 0.0f;
		upperLegToHipJoint[i].Identity();
		lowerLegToKneeJoint[i].Identity();
		oldAnkleHeights[i] = 0.0f;
	}
	waistJoint = INVALID_JOINT;

	smoothing = 0.75f;
	waistSmoothing = 0.5f;
	footShift = 0.0f;
	waistShift = 0.0f;
	minWaistFloorDist = 0.0f;
	minWaistAnkleDist = 0.0f;
	footUpTrace = 32.0f;
	footDownTrace = 32.0f;
	tiltWaist = false;
	usePivot = false;

	pivotFoot = -1;
	pivotYaw = 0.0f;
	pivotPos.Zero();

	oldHeightsValid = false;
	oldWaistHeight = 0.0f;
	waistOffset.Zero();
}

/*
================
idIK_Walk::~idIK_Walk
================
*/
idIK_Walk::~idIK_Walk()
{
	if( footModel )
	{
		delete footModel;
	}
}

/*
================
idIK_Walk::Save
================
*/
void idIK_Walk::Save( idSaveGame* savefile ) const
{
	int i;

	idIK::Save( savefile );

	savefile->WriteClipModel( footModel );

	savefile->WriteInt( numLegs );
	savefile->WriteInt( enabledLegs );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->WriteInt( footJoints[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->WriteInt( ankleJoints[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->WriteInt( kneeJoints[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->WriteInt( hipJoints[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->WriteInt( dirJoints[i] );
	savefile->WriteInt( waistJoint );

	for( i = 0; i < MAX_LEGS; i++ )
		savefile->WriteVec3( hipForward[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->WriteVec3( kneeForward[i] );

	for( i = 0; i < MAX_LEGS; i++ )
		savefile->WriteFloat( upperLegLength[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->WriteFloat( lowerLegLength[i] );

	for( i = 0; i < MAX_LEGS; i++ )
		savefile->WriteMat3( upperLegToHipJoint[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->WriteMat3( lowerLegToKneeJoint[i] );

	savefile->WriteFloat( smoothing );
	savefile->WriteFloat( waistSmoothing );
	savefile->WriteFloat( footShift );
	savefile->WriteFloat( waistShift );
	savefile->WriteFloat( minWaistFloorDist );
	savefile->WriteFloat( minWaistAnkleDist );
	savefile->WriteFloat( footUpTrace );
	savefile->WriteFloat( footDownTrace );
	savefile->WriteBool( tiltWaist );
	savefile->WriteBool( usePivot );

	savefile->WriteInt( pivotFoot );
	savefile->WriteFloat( pivotYaw );
	savefile->WriteVec3( pivotPos );
	savefile->WriteBool( oldHeightsValid );
	savefile->WriteFloat( oldWaistHeight );
	for( i = 0; i < MAX_LEGS; i++ )
	{
		savefile->WriteFloat( oldAnkleHeights[i] );
	}
	savefile->WriteVec3( waistOffset );
}

/*
================
idIK_Walk::Restore
================
*/
void idIK_Walk::Restore( idRestoreGame* savefile )
{
	int i;

	idIK::Restore( savefile );

	savefile->ReadClipModel( footModel );

	savefile->ReadInt( numLegs );
	savefile->ReadInt( enabledLegs );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->ReadInt( ( int& )footJoints[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->ReadInt( ( int& )ankleJoints[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->ReadInt( ( int& )kneeJoints[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->ReadInt( ( int& )hipJoints[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->ReadInt( ( int& )dirJoints[i] );
	savefile->ReadInt( ( int& )waistJoint );

	for( i = 0; i < MAX_LEGS; i++ )
		savefile->ReadVec3( hipForward[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->ReadVec3( kneeForward[i] );

	for( i = 0; i < MAX_LEGS; i++ )
		savefile->ReadFloat( upperLegLength[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->ReadFloat( lowerLegLength[i] );

	for( i = 0; i < MAX_LEGS; i++ )
		savefile->ReadMat3( upperLegToHipJoint[i] );
	for( i = 0; i < MAX_LEGS; i++ )
		savefile->ReadMat3( lowerLegToKneeJoint[i] );

	savefile->ReadFloat( smoothing );
	savefile->ReadFloat( waistSmoothing );
	savefile->ReadFloat( footShift );
	savefile->ReadFloat( waistShift );
	savefile->ReadFloat( minWaistFloorDist );
	savefile->ReadFloat( minWaistAnkleDist );
	savefile->ReadFloat( footUpTrace );
	savefile->ReadFloat( footDownTrace );
	savefile->ReadBool( tiltWaist );
	savefile->ReadBool( usePivot );

	savefile->ReadInt( pivotFoot );
	savefile->ReadFloat( pivotYaw );
	savefile->ReadVec3( pivotPos );
	savefile->ReadBool( oldHeightsValid );
	savefile->ReadFloat( oldWaistHeight );
	for( i = 0; i < MAX_LEGS; i++ )
	{
		savefile->ReadFloat( oldAnkleHeights[i] );
	}
	savefile->ReadVec3( waistOffset );
}

/*
================
idIK_Walk::Init
================
*/
bool idIK_Walk::Init( idEntity* self, const char* anim, const idVec3& modelOffset )
{
	int i;
	float footSize;
	idVec3 verts[4];
	idTraceModel trm;
	const char* jointName;
	idVec3 dir, ankleOrigin, kneeOrigin, hipOrigin, dirOrigin;
	idMat3 axis, ankleAxis, kneeAxis, hipAxis;

	static idVec3 footWinding[4] =
			{
					idVec3( 1.0f,  1.0f, 0.0f ),
					idVec3( -1.0f,  1.0f, 0.0f ),
					idVec3( -1.0f, -1.0f, 0.0f ),
					idVec3( 1.0f, -1.0f, 0.0f )
			};

	if( !self )
	{
		return false;
	}

	numLegs = Min( self->spawnArgs.GetInt( "ik_numLegs", "0" ), MAX_LEGS );
	if( numLegs == 0 )
	{
		return true;
	}

	if( !idIK::Init( self, anim, modelOffset ) )
	{
		return false;
	}

	int numJoints = animator->NumJoints();
	idJointMat* joints = ( idJointMat* )_alloca16( numJoints * sizeof( joints[0] ) );

	// create the animation frame used to setup the IK
	gameEdit->ANIM_CreateAnimFrame( animator->ModelHandle(), animator->GetAnim( modifiedAnim )->MD5Anim( 0 ), numJoints, joints, 1, animator->ModelDef()->GetVisualOffset() + modelOffset, animator->RemoveOrigin() );

	enabledLegs = 0;

	// get all the joints
	for( i = 0; i < numLegs; i++ )
	{

		jointName = self->spawnArgs.GetString( va( "ik_foot%d", i + 1 ) );
		footJoints[i] = animator->GetJointHandle( jointName );
		if( footJoints[i] == INVALID_JOINT )
		{
			gameLocal.Error( "idIK_Walk::Init: invalid foot joint '%s'", jointName );
		}

		jointName = self->spawnArgs.GetString( va( "ik_ankle%d", i + 1 ) );
		ankleJoints[i] = animator->GetJointHandle( jointName );
		if( ankleJoints[i] == INVALID_JOINT )
		{
			gameLocal.Error( "idIK_Walk::Init: invalid ankle joint '%s'", jointName );
		}

		jointName = self->spawnArgs.GetString( va( "ik_knee%d", i + 1 ) );
		kneeJoints[i] = animator->GetJointHandle( jointName );
		if( kneeJoints[i] == INVALID_JOINT )
		{
			gameLocal.Error( "idIK_Walk::Init: invalid knee joint '%s'\n", jointName );
		}

		jointName = self->spawnArgs.GetString( va( "ik_hip%d", i + 1 ) );
		hipJoints[i] = animator->GetJointHandle( jointName );
		if( hipJoints[i] == INVALID_JOINT )
		{
			gameLocal.Error( "idIK_Walk::Init: invalid hip joint '%s'\n", jointName );
		}

		jointName = self->spawnArgs.GetString( va( "ik_dir%d", i + 1 ) );
		dirJoints[i] = animator->GetJointHandle( jointName );

		enabledLegs |= 1 << i;
	}

	jointName = self->spawnArgs.GetString( "ik_waist" );
	waistJoint = animator->GetJointHandle( jointName );
	if( waistJoint == INVALID_JOINT )
	{
		gameLocal.Error( "idIK_Walk::Init: invalid waist joint '%s'\n", jointName );
	}

	// get the leg bone lengths and rotation matrices
	for( i = 0; i < numLegs; i++ )
	{
		oldAnkleHeights[i] = 0.0f;

		ankleAxis = joints[ ankleJoints[ i ] ].ToMat3();
		ankleOrigin = joints[ ankleJoints[ i ] ].ToVec3();

		kneeAxis = joints[ kneeJoints[ i ] ].ToMat3();
		kneeOrigin = joints[ kneeJoints[ i ] ].ToVec3();

		hipAxis = joints[ hipJoints[ i ] ].ToMat3();
		hipOrigin = joints[ hipJoints[ i ] ].ToVec3();

		// get the IK direction
		if( dirJoints[i] != INVALID_JOINT )
		{
			dirOrigin = joints[ dirJoints[ i ] ].ToVec3();
			dir = dirOrigin - kneeOrigin;
		}
		else
		{
			dir.Set( 1.0f, 0.0f, 0.0f );
		}

		hipForward[i] = dir * hipAxis.Transpose();
		kneeForward[i] = dir * kneeAxis.Transpose();

		// conversion from upper leg bone axis to hip joint axis
		upperLegLength[i] = GetBoneAxis( hipOrigin, kneeOrigin, dir, axis );
		upperLegToHipJoint[i] = hipAxis * axis.Transpose();

		// conversion from lower leg bone axis to knee joint axis
		lowerLegLength[i] = GetBoneAxis( kneeOrigin, ankleOrigin, dir, axis );
		lowerLegToKneeJoint[i] = kneeAxis * axis.Transpose();
	}

	smoothing = self->spawnArgs.GetFloat( "ik_smoothing", "0.75" );
	waistSmoothing = self->spawnArgs.GetFloat( "ik_waistSmoothing", "0.75" );
	footShift = self->spawnArgs.GetFloat( "ik_footShift", "0" );
	waistShift = self->spawnArgs.GetFloat( "ik_waistShift", "0" );
	minWaistFloorDist = self->spawnArgs.GetFloat( "ik_minWaistFloorDist", "0" );
	minWaistAnkleDist = self->spawnArgs.GetFloat( "ik_minWaistAnkleDist", "0" );
	footUpTrace = self->spawnArgs.GetFloat( "ik_footUpTrace", "32" );
	footDownTrace = self->spawnArgs.GetFloat( "ik_footDownTrace", "32" );
	tiltWaist = self->spawnArgs.GetBool( "ik_tiltWaist", "0" );
	usePivot = self->spawnArgs.GetBool( "ik_usePivot", "0" );

	// setup a clip model for the feet
	footSize = self->spawnArgs.GetFloat( "ik_footSize", "4" ) * 0.5f;
	if( footSize > 0.0f )
	{
		for( i = 0; i < 4; i++ )
		{
			verts[i] = footWinding[i] * footSize;
		}
		trm.SetupPolygon( verts, 4 );
        footModel = new idClipModel( trm );
	}

	initialized = true;
	return true;
}

/*
================
idIK_Walk::Evaluate
================
*/
void idIK_Walk::Evaluate()
{
	int i, newPivotFoot = -1;
	float modelHeight, jointHeight, lowestHeight, floorHeights[MAX_LEGS];
	float shift, smallestShift, newHeight, step, newPivotYaw, height, largestAnkleHeight;
	idVec3 modelOrigin, normal, hipDir, kneeDir, start, end, jointOrigins[MAX_LEGS];
	idVec3 footOrigin, ankleOrigin, kneeOrigin, hipOrigin, waistOrigin;
	idMat3 modelAxis, waistAxis, axis;
	idMat3 hipAxis[MAX_LEGS], kneeAxis[MAX_LEGS], ankleAxis[MAX_LEGS];
	trace_t results;

	// Koz begin
	bool isPlayer = false;
	idPlayer *player;
	player = gameLocal.GetLocalPlayer();
	if ( player )
	{
		if ( player->entityNumber == self->entityNumber )
		{
			isPlayer = true;
		}
	}
	// Koz end

	if( !self || !gameLocal.isNewFrame )
	{
		return;
	}

	// if no IK enabled on any legs
	if( !enabledLegs )
	{
		return;
	}

	normal = - self->GetPhysics()->GetGravityNormal();
	modelOrigin = self->GetPhysics()->GetOrigin();
	modelAxis = self->GetRenderEntity()->axis;
	modelHeight = modelOrigin * normal;

	modelOrigin += modelOffset * modelAxis;

	// create frame without joint mods
	animator->CreateFrame( gameLocal.time, false );

	// get the joint positions for the feet
	lowestHeight = idMath::INFINITY;
	for( i = 0; i < numLegs; i++ )
	{
		animator->GetJointTransform( footJoints[i], gameLocal.time, footOrigin, axis );
		jointOrigins[i] = modelOrigin + footOrigin * modelAxis;
		jointHeight = jointOrigins[i] * normal;
		if( jointHeight < lowestHeight )
		{
			lowestHeight = jointHeight;
			newPivotFoot = i;
		}
	}

	if( usePivot )
	{

		newPivotYaw = modelAxis[0].ToYaw();

		// change pivot foot
		if( newPivotFoot != pivotFoot || idMath::Fabs( idMath::AngleNormalize180( newPivotYaw - pivotYaw ) ) > 30.0f )
		{
			pivotFoot = newPivotFoot;
			pivotYaw = newPivotYaw;
			animator->GetJointTransform( footJoints[pivotFoot], gameLocal.time, footOrigin, axis );
			pivotPos = modelOrigin + footOrigin * modelAxis;
		}

		// keep pivot foot in place
		jointOrigins[pivotFoot] = pivotPos;
	}

	// get the floor heights for the feet
	for( i = 0; i < numLegs; i++ )
	{

		if( !( enabledLegs & ( 1 << i ) ) )
		{
			continue;
		}

		start = jointOrigins[i] + normal * footUpTrace;
		end = jointOrigins[i] - normal * footDownTrace;
		gameLocal.clip.Translation( results, start, end, footModel, mat3_identity, CONTENTS_SOLID | CONTENTS_IKCLIP, self );
		floorHeights[i] = results.endpos * normal;

		if( ik_debug.GetBool() && footModel )
		{
			idFixedWinding w;
			for( int j = 0; j < footModel->GetTraceModel()->numVerts; j++ )
			{
				w += footModel->GetTraceModel()->verts[j];
			}
			gameRenderWorld->DebugWinding( colorRed, w, results.endpos, results.endAxis );
		}
	}



	const idPhysics* phys = self->GetPhysics();

	// test whether or not the character standing on the ground
	bool onGround = phys->HasGroundContacts();

	// test whether or not the character is standing on a plat
	bool onPlat = false;
	for( i = 0; i < phys->GetNumContacts(); i++ )
	{
		idEntity* ent = gameLocal.entities[ phys->GetContact( i ).entityNum ];
		if( ent != NULL && ent->IsType( idPlat::Type ) )
		{
			onPlat = true;
			break;
		}
	}

	// adjust heights of the ankles
	smallestShift = idMath::INFINITY;
	largestAnkleHeight = -idMath::INFINITY;
	for( i = 0; i < numLegs; i++ )
	{

		if( onGround && ( enabledLegs & ( 1 << i ) ) )
		{
			shift = floorHeights[i] - modelHeight + footShift;
		}
		else
		{
			shift = 0.0f;
		}

		if( shift < smallestShift )
		{
			smallestShift = shift;
		}

		animator->GetJointTransform( ankleJoints[i], gameLocal.time, ankleOrigin, ankleAxis[i] );
		jointOrigins[i] = modelOrigin + ankleOrigin * modelAxis;

		height = jointOrigins[i] * normal;

		if( oldHeightsValid && !onPlat )
		{
			step = height + shift - oldAnkleHeights[i];
			shift -= smoothing * step;
		}

		newHeight = height + shift;
		if( newHeight > largestAnkleHeight )
		{
			largestAnkleHeight = newHeight;
		}

		oldAnkleHeights[i] = newHeight;

		jointOrigins[i] += shift * normal;
	}

	animator->GetJointTransform( waistJoint, gameLocal.time, waistOrigin, waistAxis );
	waistOrigin = modelOrigin + waistOrigin * modelAxis;



	// adjust position of the waist
	waistOffset = ( smallestShift + waistShift ) * normal;
	// Koz add waist IK for crouching
	if ( isPlayer  )
	{

		if ( vr_crouchMode.GetInteger() == 0 || (vr_crouchMode.GetInteger() != 0 && !player->IsCrouching()) )
		{
			//common->Printf( "Body delta z %f\n", commonVr->poseHmdBodyPositionDelta.z );
			if ( commonVr->poseHmdBodyPositionDelta.z < 0.0f )
			{
				waistOffset.z += commonVr->poseHmdBodyPositionDelta.z;
			}
		}
	}

	// if the waist should be at least a certain distance above the floor
	if( minWaistFloorDist > 0.0f && waistOffset * normal < 0.0f )
	{
		start = waistOrigin;
		end = waistOrigin + waistOffset - normal * minWaistFloorDist;
		gameLocal.clip.Translation( results, start, end, footModel, modelAxis, CONTENTS_SOLID | CONTENTS_IKCLIP, self );
		height = ( waistOrigin + waistOffset - results.endpos ) * normal;
		if( height < minWaistFloorDist )
		{
			waistOffset += ( minWaistFloorDist - height ) * normal;
		}
	}

	// if the waist should be at least a certain distance above the ankles
	if( minWaistAnkleDist > 0.0f )
	{
		height = ( waistOrigin + waistOffset ) * normal;
		if( height - largestAnkleHeight < minWaistAnkleDist )
		{
			waistOffset += ( minWaistAnkleDist - ( height - largestAnkleHeight ) ) * normal;
		}
	}

	if( oldHeightsValid )
	{
		// smoothly adjust height of waist
		newHeight = ( waistOrigin + waistOffset ) * normal;
		step = newHeight - oldWaistHeight;
		waistOffset -= waistSmoothing * step * normal;
	}

	// save height of waist for smoothing
	oldWaistHeight = ( waistOrigin + waistOffset ) * normal;

	if( !oldHeightsValid )
	{
		oldHeightsValid = true;
		return;
	}

	// solve IK
	for( i = 0; i < numLegs; i++ )
	{

		// get the position of the hip in world space
		animator->GetJointTransform( hipJoints[i], gameLocal.time, hipOrigin, axis );
		hipOrigin = modelOrigin + waistOffset + hipOrigin * modelAxis;
		hipDir = hipForward[i] * axis * modelAxis;

		// get the IK bend direction
		animator->GetJointTransform( kneeJoints[i], gameLocal.time, kneeOrigin, axis );
		kneeDir = kneeForward[i] * axis * modelAxis;

		// solve IK and calculate knee position
		SolveTwoBones( hipOrigin, jointOrigins[i], kneeDir, upperLegLength[i], lowerLegLength[i], kneeOrigin );

		if( ik_debug.GetBool() )
		{
			gameRenderWorld->DebugLine( colorCyan, hipOrigin, kneeOrigin );
			gameRenderWorld->DebugLine( colorRed, kneeOrigin, jointOrigins[i] );
			gameRenderWorld->DebugLine( colorYellow, kneeOrigin, kneeOrigin + hipDir );
			gameRenderWorld->DebugLine( colorGreen, kneeOrigin, kneeOrigin + kneeDir );
		}

		// get the axis for the hip joint
		GetBoneAxis( hipOrigin, kneeOrigin, hipDir, axis );
		hipAxis[i] = upperLegToHipJoint[i] * ( axis * modelAxis.Transpose() );

		// get the axis for the knee joint
		GetBoneAxis( kneeOrigin, jointOrigins[i], kneeDir, axis );
		kneeAxis[i] = lowerLegToKneeJoint[i] * ( axis * modelAxis.Transpose() );
	}

	// set the joint mods
	animator->SetJointAxis( waistJoint, JOINTMOD_WORLD_OVERRIDE, waistAxis );
	animator->SetJointPos( waistJoint, JOINTMOD_WORLD_OVERRIDE, ( waistOrigin + waistOffset - modelOrigin ) * modelAxis.Transpose() );
	for( i = 0; i < numLegs; i++ )
	{
		animator->SetJointAxis( hipJoints[i], JOINTMOD_WORLD_OVERRIDE, hipAxis[i] );
		animator->SetJointAxis( kneeJoints[i], JOINTMOD_WORLD_OVERRIDE, kneeAxis[i] );
		animator->SetJointAxis( ankleJoints[i], JOINTMOD_WORLD_OVERRIDE, ankleAxis[i] );
	}

	ik_activate = true;
}

/*
================
idIK_Walk::ClearJointMods
================
*/
void idIK_Walk::ClearJointMods()
{
	int i;

	if( !self || !ik_activate )
	{
		return;
	}

	animator->SetJointAxis( waistJoint, JOINTMOD_NONE, mat3_identity );
	animator->SetJointPos( waistJoint, JOINTMOD_NONE, vec3_origin );
	for( i = 0; i < numLegs; i++ )
	{
		animator->SetJointAxis( hipJoints[i], JOINTMOD_NONE, mat3_identity );
		animator->SetJointAxis( kneeJoints[i], JOINTMOD_NONE, mat3_identity );
		animator->SetJointAxis( ankleJoints[i], JOINTMOD_NONE, mat3_identity );
	}

	ik_activate = false;
}

/*
================
idIK_Walk::EnableAll
================
*/
void idIK_Walk::EnableAll()
{
	enabledLegs = ( 1 << numLegs ) - 1;
	oldHeightsValid = false;
}

/*
================
idIK_Walk::DisableAll
================
*/
void idIK_Walk::DisableAll()
{
	enabledLegs = 0;
	oldHeightsValid = false;
}

/*
================
idIK_Walk::EnableLeg
================
*/
void idIK_Walk::EnableLeg( int num )
{
	enabledLegs |= 1 << num;
}

/*
================
idIK_Walk::DisableLeg
================
*/
void idIK_Walk::DisableLeg( int num )
{
	enabledLegs &= ~( 1 << num );
}


/*
===============================================================================

  idIK_Reach

===============================================================================
*/

/*
================
idIK_Reach::idIK_Reach
================
*/
idIK_Reach::idIK_Reach()
{
	int i;

	initialized = false;
	numArms = 0;
	enabledArms = 0;
	for( i = 0; i < MAX_ARMS; i++ )
	{
		handJoints[i] = INVALID_JOINT;
		elbowJoints[i] = INVALID_JOINT;
		shoulderJoints[i] = INVALID_JOINT;
		dirJoints[i] = INVALID_JOINT;
		shoulderForward[i].Zero();
		elbowForward[i].Zero();
		upperArmLength[i] = 0.0f;
		lowerArmLength[i] = 0.0f;
		upperArmToShoulderJoint[i].Identity();
		lowerArmToElbowJoint[i].Identity();
	}
}

/*
================
idIK_Reach::~idIK_Reach
================
*/
idIK_Reach::~idIK_Reach()
{
}

/*
================
idIK_Reach::Save
================
*/
void idIK_Reach::Save( idSaveGame* savefile ) const
{
	int i;
	idIK::Save( savefile );

	savefile->WriteInt( numArms );
	savefile->WriteInt( enabledArms );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->WriteInt( handJoints[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->WriteInt( elbowJoints[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->WriteInt( shoulderJoints[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->WriteInt( dirJoints[i] );

	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->WriteVec3( shoulderForward[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->WriteVec3( elbowForward[i] );

	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->WriteFloat( upperArmLength[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->WriteFloat( lowerArmLength[i] );

	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->WriteMat3( upperArmToShoulderJoint[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->WriteMat3( lowerArmToElbowJoint[i] );
}

/*
================
idIK_Reach::Restore
================
*/
void idIK_Reach::Restore( idRestoreGame* savefile )
{
	int i;
	idIK::Restore( savefile );

	savefile->ReadInt( numArms );
	savefile->ReadInt( enabledArms );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->ReadInt( ( int& )handJoints[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->ReadInt( ( int& )elbowJoints[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->ReadInt( ( int& )shoulderJoints[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->ReadInt( ( int& )dirJoints[i] );

	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->ReadVec3( shoulderForward[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->ReadVec3( elbowForward[i] );

	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->ReadFloat( upperArmLength[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->ReadFloat( lowerArmLength[i] );

	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->ReadMat3( upperArmToShoulderJoint[i] );
	for( i = 0; i <  MAX_ARMS; i++ )
		savefile->ReadMat3( lowerArmToElbowJoint[i] );
}

/*
================
idIK_Reach::Init
================
*/
bool idIK_Reach::Init( idEntity* self, const char* anim, const idVec3& modelOffset )
{
	int i;
	const char* jointName;
	idTraceModel trm;
	idVec3 dir, handOrigin, elbowOrigin, shoulderOrigin, dirOrigin;
	idMat3 axis, handAxis, elbowAxis, shoulderAxis;

	elbowAxisMotion[0] = mat3_identity;
	elbowAxisMotion[1] = mat3_identity;

	if( !self )
	{
		return false;
	}

	jointName = self->spawnArgs.GetString("mesh" );
	if ( strstr( jointName, "spplayer" ) ) common->Printf( "IK Player found" );

	numArms = Min( self->spawnArgs.GetInt( "ik_numArms", "0" ), MAX_ARMS );
	if( numArms == 0 )
	{
		return true;
	}
	else common->Printf( "idIK_Reach::Init IK found %d Arms\n",numArms );

	if( !idIK::Init( self, anim, modelOffset ) )
	{
		return false;
	}

	int numJoints = animator->NumJoints();
	idJointMat* joints = ( idJointMat* )_alloca16( numJoints * sizeof( joints[0] ) );

	// create the animation frame used to setup the IK
	gameEdit->ANIM_CreateAnimFrame( animator->ModelHandle(), animator->GetAnim( modifiedAnim )->MD5Anim( 0 ), numJoints, joints, 1, animator->ModelDef()->GetVisualOffset() + modelOffset, animator->RemoveOrigin() );

	enabledArms = 0;

	// get all the joints
	for( i = 0; i < numArms; i++ )
	{

		jointName = self->spawnArgs.GetString( va( "ik_hand%d", i + 1 ) );
		handJoints[i] = animator->GetJointHandle( jointName );
		if( handJoints[i] == INVALID_JOINT )
		{
			jointName = i ? "Lhand1" : "Rhand1";
			handJoints[i] = animator->GetJointHandle( jointName );
			if( handJoints[i] == INVALID_JOINT )
			{
				gameLocal.Error( "idIK_Reach::Init: invalid hand joint '%s'", jointName );
			}
		}

		jointName = self->spawnArgs.GetString( va( "ik_elbow%d", i + 1 ) );
		elbowJoints[i] = animator->GetJointHandle( jointName );
		if( elbowJoints[i] == INVALID_JOINT )
		{
			gameLocal.Error( "idIK_Reach::Init: invalid elbow joint '%s'\n", jointName );
		}

		jointName = self->spawnArgs.GetString( va( "ik_shoulder%d", i + 1 ) );
		shoulderJoints[i] = animator->GetJointHandle( jointName );
		if( shoulderJoints[i] == INVALID_JOINT )
		{
			gameLocal.Error( "idIK_Reach::Init: invalid shoulder joint '%s'\n", jointName );
		}

		jointName = self->spawnArgs.GetString( va( "ik_wrist%d", i + 1 ) );
		wristJoints[i] = animator->GetJointHandle( jointName );
		if ( wristJoints[i] == INVALID_JOINT )
		{
			gameLocal.Error( "idIK_Reach::Init: invalid wrist joint '%s'\n", jointName );
		}

		jointName = self->spawnArgs.GetString( va( "ik_elbowDir%d", i + 1 ) );
		dirJoints[i] = animator->GetJointHandle( jointName );

		enabledArms |= 1 << i;
	}

	// get the arm bone lengths and rotation matrices
	for( i = 0; i < numArms; i++ )
	{

		handAxis = joints[ handJoints[ i ] ].ToMat3();
		handOrigin = joints[ handJoints[ i ] ].ToVec3();

		elbowAxis = joints[ elbowJoints[ i ] ].ToMat3();
		elbowOrigin = joints[ elbowJoints[ i ] ].ToVec3();

		shoulderAxis = joints[ shoulderJoints[ i ] ].ToMat3();
		shoulderOrigin = joints[ shoulderJoints[ i ] ].ToVec3();

		// get the IK direction
		if( 0 && dirJoints[i] != INVALID_JOINT )
		{
			dirOrigin = joints[ dirJoints[ i ] ].ToVec3();
			dir = dirOrigin - elbowOrigin;
			common->Printf( "Elbow %d dir found!\n", i );
		}
		else
		{
			dir.Set( -.5f, -.2f * ( -i )  , 0.0f );
		}

		shoulderForward[i] = dir * shoulderAxis.Transpose();
		elbowForward[i] = dir * elbowAxis.Transpose();

		// conversion from upper arm bone axis to should joint axis
		upperArmLength[i] = GetBoneAxis( shoulderOrigin, elbowOrigin, dir, axis );
		upperArmToShoulderJoint[i] = shoulderAxis * axis.Transpose();

		// conversion from lower arm bone axis to elbow joint axis
		lowerArmLength[i] = GetBoneAxis( elbowOrigin, handOrigin, dir, axis );
		lowerArmToElbowJoint[i] = elbowAxis * axis.Transpose();
	}

	initialized = true;

	return true;
}

/*
================
idIK_Reach::Evaluate
================
*/
void idIK_Reach::Evaluate()
{

	int i;
	idVec3 modelOrigin, shoulderOrigin, elbowOrigin, handOrigin, shoulderDir, elbowDir, handDir;
	idMat3 modelAxis, axis;
	idMat3 shoulderAxis[MAX_ARMS], elbowAxis[MAX_ARMS];
	idVec3 elbowPos[MAX_ARMS], handPos[MAX_ARMS], shoulderPos[MAX_ARMS]; // Koz
	trace_t trace;


	modelOrigin = self->GetRenderEntity()->origin;
	modelAxis = self->GetRenderEntity()->axis;


	idMat3 wristMod[2] = { mat3_identity, mat3_identity };
	// solve IK
	for ( i = 0; i < numArms; i++ )
	{
		// get the position of the shoulder in world space
		animator->GetJointTransform( shoulderJoints[i], gameLocal.time, shoulderOrigin, axis );
		shoulderOrigin = modelOrigin + shoulderOrigin * modelAxis;
		shoulderDir = shoulderForward[i] * axis * modelAxis;

		// get the position of the hand in world space
		animator->GetJointTransform( handJoints[i], gameLocal.time, handOrigin, axis );
		handOrigin = modelOrigin + handOrigin * modelAxis;
		idVec3 localHandDir;
		if( i == 1 )
			localHandDir = shoulderForward[1] * axis;
		else
			localHandDir = idVec3( 1.0f, 1.0f, -0.4f ) * axis; // Carl HACK! Todo! Why is it like this???
//			localHandDir = idVec3( rhx.GetFloat(), rhy.GetFloat(), rhz.GetFloat() ) * axis;
		localHandDir.Normalize();
		handDir = localHandDir * modelAxis;

		shoulderPos[i] = shoulderOrigin - modelOrigin;
		shoulderPos[i] = shoulderPos[i] * modelAxis.Transpose();
		handPos[i] = handOrigin - modelOrigin;
		handPos[i] = handPos[i] * modelAxis.Transpose();

		// if the hand is pointing across the body, the elbow can bend that way, otherwise it can't.
		// (x = forward, y = left, z = up)
		bool elbowCanMatchHand = ( i == 1 ) ? localHandDir.y < 0 : localHandDir.y > 0 ;
		// We also need to match our hand if our hand is reaching over our shoulder
		elbowCanMatchHand = elbowCanMatchHand || ( handPos[i].z >= shoulderPos[i].z && localHandDir.x < -0.4f );

		// get the IK bend direction
		animator->GetJointTransform( elbowJoints[i], gameLocal.time, elbowOrigin, axis );
		elbowDir = elbowForward[i] * axis * modelAxis;

		if( elbowCanMatchHand )
		{
			// Use 80% hand dir, 20% original elbow dir
			elbowDir.SLerp( elbowDir, -handDir, 0.8f );
		}

		// solve IK and calculate elbow position
		SolveTwoArmBones( shoulderOrigin, handOrigin, elbowDir, upperArmLength[i] , lowerArmLength[i] , elbowOrigin );

		elbowPos[i] = elbowOrigin - modelOrigin; // Koz actually move the elbow joint
		elbowPos[i] = elbowPos[i] * modelAxis.Transpose();

		// uncomment these if SolveTwoArmBones changes shoulderOrigin or handOrigin
		// handPos[i] = handOrigin - modelOrigin;
		// handPos[i] = handPos[i] * modelAxis.Transpose();

		// shoulderPos[i] = shoulderOrigin - modelOrigin;
		// shoulderPos[i] = shoulderPos[i] * modelAxis.Transpose();

		// get the axis for the shoulder joint
		GetBoneAxis( shoulderOrigin, elbowOrigin, shoulderDir, axis );

		if( ik_debug.GetBool() )
		{
			if( elbowCanMatchHand )
				gameRenderWorld->DebugArrow( colorMagenta, handOrigin, handOrigin + handDir * 8.0f, 1 );
			else
				gameRenderWorld->DebugArrow( colorRed, handOrigin, handOrigin + handDir * 8.0f, 1 );
			gameRenderWorld->DrawText( va( "%.2f, %.2f, %.2f", localHandDir.x, localHandDir.y, localHandDir.z ), handOrigin + handDir * 11.0f, 0.06f, colorWhite, modelAxis );
			gameRenderWorld->DrawText( va( "%.2f, %.2f, %.2f", handPos[i].x, handPos[i].y, handPos[i].z ), handOrigin + handDir * 9.0f, 0.04f, colorWhite, modelAxis );
			gameRenderWorld->DrawText( va( "%.2f, %.2f, %.2f", shoulderPos[i].x, shoulderPos[i].y, shoulderPos[i].z ), shoulderOrigin + handDir * 11.0f, 0.04f, colorWhite, modelAxis );
			gameRenderWorld->DebugLine( colorCyan, shoulderOrigin, elbowOrigin );
			gameRenderWorld->DebugLine( colorRed, elbowOrigin, handOrigin );
			gameRenderWorld->DebugArrow( colorYellow, elbowOrigin, elbowOrigin + elbowDir * 8.0f, 1 );
			gameRenderWorld->DebugLine( colorGreen, elbowOrigin, elbowOrigin + shoulderDir * 2.0f );

		}

		// roll the shoulder a bit
		static idMat3 roll;
		static idMat3 trsp;


		if ( i == 0 )
		{
			roll = idAngles( 0.0f, 0.0f, (-commonVr->handRoll[i] * .1 ) - 50 ).ToMat3();
		}
		else
		{
			roll = idAngles( 0.0f, 0.0f, (-commonVr->handRoll[i] * .1) + 50 ).ToMat3();
		}

		trsp = roll * axis;
		shoulderAxis[i] = upperArmToShoulderJoint[i] * (trsp * modelAxis.Transpose());

		// get the axis for the elbow joint
		GetBoneAxis( elbowOrigin, handOrigin, elbowDir, axis, false );
		elbowAxis[i] = lowerArmToElbowJoint[i] * ( axis * modelAxis.Transpose() );

		/*
		gameRenderWorld->DebugLine(colorPurple, handOrigin, handOrigin + 3 * (elbowAxis[i] * modelAxis)[0], 20);
		gameRenderWorld->DebugLine(colorBrown, handOrigin, handOrigin - 3 * (elbowAxis[i] * modelAxis)[0], 20);
		gameRenderWorld->DebugLine(colorWhite, handOrigin, handOrigin + 3 * (elbowAxis[i] * modelAxis)[1], 20);
		gameRenderWorld->DebugLine(colorPink, handOrigin, handOrigin - 3 * (elbowAxis[i] * modelAxis)[1], 20);
		gameRenderWorld->DebugLine(colorOrange, handOrigin, handOrigin + 3 * (elbowAxis[i] * modelAxis)[2], 20);
		gameRenderWorld->DebugLine(colorCyan, handOrigin, handOrigin - 3 * (elbowAxis[i] * modelAxis)[2], 20);
		*/

		// vars to check if elbow axis needs to be flipped.
		idMat3 diff = mat3_identity;
		idVec3 shoulderToHand;
		idVec3 shoulderToGround;
		const idVec3 gravityDir = idVec3(0, 0, -1);
		//const idMat3 rollf[2] = { idAngles(180.0f, 1.0f, 4.0f).ToMat3(), idAngles(180.0f, 0.0f, -7.0f).ToMat3() };
		const idMat3 rollf[2] = { idAngles( 180.0f, 1.0f, 4.0f ).ToMat3(), idAngles( 180.0f, 1.0f, 4.0f ).ToMat3() };
		float crossDiff;
		idVec3 shoulderPos;
		idVec3 shoulderHandGroundCross;


		// calc roll for wrist joint based on motion controller orientation
		// this is really a bunch of hacked crap, but it looks a little better than before
		idQuat handQ;
		idMat3 handOr;
		idVec3 handP;
		idVec3 gr;
		idVec3 ma;
		idVec3 n;
		idVec3 v;
		idVec3 np;
		idVec3 br;
		float dist;
		float dot;
		float angle1;
		float angle2;

		animator->GetJointTransform(shoulderJoints[i], gameLocal.time, shoulderPos, diff);

		shoulderToHand = handPos[i] - shoulderPos;
		shoulderToGround = shoulderPos - (shoulderPos + (gravityDir * 20));
		shoulderToHand.Normalize();
		shoulderToGround.Normalize();
		shoulderHandGroundCross = shoulderToHand.Cross(shoulderToGround);
		crossDiff = shoulderHandGroundCross * elbowAxis[i][2];

		wristMod[i] = idAngles(0.0f, 0.0f, 0.0f).ToMat3();

		if (i == 1) // this is the left hand
		{
			if ( crossDiff > 0 ) // elbow joint has flipped so correct it
			{
				elbowAxis[i] = rollf[i] * elbowAxis[i];
			}

			if (commonVr->VR_USE_MOTION_CONTROLS)
			{
				commonVr->MotionControlGetHand( i, handP, handQ ); // get the rotation of the hand controller


				handOr = handQ.ToMat3() * idAngles( 0.0f, -commonVr->bodyYawOffset, 0.0f ).ToMat3();

				gr = handOr[2]; // axis of the hand controller used for 'roll'
				ma = -elbowAxis[i][2]; //axis of elbow joint ( forearm ) used to calculate roll difference from gr.

				//project new point np from point 'v' onto the plane defined by handOr[2] and normal elbowAxis[i][1]
				n = elbowAxis[i][1];
				v = gr;
				dist = v * n;
				np = v - dist * n;
				np.Normalize();

				// this is the angle of roll.
				dot = np * ma;
				angle1 = RAD2DEG(acosf(dot));

				//check to see if angle should be positive or neg, by checking if angle is >90 deg from axis orthogonal to -elbowAxis[i][2]
				br = elbowAxis[i][0];
				dot = np * br;
				angle2 = RAD2DEG(acosf(dot));
				if (angle2 > 90) angle1 = -angle1;

				// fix the roll a little to better align with the elbow joint.

				angle1 = angle1 - 30.0f;
				angle1 = idMath::ClampFloat( -66.0f, 66.0f, angle1 );

				angle2 = angle1 * 0.3f; // roll the elbow 1/3 of the total roll.
				angle1 -= angle2;

				wristMod[i] = idAngles(angle1, 0.0f, 0.0f).ToMat3();

				// roll the elbow a little;
				idMat3 trsp = idAngles( -angle2, 0.0f, 0.0f).ToMat3();
				elbowAxis[i] = trsp * elbowAxis[i];

			}
		}
		else // i == 0 - this is the right hand.
		{
			if (crossDiff < 0 ) //elbow joint has flipped so correct it
			{
				elbowAxis[i] = rollf[i] * elbowAxis[i];
			}

			if (commonVr->VR_USE_MOTION_CONTROLS)
			{
				commonVr->MotionControlGetHand( i, handP, handQ ); // get the rotation of the hand controller

				handOr = handQ.ToMat3() * idAngles( 0.0f, -commonVr->bodyYawOffset, 0.0f ).ToMat3();;

				gr = handOr[2]; // axis of the hand controller used for 'roll'
				ma = -elbowAxis[i][2]; //axis of elbow joint ( forearm ) used to calculate roll difference from gr.

				//project new point np from point 'v' onto the plane defined by handOr[2] and normal elbowAxis[i][1]
				n = elbowAxis[i][1];
				v = gr;
				dist = v * n;
				np = v - dist * n;
				np.Normalize();

				// this is the angle of roll.
				dot = np * ma;
				angle1 = RAD2DEG(acosf(dot));

				//check to see if angle should be positive or neg, by checking if angle is >90 deg from axis orthogonal to -elbowAxis[i][2]
				br = -elbowAxis[i][0];
				dot = np * br;
				angle2 = RAD2DEG(acosf(dot));
				if ( angle2 > 90 ) angle1 = -angle1;

				// fix the roll a little to better align with the elbow joint.
				angle1 = angle1 - 20.0f;
				angle1 = idMath::ClampFloat( -66.0f, 66.0f, angle1 );

				angle2 = angle1 * 0.3f;  //roll the elbow 1/3 of the total roll.
				angle1 -= angle2;

				wristMod[i] = idAngles(-angle1, 0.0f, 0.0f).ToMat3();

				// roll the elbow a little;
				idMat3 trsp = idAngles( angle2, 0.0f, 0.0f).ToMat3();
				elbowAxis[i] = trsp * elbowAxis[i];

			}
		}
	}

	for ( i = 0; i < numArms; i++ )
	{
		animator->SetJointAxis( elbowJoints[i], JOINTMOD_WORLD_OVERRIDE, elbowAxis[i] );
		animator->SetJointAxis( shoulderJoints[i], JOINTMOD_WORLD_OVERRIDE, shoulderAxis[i] );
		animator->SetJointPos( elbowJoints[i], JOINTMOD_WORLD_OVERRIDE, elbowPos[i] );// Koz
		animator->SetJointPos( handJoints[i], JOINTMOD_WORLD_OVERRIDE, handPos[i] );// Koz
		animator->SetJointAxis(wristJoints[i], JOINTMOD_LOCAL, wristMod[i]);
	}

	ik_activate = true;
}

/*
================
idIK_Reach::ClearJointMods
================
*/
void idIK_Reach::ClearJointMods()
{
	int i;

	if( !self || !ik_activate )
	{
		return;
	}

	for( i = 0; i < numArms; i++ )
	{
		animator->SetJointAxis( shoulderJoints[i], JOINTMOD_NONE, mat3_identity );
		animator->SetJointAxis( elbowJoints[i], JOINTMOD_NONE, mat3_identity );
		animator->SetJointAxis( handJoints[i], JOINTMOD_NONE, mat3_identity );
	}

	ik_activate = false;
}