/** 
 * @file llpose.cpp
 * @brief Implementation of LLPose class.
 *
 * Copyright (c) 2001-$CurrentYear$, Linden Research, Inc.
 * $License$
 */

//-----------------------------------------------------------------------------
// Header Files
//-----------------------------------------------------------------------------
#include "linden_common.h"

#include "llpose.h"

#include "llmotion.h"
#include "llmath.h"

//-----------------------------------------------------------------------------
// Static
//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------
// LLPose
//-----------------------------------------------------------------------------
LLPose::~LLPose()
{
}

//-----------------------------------------------------------------------------
// getFirstJointState()
//-----------------------------------------------------------------------------
LLJointState *LLPose::getFirstJointState()
{
	mListIter = mJointMap.begin();
	if (mListIter == mJointMap.end())
	{
		return NULL;
	}
	else
	{
		return mListIter->second;
	}
}

//-----------------------------------------------------------------------------
// getNextJointState()
//-----------------------------------------------------------------------------
LLJointState *LLPose::getNextJointState()
{
	mListIter++;
	if (mListIter == mJointMap.end())
	{
		return NULL;
	}
	else
	{
		return mListIter->second;
	}
}

//-----------------------------------------------------------------------------
// addJointState()
//-----------------------------------------------------------------------------
BOOL LLPose::addJointState(LLJointState *jointState)
{
	if (mJointMap.find(jointState->getJoint()->getName()) == mJointMap.end())
	{
		mJointMap[jointState->getJoint()->getName()] = jointState;
	}
	return TRUE;
}

//-----------------------------------------------------------------------------
// removeJointState()
//-----------------------------------------------------------------------------
BOOL LLPose::removeJointState(LLJointState *jointState)
{
	mJointMap.erase(jointState->getJoint()->getName());
	return TRUE;
}

//-----------------------------------------------------------------------------
// removeAllJointStates()
//-----------------------------------------------------------------------------
BOOL LLPose::removeAllJointStates()
{
	mJointMap.clear();
	return TRUE;
}

//-----------------------------------------------------------------------------
// findJointState()
//-----------------------------------------------------------------------------
LLJointState* LLPose::findJointState(LLJoint *joint)
{
	joint_map_iterator iter = mJointMap.find(joint->getName());

	if (iter == mJointMap.end())
	{
		return NULL;
	}
	else
	{
		return iter->second;
	}
}

//-----------------------------------------------------------------------------
// findJointState()
//-----------------------------------------------------------------------------
LLJointState* LLPose::findJointState(const std::string &name)
{
	joint_map_iterator iter = mJointMap.find(name);

	if (iter == mJointMap.end())
	{
		return NULL;
	}
	else
	{
		return iter->second;
	}
}

//-----------------------------------------------------------------------------
// setWeight()
//-----------------------------------------------------------------------------
void LLPose::setWeight(F32 weight)
{
	joint_map_iterator iter;
	for(iter = mJointMap.begin(); 
		iter != mJointMap.end();
		++iter)
	{
		iter->second->setWeight(weight);
	}
	mWeight = weight;
}

//-----------------------------------------------------------------------------
// getWeight()
//-----------------------------------------------------------------------------
F32 LLPose::getWeight() const
{
	return mWeight;
}

//-----------------------------------------------------------------------------
// getNumJointStates()
//-----------------------------------------------------------------------------
S32 LLPose::getNumJointStates() const
{
	return (S32)mJointMap.size();
}

//-----------------------------------------------------------------------------
// LLJointStateBlender
//-----------------------------------------------------------------------------

LLJointStateBlender::LLJointStateBlender()
{
	for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
	{
		mJointStates[i] = NULL;
		mPriorities[i] = S32_MIN;
	}
}

LLJointStateBlender::~LLJointStateBlender()
{
	
}

//-----------------------------------------------------------------------------
// addJointState()
//-----------------------------------------------------------------------------
BOOL LLJointStateBlender::addJointState(LLJointState *joint_state, S32 priority, BOOL additive_blend)
{
	llassert(joint_state);

	if (!joint_state->getJoint())
		// this joint state doesn't point to an actual joint, so we don't care about applying it
		return FALSE;

	for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
	{
		if (NULL == mJointStates[i])
		{
			mJointStates[i] = joint_state;
			mPriorities[i] = priority;
			mAdditiveBlends[i] = additive_blend;
			return TRUE;
		} 
		else if (priority > mPriorities[i])
		{
			// we're at a higher priority than the current joint state in this slot
			// so shift everyone over
			// previous joint states (newer motions) with same priority should stay in place
			for (S32 j = JSB_NUM_JOINT_STATES - 1; j > i; j--)
			{
				mJointStates[j] = mJointStates[j - 1];
				mPriorities[j] = mPriorities[j - 1];
				mAdditiveBlends[j] = mAdditiveBlends[j - 1];
			}
			// now store ourselves in this slot
			mJointStates[i] = joint_state;
			mPriorities[i] = priority;
			mAdditiveBlends[i] = additive_blend;
			return TRUE;
		}
	}

	return FALSE;
}

