//===========================================================================//
// File:	EyePointManager.hpp                                                 
// Project: MechWarrior 4
// Contents: 
//			
//---------------------------------------------------------------------------//
//   Date    Who  Modification                                               //
// --------  ---  ---------------------------------------------------------- //
// 03/20/00  JSE  Initial coding, 
//---------------------------------------------------------------------------//
// Copyright (C) 2000, Microsoft Corp.
// All Rights reserved worldwide
// This unpublished sourcecode is PROPRIETARY and CONFIDENTIAL
//===========================================================================//

#include "MW4Headers.hpp"

#include "EyePointManager.hpp"
#include "mech.hpp"


//#############################################################################
//###########################    EyePointManager    ###############################
//#############################################################################

EyePointManager::ClassData*
	EyePointManager::DefaultData = NULL;

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	EyePointManager::InitializeClass()
{
	Verify(!DefaultData);
	DefaultData =
		new ClassData(
			EyePointManagerClassID,
			"MechWarrior4::EyePointManager",
			Receiver::DefaultData,
			0,
			NULL
		);
	Check_Object(DefaultData);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	EyePointManager::TerminateClass()
{
	Check_Object(DefaultData);
	delete DefaultData;
	DefaultData = NULL;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
EyePointManager::EyePointManager(Mech *parent) :
	Adept::Receiver(DefaultData)
{
	m_parentMech = parent;

	
	initilized = false;
	m_eyeJoint = NULL;
	m_jointWorld = NULL;
	m_eyeParent = NULL;

	m_springOffset = LinearMatrix4D::Identity;
	m_eyeDirection = LinearMatrix4D::Identity;
	m_originalEyeToParent = LinearMatrix4D::Identity;
	m_originalEyeToTorso = LinearMatrix4D::Identity;
	m_originalTorsoToRoot = LinearMatrix4D::Identity;
	m_torsoRotation = LinearMatrix4D::Identity;
	eyeState = StabalizedMode;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

EyePointManager::~EyePointManager()
{
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

void EyePointManager::Stabalize(Stuff::Scalar time)
{
	if (eyeState == ToFollowMode || eyeState == FollowMode)
	{
		eyeState = ToStabalizedMode;
		transitionTimer = 0.0f;
		transitionTime = time;
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

void EyePointManager::Follow(Stuff::Scalar time)
{
	if (eyeState == ToStabalizedMode || eyeState == StabalizedMode)
	{
		eyeState = ToFollowMode;
		transitionTimer = 0.0f;
		transitionTime = time;
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

void EyePointManager::PostCollisionExecute(Stuff::Scalar time_slice)
{
	POSTCOLLISION_LOGIC("EyePoint Manager");


	if (!initilized)
	{
		initilized = true;

		m_jointWorld = m_parentMech->FindChildMover("joint_WORLD");
		Check_Object(m_jointWorld);
		m_eyeJoint = m_parentMech->FindChildMover("site_eyepoint");
		Check_Object(m_eyeJoint);
		MWMover *torso_joint = m_parentMech->FindChildMover("joint_torso");
		Check_Object(torso_joint);
		m_eyeParent = Cast_Object(MWMover *, m_eyeJoint->GetParentEntity());
		
		m_springOffset = LinearMatrix4D::Identity;
		m_eyeDirection = LinearMatrix4D::Identity;
		
		m_originalEyeToParent = m_eyeJoint->GetLocalToParent();

		LinearMatrix4D world_to_root;
		world_to_root.Invert(m_jointWorld->GetLocalToWorld());
		
		LinearMatrix4D world_to_torso;
		world_to_torso.Invert(torso_joint->GetLocalToWorld());
		
		m_originalEyeToTorso.Multiply(m_eyeJoint->GetLocalToWorld(), world_to_torso);
		m_originalTorsoToRoot.Multiply(torso_joint->GetLocalToWorld(), world_to_root);
	}
	

	if (!m_parentMech->usingEyeSpring)
	{
		POSTCOLLISION_LOGIC("EyePoint Manager::External");
			
		LinearMatrix4D eye_to_parent;
		eye_to_parent.Multiply(m_eyeDirection, m_originalEyeToParent);

		m_eyeJoint->SetNewLocalToParent(eye_to_parent);
		m_eyeJoint->SyncMatrices(true);
		return;
	}


	switch(eyeState)
	{
	case StabalizedMode:
		{
			POSTCOLLISION_LOGIC("EyePoint Manager::Stabalized Mode");
				
			LinearMatrix4D eye_to_parent;
			CalculateStabalizedEye(eye_to_parent);
			m_eyeJoint->SetNewLocalToParent(eye_to_parent);
			m_eyeJoint->SyncMatrices(true);

		}
		break;
	
	case FollowMode:
		{
			POSTCOLLISION_LOGIC("EyePoint Manager::Follow Mode");

			LinearMatrix4D eye_to_parent;
			CalculateFollowEye(eye_to_parent);
			m_eyeJoint->SetNewLocalToParent(eye_to_parent);
			m_eyeJoint->SyncMatrices(true);

		}
		break;
	
	case ToFollowMode:
		{
			POSTCOLLISION_LOGIC("EyePoint Manager::To Follow Mode");

			transitionTimer += time_slice;
			if (transitionTimer > transitionTime)
			{
				eyeState = FollowMode;
				LinearMatrix4D eye_to_parent;
				CalculateFollowEye(eye_to_parent);
				m_eyeJoint->SetNewLocalToParent(eye_to_parent);
				m_eyeJoint->SyncMatrices(true);
	
				time_slice = transitionTime;
			}
			else
			{
				LinearMatrix4D stab_eye_to_parent;
				CalculateStabalizedEye(stab_eye_to_parent);
				
				LinearMatrix4D fol_eye_to_parent;
				CalculateFollowEye(fol_eye_to_parent);
				

				Scalar lerp = transitionTimer/transitionTime;

				UnitQuaternion start_rot;
				start_rot = stab_eye_to_parent;
				UnitQuaternion stop_rot;
				stop_rot = fol_eye_to_parent;
				UnitQuaternion lerp_rot;
				lerp_rot.Lerp(start_rot, stop_rot, lerp);

				Point3D start_pos(stab_eye_to_parent);
				Point3D end_pos(fol_eye_to_parent);
				Point3D lerp_pos;
				lerp_pos.Lerp(start_pos, end_pos, lerp);
				
				LinearMatrix4D eye_to_parent;
				eye_to_parent.BuildRotation(lerp_rot);
				eye_to_parent.BuildTranslation(lerp_pos);
				m_eyeJoint->SetNewLocalToParent(eye_to_parent);
				m_eyeJoint->SyncMatrices(true);
			}
		}
		break;

	case ToStabalizedMode:
		{
			POSTCOLLISION_LOGIC("EyePoint Manager::To Stabalized Mode");

			transitionTimer += time_slice;
			if (transitionTimer > transitionTime)
			{
				eyeState = StabalizedMode;
				LinearMatrix4D eye_to_parent;
				CalculateStabalizedEye(eye_to_parent);
				m_eyeJoint->SetNewLocalToParent(eye_to_parent);
				m_eyeJoint->SyncMatrices(true);
	
				time_slice = transitionTime;
			}
			else
			{
				LinearMatrix4D stab_eye_to_parent;
				CalculateStabalizedEye(stab_eye_to_parent);
				
				LinearMatrix4D fol_eye_to_parent;
				CalculateFollowEye(fol_eye_to_parent);
				

				Scalar lerp = transitionTimer/transitionTime;


				UnitQuaternion start_rot;
				start_rot = fol_eye_to_parent;
				UnitQuaternion stop_rot;
				stop_rot = stab_eye_to_parent;
				UnitQuaternion lerp_rot;
				lerp_rot.Lerp(start_rot, stop_rot, lerp);

				Point3D start_pos(fol_eye_to_parent);
				Point3D end_pos(stab_eye_to_parent);
				Point3D lerp_pos;
				lerp_pos.Lerp(start_pos, end_pos, lerp);
				
				LinearMatrix4D eye_to_parent;
				eye_to_parent.BuildRotation(lerp_rot);
				eye_to_parent.BuildTranslation(lerp_pos);
				m_eyeJoint->SetNewLocalToParent(eye_to_parent);
				m_eyeJoint->SyncMatrices(true);
			}
		}
		break;
	}

	
	
	

}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

void EyePointManager::CalculateStabalizedEye(LinearMatrix4D &eye_to_parent)
{
	// build the matrix for the eye to root matrix
	POSTCOLLISION_LOGIC("EyePoint Manager::Calc Stabalized");

	LinearMatrix4D eye_to_root;
	LinearMatrix4D temp1;
	LinearMatrix4D temp2;


	// move to the torso and rotate
	temp1.Multiply(m_torsoRotation, m_originalTorsoToRoot);

	// move up the chain to the eye
	temp2.Multiply(m_originalEyeToTorso, temp1);

	// put in the spring
	temp1.Multiply(m_springOffset, temp2);

	// put in the eye direction
	eye_to_root.Multiply(m_eyeDirection, temp1);


	// move the eye into world space
	LinearMatrix4D eye_to_world;
	eye_to_world.Multiply(eye_to_root, m_jointWorld->GetLocalToWorld());

	// make the matrix to move the world point into parent space
	LinearMatrix4D world_to_parent;
	world_to_parent.Invert(m_eyeParent->GetLocalToWorld());

	// move the world eye point into eye space
	eye_to_parent.Multiply(eye_to_world, world_to_parent);
}


//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

void EyePointManager::CalculateFollowEye(LinearMatrix4D &eye_to_parent)
{

	POSTCOLLISION_LOGIC("EyePoint Manager::Calc Follow");

	LinearMatrix4D temp1;
	LinearMatrix4D temp2;

	//start with the generic position
	temp1 = m_originalEyeToParent;

	// put in the spring
	temp2.Multiply(m_springOffset, temp1);

	// put in the eye direction
	eye_to_parent.Multiply(m_eyeDirection, temp2);

}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	EyePointManager::Reuse()
{
	Check_Object(this);


	m_springOffset = LinearMatrix4D::Identity;
	m_eyeDirection = LinearMatrix4D::Identity;
	m_torsoRotation = LinearMatrix4D::Identity;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	EyePointManager::TestInstance()
{
	Verify(IsDerivedFrom(DefaultData));
}