//-----------------------------------------------------------------------------
// blendJointStates()
//-----------------------------------------------------------------------------
void LLJointStateBlender::blendJointStates(BOOL apply_now)
{
	// we need at least one joint to blend
	// if there is one, it will be in slot zero according to insertion logic
	// instead of resetting joint state to default, just leave it unchanged from last frame
	if (NULL == mJointStates[0])
	{
		return;
	}

	LLJoint* target_joint = apply_now ? mJointStates[0]->getJoint() : &mJointCache;

	const S32 POS_WEIGHT = 0;
	const S32 ROT_WEIGHT = 1;
	const S32 SCALE_WEIGHT = 2;

	F32				sum_weights[3];
	U32				sum_usage = 0;

	LLVector3		blended_pos = target_joint->getPosition();
	LLQuaternion	blended_rot = target_joint->getRotation();
	LLVector3		blended_scale = target_joint->getScale();

	LLVector3		added_pos;
	LLQuaternion	added_rot;
	LLVector3		added_scale;

	//S32				joint_state_index;

	sum_weights[POS_WEIGHT] = 0.f;
	sum_weights[ROT_WEIGHT] = 0.f;
	sum_weights[SCALE_WEIGHT] = 0.f;

	for(S32 joint_state_index = 0; 
		joint_state_index < JSB_NUM_JOINT_STATES && mJointStates[joint_state_index] != NULL;
		joint_state_index++)
	{
		U32 current_usage = mJointStates[joint_state_index]->getUsage();
		F32 current_weight = mJointStates[joint_state_index]->getWeight();
		LLJointState* jsp = mJointStates[joint_state_index];

		if (current_weight == 0.f)
		{
			continue;
		}

		if (mAdditiveBlends[joint_state_index])
		{
			if(current_usage & LLJointState::POS)
			{
				F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[POS_WEIGHT]);

				// add in pos for this jointstate modulated by weight
				added_pos += jsp->getPosition() * (new_weight_sum - sum_weights[POS_WEIGHT]);
				//sum_weights[POS_WEIGHT] = new_weight_sum;
			}

			// now do scale
			if(current_usage & LLJointState::SCALE)
			{
				F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[SCALE_WEIGHT]);

				// add in scale for this jointstate modulated by weight
				added_scale += jsp->getScale() * (new_weight_sum - sum_weights[SCALE_WEIGHT]);
				//sum_weights[SCALE_WEIGHT] = new_weight_sum;
			}

			if (current_usage & LLJointState::ROT)
			{
				F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[ROT_WEIGHT]);

				// add in rotation for this jointstate modulated by weight
				added_rot = nlerp((new_weight_sum - sum_weights[ROT_WEIGHT]), added_rot, jsp->getRotation()) * added_rot;
				//sum_weights[ROT_WEIGHT] = new_weight_sum;
			}
		}
		else
		{
			// blend two jointstates together
		
			// blend position
			if(current_usage & LLJointState::POS)
			{
				if(sum_usage & LLJointState::POS)
				{
					F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[POS_WEIGHT]);

					// blend positions from both
					blended_pos = lerp(mJointStates[joint_state_index]->getPosition(), blended_pos, sum_weights[POS_WEIGHT] / new_weight_sum);
					sum_weights[POS_WEIGHT] = new_weight_sum;
				} 
				else
				{
					// copy position from current
					blended_pos = mJointStates[joint_state_index]->getPosition();
					sum_weights[POS_WEIGHT] = current_weight;
				}
			}
			
			// now do scale
			if(current_usage & LLJointState::SCALE)
			{
				if(sum_usage & LLJointState::SCALE)
				{
					F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[SCALE_WEIGHT]);

					// blend scales from both
					blended_scale = lerp(mJointStates[joint_state_index]->getScale(), blended_scale, sum_weights[SCALE_WEIGHT] / new_weight_sum);
					sum_weights[SCALE_WEIGHT] = new_weight_sum;
				} 
				else
				{
					// copy scale from current
					blended_scale = mJointStates[joint_state_index]->getScale();
					sum_weights[SCALE_WEIGHT] = current_weight;
				}
			}

			// rotation
			if (current_usage & LLJointState::ROT)
			{
				if(sum_usage & LLJointState::ROT)
				{
					F32 new_weight_sum = llmin(1.f, current_weight + sum_weights[ROT_WEIGHT]);

					// blend rotations from both
					blended_rot = nlerp(sum_weights[ROT_WEIGHT] / new_weight_sum, mJointStates[joint_state_index]->getRotation(), blended_rot);
					sum_weights[ROT_WEIGHT] = new_weight_sum;
				} 
				else
				{
					// copy rotation from current
					blended_rot = mJointStates[joint_state_index]->getRotation();
					sum_weights[ROT_WEIGHT] = current_weight;
				}
			}

			// update resulting usage mask
			sum_usage = sum_usage | current_usage;
		}
	}

	// apply blended transforms
	target_joint->setPosition(blended_pos);
	target_joint->setScale(blended_scale);
	target_joint->setRotation(blended_rot);

	// apply additive transforms
	target_joint->setPosition(target_joint->getPosition() + added_pos);
	target_joint->setScale(target_joint->getScale() + added_scale);
	target_joint->setRotation(added_rot * target_joint->getRotation());

	if (apply_now)
	{
		// now clear joint states
		for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
		{
			mJointStates[i] = NULL;
		}
	}
}

//-----------------------------------------------------------------------------
// interpolate()
//-----------------------------------------------------------------------------
void LLJointStateBlender::interpolate(F32 u)
{
	// only interpolate if we have a joint state
	if (!mJointStates[0])
	{
		return;
	}
	LLJoint* target_joint = mJointStates[0]->getJoint();

	if (!target_joint)
	{
		return;
	}

	target_joint->setPosition(lerp(target_joint->getPosition(), mJointCache.getPosition(), u));
	target_joint->setScale(lerp(target_joint->getScale(), mJointCache.getScale(), u));
	target_joint->setRotation(nlerp(u, target_joint->getRotation(), mJointCache.getRotation()));
}

//-----------------------------------------------------------------------------
// clear()
//-----------------------------------------------------------------------------
void LLJointStateBlender::clear()
{
	// now clear joint states
	for(S32 i = 0; i < JSB_NUM_JOINT_STATES; i++)
	{
		mJointStates[i] = NULL;
	}
}

//-----------------------------------------------------------------------------
// resetCachedJoint()
//-----------------------------------------------------------------------------
void LLJointStateBlender::resetCachedJoint()
{
	if (!mJointStates[0])
	{
		return;
	}
	LLJoint* source_joint = mJointStates[0]->getJoint();
	mJointCache.setPosition(source_joint->getPosition());
	mJointCache.setScale(source_joint->getScale());
	mJointCache.setRotation(source_joint->getRotation());
}

//-----------------------------------------------------------------------------
// LLPoseBlender
//-----------------------------------------------------------------------------

LLPoseBlender::LLPoseBlender()
{
}

LLPoseBlender::~LLPoseBlender()
{
	mJointStateBlenderPool.deleteAllData();
}

//-----------------------------------------------------------------------------
// addMotion()
//-----------------------------------------------------------------------------
BOOL LLPoseBlender::addMotion(LLMotion* motion)
{
	LLPose* pose = motion->getPose();

	for(LLJointState *jsp = pose->getFirstJointState(); jsp; jsp = pose->getNextJointState())
	{
		LLJoint *jointp = jsp->getJoint();
		LLJointStateBlender* joint_blender;
		if (!mJointStateBlenderPool.checkData(jointp))
		{
			// this is the first time we are animating this joint
			// so create new jointblender and add it to our pool
			joint_blender = new LLJointStateBlender();
			mJointStateBlenderPool.addData(jointp, joint_blender);
		} else
		{
			joint_blender = mJointStateBlenderPool.getData(jointp);
		}

		if (jsp->getPriority() == LLJoint::USE_MOTION_PRIORITY)
		{
			joint_blender->addJointState(jsp, motion->getPriority(), motion->getBlendType() == LLMotion::ADDITIVE_BLEND);
		}
		else
		{
			joint_blender->addJointState(jsp, jsp->getPriority(), motion->getBlendType() == LLMotion::ADDITIVE_BLEND);
		}

		// add it to our list of active blenders
		if(!mActiveBlenders.checkData(joint_blender))
		{
			mActiveBlenders.addData(joint_blender);
		}
	}
	return TRUE;
}

//-----------------------------------------------------------------------------
// blendAndApply()
//-----------------------------------------------------------------------------
void LLPoseBlender::blendAndApply()
{
	for (LLJointStateBlender* jsbp = mActiveBlenders.getFirstData(); 
		jsbp;
		jsbp = mActiveBlenders.getNextData())
	{
		jsbp->blendJointStates();
	}

	// we're done now so there are no more active blenders for this frame
	mActiveBlenders.removeAllNodes();
}

//-----------------------------------------------------------------------------
// blendAndCache()
//-----------------------------------------------------------------------------
void LLPoseBlender::blendAndCache(BOOL reset_cached_joints)
{
	for (LLJointStateBlender* jsbp = mActiveBlenders.getFirstData();
		jsbp;
		jsbp = mActiveBlenders.getNextData())
	{
		if (reset_cached_joints)
		{
			jsbp->resetCachedJoint();
		}
		jsbp->blendJointStates(FALSE);
	}
}

//-----------------------------------------------------------------------------
// interpolate()
//-----------------------------------------------------------------------------
void LLPoseBlender::interpolate(F32 u)
{
	for (LLJointStateBlender* jsbp = mActiveBlenders.getFirstData();
		jsbp;
		jsbp = mActiveBlenders.getNextData())
	{
		jsbp->interpolate(u);
	}
}

//-----------------------------------------------------------------------------
// clearBlenders()
//-----------------------------------------------------------------------------
void LLPoseBlender::clearBlenders()
{
	for (LLJointStateBlender* jsbp = mActiveBlenders.getFirstData();
		jsbp;
		jsbp = mActiveBlenders.getNextData())
	{
		jsbp->clear();
	}

	mActiveBlenders.removeAllNodes();
}