//===========================================================================//
// File:	Torso.cpp										
//---------------------------------------------------------------------------//
//   Date    Who  Modification                                               //
// --------  ---  ---------------------------------------------------------- //
// 02/12/99  DPB  Created File
//---------------------------------------------------------------------------//
// Copyright (C) 1998, Fasa Interactive										 //
// All Rights reserved worldwide                                             //
// This unpublished sourcecode is PROPRIETARY and CONFIDENTIAL               //
//===========================================================================//

#include "MW4Headers.hpp"

#include "Torso.hpp"
#include "MWObject.hpp"
#include "Mech.hpp"
#include "EyePointManager.hpp"
#include "SubsystemClassData.hpp"
#include "vehicleinterface.hpp"

//
// These variables control the look down ability (DFA view)
//
#define LOOKDOWN_ANGLE (65.0f)
#define LOOK_MAXMODIFYUP   (-15.0f)
#define LOOK_MAXMODIFYDOWN (15.0f)


#define LOOK_SIDE_TRANSITION_TIME (0.5f)

//#############################################################################
//###############################    Torso    ##############################
//#############################################################################

Torso::ClassData*
	Torso::DefaultData = NULL;

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Torso::InitializeClass()
{
	Verify(!DefaultData);
	DefaultData =
		new ClassData(
			TorsoClassID,
			"MechWarrior4::Torso",
			Subsystem::DefaultData,
			0, NULL,
			(Entity::Factory)Make,
			(Entity::CreateMessage::Factory)CreateMessage::ConstructCreateMessage,
			ExecutionStateEngine::DefaultData,
			(Entity::GameModel::Factory)GameModel::ConstructGameModel,
			NULL,
			(Entity::GameModel::ReadAndVerifier)GameModel::ReadAndVerify,
			(Entity::GameModel::ModelWrite)GameModel::WriteToText,
			(Entity::GameModel::ModelSave)GameModel::SaveGameModel,
			(Subsystem::StreamCreate) CreateStream
		);
	Register_Object(DefaultData);


	DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		TwistSpeed,
		twistSpeed,
		Scalar
	);

	DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		PitchSpeed,
		pitchSpeed,
		Scalar
	);

	DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		TwistRadius,
		twistRadius,
		Scalar
	);

	DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		PitchRadius,
		pitchRadius,
		Scalar
	);

	CUSTOM_DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		TwistJointName,
		twistJointName,
		const char *,
		CharClassID
	);

	CUSTOM_DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		PitchJointName,
		pitchJointName,
		const char *,
		CharClassID
	);


	CUSTOM_DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		LeftArmJointName,
		leftArmJointName,
		const char *,
		CharClassID
	);

	CUSTOM_DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		RightArmJointName,
		rightArmJointName,
		const char *,
		CharClassID
	);

	CUSTOM_DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		EyeJointName,
		eyeJointName,
		const char *,
		CharClassID
	);

	DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		ArmRatioAngle,
		armRatioAngle,
		Scalar
	);

	CUSTOM_DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		CageJointName,
		cageJointName,
		const char *,
		CharClassID
	);

	DIRECT_GAME_MODEL_ATTRIBUTE(
		Torso__GameModel,
		CageRatioAngle,
		cageRatioAngle,
		Scalar
	);
}


//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Torso::TerminateClass()
{
	Unregister_Object(DefaultData);
	delete DefaultData;
	DefaultData = NULL;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Torso*
	Torso::Make(
		CreateMessage *message,
		ReplicatorID *base_id
	)
{
	Check_Object(message);
	gos_PushCurrentHeap(Heap);
	Torso *new_entity =
		new Torso(DefaultData, message, base_id, NULL);
	gos_PopCurrentHeap();
	Check_Object(new_entity);
	return new_entity;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Torso::Torso(
	ClassData *class_data,
	CreateMessage *message,
	ReplicatorID *base_id,
	ElementRenderer::Element *element
):
	Subsystem(class_data, message, base_id, element)
	, surrender_time(0)
{
	Check_Pointer(this);
	Check_Object(message);

	twistJoint = NULL;
	pitchJoint = NULL;
	rotationJoint = NULL;
	leftArmJoint = NULL;
	rightArmJoint = NULL;
	eyeJoint = NULL;
	cageJoint = NULL;
	eyeTransitionEndTime = 0.0f;
	CommonCreation(message);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Torso::CommonCreation(CreateMessage *message)
{
	Check_Object(this);
	Check_Object(message);
	
	yawDemand = 0.0f;	
	pitchDemand = 0.0f;
	leftArmYaw = 0.0f;
	rightArmYaw = 0.0f;
	armPitch = 0.0f;

	eyeYaw = 0.0f;
	eyePitch = 0.0f;

	yawValue = 0.0f;
	pitchValue = 0.0f;
	surrender_time = 0.0f;

	torsoRequest = Point3D (0,0,0);
	followTorsoRequest = false;	

	internalTorso = false;

	lookDirection = LookForwardDirection;
	leftArmTwistState = ForwardTwistState;
	rightArmTwistState = ForwardTwistState;
	armPitchState = ForwardPitchState;
	armTwistTime = 1.0f;
	armPitchTime = 1.0f;
	leftArmTwistTimer = 0.0f;
	rightArmTwistTimer = 0.0f;
	pitchArmTwistTimer = 0.0f;

	heatMultiplier = 1.0f;


	yawValueCorrection = 0.0f;
	oldYawValue = 0.0f;
	pitchValueCorrection = 0.0f;
	oldPitchValue = 0.0f;
	correctionTime = 0.0f;
	correctionCurTime = 0.0f;

	eyeTransitionEndTime = 0.0f;

}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Torso::Respawn(Entity::CreateMessage *message)
{
	Check_Object(this);
	Check_Object(message);

	Subsystem::Respawn(message);
	CreateMessage *torso_message = Cast_Pointer(CreateMessage *, message);
	CommonCreation(torso_message);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Torso::~Torso()
{
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Torso::PreCollisionExecute(Time till)
{
	Check_Object(this);
	Check_Object(executionState);
	PRECOLLISION_LOGIC("Subsystem::Torso");

	if (GetParentVehicle()->GetClassID() == MechClassID)
	{
		Mech *mech = Cast_Object(Mech *, GetParentVehicle());
		EyePointManager *eye_point_manager = mech->GetEyePointManager();
		if (eye_point_manager && !eye_point_manager->initilized)
		{
			return;
		}
	}

	const GameModel *model;
	model = GetGameModel();
	Check_Object(model);

	Scalar time_slice = GetTimeSlice(till);

	yawValue += model->twistSpeed * yawDemand * time_slice * heatMultiplier;
	yawValue = Radian::Normalize(yawValue);

	

  //
  // If we are in a pitch up or down view we need to respond to pitch demand with only the camera so that the user
  // can return to the previous angle when they leave this view. It screws with weapon aiming a bit, but overall should
  // play better this way.
  //

// MSL 5.04 Back View Torso
	if (/*lookDirection == LookBackDirection || */lookDirection == LookDownDirection)
	{
		//
		// For now, to limit abuse the speeds will be clamped to the same speeds as the torso itself with the exception of the
		// snap to the look down/up direction
		//
		lookTypeVerticalAdjust += model->pitchSpeed * pitchDemand * time_slice * heatMultiplier;
		lookTypeVerticalAdjust = Radian::Normalize(lookTypeVerticalAdjust);
		Max_Clamp(lookTypeVerticalAdjust, LOOK_MAXMODIFYDOWN  *Stuff::Radians_Per_Degree);
		Min_Clamp(lookTypeVerticalAdjust, LOOK_MAXMODIFYUP    *Stuff::Radians_Per_Degree);
// MSL 5.04 Back View Torso
//		if (lookDirection == LookBackDirection)
//		{
//			eyePitch = -(pitchValue + armPitch) + lookTypeVerticalAdjust;
//			eyePitch = Radian::Normalize(eyePitch);
//		}
//		else
//		{
			eyePitch = (LOOKDOWN_ANGLE * Stuff::Radians_Per_Degree) - pitchValue  + lookTypeVerticalAdjust;
			eyePitch = Radian::Normalize(eyePitch);
//		}
	}
	else
	{
		if (lookDirection == LookBackDirection)
		{
			pitchValue -= model->pitchSpeed * pitchDemand * time_slice * heatMultiplier;
			pitchValue = Radian::Normalize(pitchValue);
		}
		else
		{
			pitchValue += model->pitchSpeed * pitchDemand * time_slice * heatMultiplier;
			pitchValue = Radian::Normalize(pitchValue);
		}

	}

#if 0
	if (correctionTime > 0.0f)
	{



		Stuff::Scalar lerp_value = correctionTime / time_slice;
		Max_Clamp(lerp_value, 1.0f);

		Scalar frame_correction;
		frame_correction = Lerp(0.0f, yawValueCorrection, lerp_value);

		SPEWALWAYS(("jerryeds", "LERP YAW : %f", frame_correction));

		yawValue += frame_correction;
		yawValueCorrection -= frame_correction;

		frame_correction = Lerp(0.0f, pitchValueCorrection, lerp_value);

		SPEWALWAYS(("jerryeds", "LERP PITCH : %f", pitchValueCorrection));


		pitchValue += frame_correction;
		pitchValueCorrection -= frame_correction;

		correctionTime -= time_slice;
	}
#endif


	if (correctionCurTime < correctionTime)
	{
		correctionCurTime += time_slice;

		Stuff::Scalar lerp_value = correctionCurTime / correctionTime;
		Max_Clamp(lerp_value, 1.0f);
		
		Scalar frame_correction = 0.0f;
#if 0
		if (yawValueCorrection)
			frame_correction = Lerp(oldYawValue, yawValueCorrection, lerp_value);
#else
		if (yawValueCorrection){
			Radian start_pos=oldYawValue;
			Radian end_pos=yawValueCorrection;
			start_pos.Normalize();
			end_pos.Normalize();

			Radian correction;
			correction.Lerp(start_pos,end_pos,lerp_value);
			frame_correction=correction;
		}
#endif
		yawValue = frame_correction;
		//SPEWALWAYS(("jerryeds", "LERP YAW : %f", frame_correction));

		frame_correction = 0.0f;
		if (pitchValueCorrection)
			frame_correction = Lerp(oldPitchValue, pitchValueCorrection, lerp_value);
		pitchValue = frame_correction;
		//SPEWALWAYS(("jerryeds", "LERP PITCH : %f", pitchValueCorrection));
		

	}

	if (yawValue > model->twistRadius)
	{
		if (GetParentVehicle ()->vehicleInterface)
			GetParentVehicle ()->vehicleInterface->ReactToEvent (VehicleInterface::TORSO_TWIST_MAX_REACHED);
	}
	if (yawValue < -model->twistRadius)
	{
		if (GetParentVehicle ()->vehicleInterface)
			GetParentVehicle ()->vehicleInterface->ReactToEvent (VehicleInterface::TORSO_TWIST_MAX_REACHED);
	}
	Max_Clamp(yawValue, model->twistRadius);
	Min_Clamp(yawValue, -model->twistRadius);
	Max_Clamp(pitchValue, model->pitchRadius);
	Min_Clamp(pitchValue, -model->pitchRadius);

	TurnArms(time_slice);

	if(rotationJoint)
	{
		SetRotation();
	}
	else if (eyeJoint != NULL)
	{
		Check_Object(leftArmJoint);
		Check_Object(rightArmJoint);
		Check_Object(eyeJoint);

		SetTwistRotation();
		SetPitchRotationSpecial();
	}
	else
	{
		if(twistJoint)
		{
			SetTwistRotation();
		}

		if(pitchJoint)
		{
			SetPitchRotation();
		}
	}
			
	Subsystem::PreCollisionExecute(till);
}


//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
bool
	Torso::ConnectSubsystem()
{
	Check_Object(this);
	
	MWObject *mw_object = GetParentVehicle();
	Check_Object(mw_object);

	const GameModel *model;
	model = GetGameModel();
	Check_Object(model);

	if(model->twistJointName)
	{
		twistJoint = mw_object->FindChildMover(model->twistJointName);
		Check_Object(twistJoint);

		const Stuff::LinearMatrix4D &local_to_world = twistJoint->GetLocalToParent();

		YawPitchRoll new_rotation;
		new_rotation= local_to_world;
		
		yawValue = new_rotation.yaw;
	}
	if(model->pitchJointName)
	{
		pitchJoint = mw_object->FindChildMover(model->pitchJointName);
		
		Check_Object(pitchJoint);

		const Stuff::LinearMatrix4D &local_to_world = pitchJoint->GetLocalToParent();

		YawPitchRoll new_rotation;
		new_rotation= local_to_world;
		
		pitchValue = new_rotation.pitch;
	}

	if (*model->leftArmJointName != '0')
	{
		leftArmJoint = mw_object->FindChildMover(model->leftArmJointName);
		Check_Object(leftArmJoint);
	}
	if (*model->rightArmJointName != '0')
	{
		rightArmJoint = mw_object->FindChildMover(model->rightArmJointName);
		Check_Object(rightArmJoint);
	}
	if (*model->eyeJointName != '0')
	{
		eyeJoint = mw_object->FindChildMover(model->eyeJointName);
		Check_Object(eyeJoint);
	}
	if (*model->cageJointName != '0')
	{
		cageJoint = mw_object->FindChildMover(model->cageJointName);
		Check_Object(cageJoint);
	}



	if (leftArmJoint || rightArmJoint || eyeJoint)
	{
		// IF YOU HAVE ONE YOU MUST HAVE ALL!!!
		Verify(leftArmJoint != NULL);
		Verify(rightArmJoint != NULL);
		Verify(eyeJoint != NULL);
	}


	if(pitchJoint == twistJoint)
	{
		rotationJoint = pitchJoint;
	}
	//Must call the inherited virtual function!!!!
	return Subsystem::ConnectSubsystem();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Torso::SetTwistRotation()
{
	Check_Object(this);
	Check_Object(twistJoint);

	YawPitchRoll new_rotation(YawPitchRoll::Identity);
	new_rotation.yaw = yawValue;

	LinearMatrix4D new_local_to_world = LinearMatrix4D::Identity;
	Point3D trans;
	trans = twistJoint->GetLocalToParent();

	new_local_to_world.BuildRotation(new_rotation);
	new_local_to_world.BuildTranslation(trans);

	twistJoint->SetNewLocalToParent(new_local_to_world);
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Torso::SetPitchRotation()
{
	Check_Object(this);
	Check_Object(pitchJoint);

	YawPitchRoll new_rotation(YawPitchRoll::Identity);
	new_rotation.pitch = pitchValue;

	LinearMatrix4D new_local_to_world = LinearMatrix4D::Identity;
	Point3D trans;
	trans = pitchJoint->GetLocalToParent();

	new_local_to_world.BuildRotation(new_rotation);
	new_local_to_world.BuildTranslation(trans);

	pitchJoint->SetNewLocalToParent(new_local_to_world);

}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Torso::SetPitchRotationSpecial()
{
	Check_Object(this);
	Check_Object(eyeJoint);
	Check_Object(leftArmJoint);
	Check_Object(rightArmJoint);

	Mech *mech = Cast_Object(Mech *, GetParentVehicle());
	EyePointManager *eye_point_manager = mech->GetEyePointManager();

	const GameModel *model = GetGameModel();
	Check_Object(model);

	if (internalTorso)
	{
		
		// set the pitch joint to be zero
		YawPitchRoll new_rotation(YawPitchRoll::Identity);
		
		LinearMatrix4D new_local_to_world = LinearMatrix4D::Identity;
		Point3D trans;
		trans = pitchJoint->GetLocalToParent();
		new_local_to_world.BuildTranslation(trans);
		pitchJoint->SetNewLocalToParent(new_local_to_world);


		// set the eye joint to have the pitch
		new_rotation = YawPitchRoll(YawPitchRoll::Identity);
		new_rotation.pitch = armPitch + eyePitch;
		new_rotation.yaw = eyeYaw;

		new_local_to_world = LinearMatrix4D::Identity;
		new_local_to_world.BuildRotation(new_rotation);
		eye_point_manager->SetEyeDirection(new_local_to_world);




		if (cageJoint)
		{
			LinearMatrix4D new_local_to_world = LinearMatrix4D::Identity;
			Point3D trans;
			YawPitchRoll rot;
		
			rot.roll = 0.0f;
			rot.yaw = 0.0f;
			rot.pitch = (armPitch + eyePitch)*model->cageRatioAngle;

			trans = cageJoint->GetLocalToParent();
			new_local_to_world.BuildTranslation(trans);
			new_local_to_world.BuildRotation(rot);
			cageJoint->SetNewLocalToParent(new_local_to_world);

		}

	}



	// calculate arm ratio...
	Scalar arm_pitch = 0.0f;
	Scalar torso_pitch = 0.0f;

	if (!internalTorso)
	{

		if (lookDirection != SurrenderDirection)
		{

			Verify(model->armRatioAngle > 0);
			arm_pitch = Abs(yawValue) / model->armRatioAngle;
			Max_Clamp(arm_pitch, 1.0f);
			torso_pitch = 1.0f - arm_pitch;
		}
		else
		{
			arm_pitch = 1.0f;
		}
	}
	else
	{
		arm_pitch = 1.0f;
		torso_pitch = 0.0f;
	}

	// set the pitch joint
	YawPitchRoll new_rotation(YawPitchRoll::Identity);
	new_rotation = YawPitchRoll(YawPitchRoll::Identity);
	new_rotation.pitch = torso_pitch * pitchValue;
	Clamp(new_rotation.pitch, -45.0f*Radians_Per_Degree, 45.0f*Radians_Per_Degree);
	
	LinearMatrix4D new_local_to_world = LinearMatrix4D::Identity;
	Point3D trans;
	trans = pitchJoint->GetLocalToParent();



	new_local_to_world.BuildRotation(new_rotation);
	new_local_to_world.BuildTranslation(trans);

	if (!internalTorso)
	{
		pitchJoint->SetNewLocalToParent(new_local_to_world);
	}
	// set the arm joints

	new_rotation = YawPitchRoll(YawPitchRoll::Identity);
	new_rotation.pitch = arm_pitch * armPitch;
	new_rotation.yaw = leftArmYaw;

	new_local_to_world = LinearMatrix4D::Identity;
	trans = leftArmJoint->GetLocalToParent();

	new_local_to_world.BuildRotation(new_rotation);
	new_local_to_world.BuildTranslation(trans);

	leftArmJoint->SetNewLocalToParent(new_local_to_world);

	
	new_rotation = YawPitchRoll(YawPitchRoll::Identity);
	new_rotation.pitch = arm_pitch * armPitch;
	new_rotation.yaw = rightArmYaw;

	new_local_to_world = LinearMatrix4D::Identity;
	trans = rightArmJoint->GetLocalToParent();

	new_local_to_world.BuildRotation(new_rotation);
	new_local_to_world.BuildTranslation(trans);

	rightArmJoint->SetNewLocalToParent(new_local_to_world);


	if (!internalTorso)
	{
		YawPitchRoll new_rotation(YawPitchRoll::Identity);
		
		LinearMatrix4D new_local_to_world = LinearMatrix4D::Identity;
		new_rotation.yaw = eyeYaw;
		new_rotation.pitch = arm_pitch * armPitch + eyePitch;
		new_local_to_world.BuildRotation(new_rotation);
		
		eye_point_manager->SetEyeDirection(new_local_to_world);
		//eye_point_manager->SetTorsoRotation(LinearMatrix4D::Identity);
	}



	{

		LinearMatrix4D torso_rot = LinearMatrix4D::Identity;
		YawPitchRoll new_rotation = YawPitchRoll::Identity;
		new_rotation.yaw = yawValue;
		torso_rot.BuildRotation(new_rotation);
		eye_point_manager->SetTorsoRotation(torso_rot);

	}

}


//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Torso::SetRotation()
{
	Check_Object(this);
	Check_Object(rotationJoint);

	YawPitchRoll new_rotation(YawPitchRoll::Identity);
	new_rotation.yaw = yawValue;
	new_rotation.pitch = pitchValue;

	LinearMatrix4D new_local_to_world = LinearMatrix4D::Identity;
	Point3D trans;
	trans = pitchJoint->GetLocalToParent();

	new_local_to_world.BuildRotation(new_rotation);
	new_local_to_world.BuildTranslation(trans);

	rotationJoint->SetNewLocalToParent(new_local_to_world);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
const LinearMatrix4D&
	Torso::GetTwistJointMatrix()
{
	Check_Object(this);
	if(twistJoint)
	{
		Check_Object(twistJoint);
		return twistJoint->GetLocalToWorld();
	}
	return LinearMatrix4D::Identity;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
const LinearMatrix4D&
	Torso::GetPitchJointMatrix()
{
	Check_Object(this);
	if(pitchJoint)
	{
		Check_Object(pitchJoint);
		return pitchJoint->GetLocalToWorld();
	}
	return LinearMatrix4D::Identity;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	Torso::GetTwistSpeed()
{
	Check_Object(this);
	const GameModel *model = GetGameModel();
	Check_Object(model);
	
	return model->twistSpeed;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Scalar
	Torso::GetPitchSpeed()
{
	Check_Object(this);
	const GameModel *model = GetGameModel();
	Check_Object(model);
	
	return model->pitchSpeed;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Torso::TestInstance() const
{
	Verify(IsDerivedFrom(DefaultData));
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
// this is an ugly hack but it works  ...
void
	Torso::PitchToPoint(const Stuff::Point3D& point)
{
	LinearMatrix4D pitch_matrix;
	
	if (leftArmJoint != 0)
	{
		pitch_matrix = leftArmJoint->GetLocalToWorld();
	}
	else
	{
		if (rightArmJoint != 0)
		{
			pitch_matrix = rightArmJoint->GetLocalToWorld();
		}
		else
		{
			if (pitchJoint == 0)
			{
				return;
			}

			pitch_matrix = pitchJoint->GetLocalToWorld();
		}
	}

	UnitVector3D unit_forward;
	pitch_matrix.GetLocalForwardInWorld(&unit_forward);

	Point3D projection(unit_forward);

	Stuff::Vector3D delta_to_point;
	delta_to_point.Subtract((Stuff::Point3D)pitch_matrix,point);

	Stuff::Scalar distance = delta_to_point.GetApproximateLength();
	
	projection *= distance;
	projection += (Stuff::Point3D)pitch_matrix;

	Stuff::Scalar pitch = Tan(Fabs(point.y - projection.y) / distance);

	if (pitch > 1)
	{
		pitch = 1;
	}

	pitchDemand = GetPitchSpeed() * pitch;

	if (point.y > projection.y)
	{
		pitchDemand = -pitchDemand;
	}
}



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

void Torso::LookSurrender()
{

	if (rightArmTwistState == ToSideTwistState || rightArmTwistState == SideTwistState)
	{
		rightArmTwistState = ToForwardTwistState;
		rightArmTwistTimer = armTwistTime - rightArmTwistTimer;
	}

	if (leftArmTwistState == ToSideTwistState || leftArmTwistState == SideTwistState)
	{
		leftArmTwistState = ToForwardTwistState;
		leftArmTwistTimer = armTwistTime - leftArmTwistTimer;
	}

	if (armPitchState == ForwardPitchState || armPitchState == ToForwardPitchState)
	{
		armPitchState = ToSurrenderState;
		pitchArmTwistTimer = armPitchTime - pitchArmTwistTimer;
	}

	lookDirection = SurrenderDirection;
}

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

void Torso::LookForward()
{

	if (lookDirection == SurrenderDirection)
		return;

	// turn off both arms
	if (rightArmTwistState == ToSideTwistState || rightArmTwistState == SideTwistState)
	{
		rightArmTwistState = ToForwardTwistState;
		rightArmTwistTimer = armTwistTime - rightArmTwistTimer;
	}

	if (leftArmTwistState == ToSideTwistState || leftArmTwistState == SideTwistState)
	{
		leftArmTwistState = ToForwardTwistState;
		leftArmTwistTimer = armTwistTime - leftArmTwistTimer;
	}

	if (armPitchState == ToSurrenderState || armPitchState == SurrenderPitchState)
	{
		armPitchState = ToForwardPitchState;
		pitchArmTwistTimer = armPitchTime - pitchArmTwistTimer;
	}

	Stuff::Scalar currentTime = (float)gos_GetElapsedTime();
	if (lookDirection != LookForwardDirection)
	{
		//
		// Since we may not have the yaw centered right now
		//
		if (lookDirection == LookBackDirection)
		{
			eyeYaw = 0.0f;
		}

		eyeTransitionEndTime = currentTime + Abs(((eyeYaw/(Stuff::Radians_Per_Degree*90.0f))*LOOK_SIDE_TRANSITION_TIME));
	}
	else if (eyeTransitionEndTime < currentTime)
	{
		eyeYaw = 0.0f;
	}
	else
	{
		Stuff::Scalar transitionRatio = (eyeTransitionEndTime - currentTime)/LOOK_SIDE_TRANSITION_TIME;
		if (eyeYaw > 0.0f)
		{
			eyeYaw = 90.0f * Stuff::Radians_Per_Degree * transitionRatio;
		}
		else
		{
			eyeYaw = -90.0f * Stuff::Radians_Per_Degree * transitionRatio;
		}
	}
	eyePitch = 0.0f;

	lookDirection = LookForwardDirection;
}

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

void Torso::LookRight()
{

	if (lookDirection == SurrenderDirection)
		return;

	// INTENTALLY REVERSED
	// The animators thought it would be a good idea to name the arms
	// like in the theater.  So the left arm is "stage left" arm..or the right arm.
	// it is by looking at the mech from the front that you decide on the left right.
	// ...go blame the animators....I'm just the messanger

	if (rightArmTwistState == ForwardTwistState || rightArmTwistState == ToForwardTwistState)
	{
		rightArmTwistState = ToSideTwistState;
		rightArmTwistTimer = armTwistTime - rightArmTwistTimer;
	}


	// turn off left arm
	if (leftArmTwistState == ToSideTwistState || leftArmTwistState == SideTwistState)
	{
		leftArmTwistState = ToForwardTwistState;
		leftArmTwistTimer = armTwistTime - leftArmTwistTimer;
	}

	Stuff::Scalar currentTime = (float)gos_GetElapsedTime();
	if (lookDirection != LookRightDirection)
	{
		if (lookDirection == LookBackDirection)
		{
			eyeYaw = 0.0f;
		}
		//
		// Since we may not have the yaw centered right now
		//
		eyeTransitionEndTime = currentTime + Abs(((eyeYaw/(Stuff::Radians_Per_Degree*90.0f))*LOOK_SIDE_TRANSITION_TIME))+LOOK_SIDE_TRANSITION_TIME;
	}
	else if (eyeTransitionEndTime < currentTime)
	{
		eyeYaw = -90.0f * Stuff::Radians_Per_Degree;
	}
	else
	{
		Stuff::Scalar transitionRatio = (LOOK_SIDE_TRANSITION_TIME - (eyeTransitionEndTime - currentTime))/LOOK_SIDE_TRANSITION_TIME;
//		Min_Clamp(transitionRatio, -LOOK_SIDE_TRANSITION_TIME);
		eyeYaw = -90.0f * Stuff::Radians_Per_Degree * transitionRatio;
	}
	eyePitch = 0.0f;

	lookDirection = LookRightDirection;

}

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

void Torso::LookLeft()
{
	if (lookDirection == SurrenderDirection)
		return;


	if (leftArmTwistState == ForwardTwistState || leftArmTwistState == ToForwardTwistState)
	{
		leftArmTwistState = ToSideTwistState;
		leftArmTwistTimer = armTwistTime - leftArmTwistTimer;
	}

	// turn off right arm
	if (rightArmTwistState == ToSideTwistState || rightArmTwistState == SideTwistState)
	{
		rightArmTwistState = ToForwardTwistState;
		rightArmTwistTimer = armTwistTime - rightArmTwistTimer;
	}
	
	Stuff::Scalar currentTime = (float)gos_GetElapsedTime();
	if (lookDirection != LookLeftDirection)
	{
		if (lookDirection == LookBackDirection)
		{
			eyeYaw = 0.0f;
		}
		//
		// Since we may not have the yaw centered right now
		//
		eyeTransitionEndTime = currentTime + Abs(((eyeYaw/(Stuff::Radians_Per_Degree*90.0f))*LOOK_SIDE_TRANSITION_TIME))+LOOK_SIDE_TRANSITION_TIME;
	}
	else if (eyeTransitionEndTime < currentTime)
	{
		eyeYaw = 90.0f * Stuff::Radians_Per_Degree;
	}
	else
	{
		Stuff::Scalar transitionRatio = (LOOK_SIDE_TRANSITION_TIME - (eyeTransitionEndTime - currentTime))/LOOK_SIDE_TRANSITION_TIME;
//		Min_Clamp(transitionRatio, -1.0f);
//		Max_Clamp(transitionRatio, 1.0f);
		eyeYaw = 90.0f * Stuff::Radians_Per_Degree * transitionRatio;
	}
	eyePitch = 0.0f;
	lookDirection = LookLeftDirection;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Torso::LookBack()
{

	if (lookDirection == SurrenderDirection)
		return;

// MSL 5.04 Rear View Arm Movement (Disabled)
//	if (leftArmTwistState == ForwardTwistState || leftArmTwistState == ToForwardTwistState)
//	{
//		leftArmTwistState = ToSideTwistState;
//		leftArmTwistTimer = armTwistTime - leftArmTwistTimer;
//	}

//	if (rightArmTwistState == ForwardTwistState || rightArmTwistState == ToForwardTwistState)
//	{
//		rightArmTwistState = ToSideTwistState;
//		rightArmTwistTimer = armTwistTime - rightArmTwistTimer;
//	}

	// turn off both arms
	if (rightArmTwistState == ToSideTwistState || rightArmTwistState == SideTwistState)
	{
		rightArmTwistState = ToForwardTwistState;
		rightArmTwistTimer = armTwistTime - rightArmTwistTimer;
	}

	if (leftArmTwistState == ToSideTwistState || leftArmTwistState == SideTwistState)
	{
		leftArmTwistState = ToForwardTwistState;
		leftArmTwistTimer = armTwistTime - leftArmTwistTimer;
	}

	if (armPitchState == ToSurrenderState || armPitchState == SurrenderPitchState)
	{
		armPitchState = ToForwardPitchState;
		pitchArmTwistTimer = armPitchTime - pitchArmTwistTimer;
	}

	eyeYaw = 180.0f * Stuff::Radians_Per_Degree;
	if (lookDirection != LookBackDirection)
	{
		lookTypeVerticalAdjust = 0.0f; // currently being reset, but we may want to change that.
	}
// MSL 5.04 Rear View Torso Movement
//	const GameModel *model = GetGameModel();
//	Check_Object(model);
	eyePitch = -(pitchValue + armPitch) + lookTypeVerticalAdjust;
// MSL 5.04 Rear View Torso Movement
//	Max_Clamp(eyePitch, model->pitchRadius);
//	Min_Clamp(eyePitch, -model->pitchRadius);
	lookDirection = LookBackDirection;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Torso::LookDown()
{
	if (lookDirection == SurrenderDirection)
		return;

	// turn off right arm
	if (rightArmTwistState == ToSideTwistState || rightArmTwistState == SideTwistState)
	{
		rightArmTwistState = ToForwardTwistState;
		rightArmTwistTimer = armTwistTime - rightArmTwistTimer;
	}

  // turn off left arm
	if (leftArmTwistState == ToSideTwistState || leftArmTwistState == SideTwistState)
	{
		leftArmTwistState = ToForwardTwistState;
		leftArmTwistTimer = armTwistTime - leftArmTwistTimer;
	}

	if (armPitchState == ToSurrenderState || armPitchState == SurrenderPitchState)
	{
		armPitchState = ToForwardPitchState;
		pitchArmTwistTimer = armPitchTime - pitchArmTwistTimer;
	}

	eyeYaw = 0.0f;
  //
  // 75 is just a guess at where we want the down pitch to be. We can experiment from this and eventually
  // expose as a property if necessary.
  //
  if (lookDirection != LookDownDirection)
  {
    lookTypeVerticalAdjust = 0.0f; // currently being reset, but we may want to change that.
  }
  eyePitch = (LOOKDOWN_ANGLE * Stuff::Radians_Per_Degree) - pitchValue + lookTypeVerticalAdjust;
	lookDirection = LookDownDirection;
}


//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void Torso::TurnArms(Stuff::Scalar time_slice)
{
	

	if (leftArmTwistState == ToSideTwistState || leftArmTwistState == ToForwardTwistState)
	{
		leftArmTwistTimer -= time_slice;
		Min_Clamp(leftArmTwistTimer, 0.0f);
		

		Stuff::Scalar lerp_time;
		lerp_time = leftArmTwistTimer/armTwistTime;

		if (leftArmTwistState == ToForwardTwistState)
		{
			lerp_time = 1.0f - lerp_time;
		}


		
		Scalar side_rotation;
		side_rotation = 75.0f*Stuff::Radians_Per_Degree;

		Scalar anim_rotation;
		anim_rotation = 0.0f;
	
		leftArmYaw = Lerp(side_rotation, anim_rotation, lerp_time);


		if (leftArmTwistTimer == 0.0f)
		{
			if (leftArmTwistState == ToForwardTwistState)
			{
				leftArmTwistState = ForwardTwistState;
			}
			else
			{
				leftArmTwistState = SideTwistState;
			}
		}
	}
	else if (leftArmTwistState == SideTwistState)
	{
		leftArmYaw = 75.0f*Stuff::Radians_Per_Degree;
	}
	else
	{
		leftArmYaw = 0.0f;
	}

	if (rightArmTwistState == ToSideTwistState || rightArmTwistState == ToForwardTwistState)
	{
		rightArmTwistTimer -= time_slice;
		Min_Clamp(rightArmTwistTimer, 0.0f);
		

		Stuff::Scalar lerp_time;
		lerp_time = rightArmTwistTimer/armTwistTime;

		if (rightArmTwistState == ToForwardTwistState)
		{
			lerp_time = 1.0f - lerp_time;
		}


		Scalar side_rotation;
		side_rotation = -75.0f*Stuff::Radians_Per_Degree;

		Scalar anim_rotation;
		anim_rotation = 0.0f;
	
		rightArmYaw = Lerp(side_rotation, anim_rotation, lerp_time);


		if (rightArmTwistTimer == 0.0f)
		{
			if (rightArmTwistState == ToForwardTwistState)
			{
				rightArmTwistState = ForwardTwistState;
			}
			else
			{
				rightArmTwistState = SideTwistState;
			}
		}
	}
	else if (rightArmTwistState == SideTwistState)
	{
		rightArmYaw = -75.0f*Stuff::Radians_Per_Degree;
	}
	else
	{
		rightArmYaw = 0.0f;
	}


	if (armPitchState == ToSurrenderState || armPitchState == ToForwardPitchState)
	{
		pitchArmTwistTimer -= time_slice;
		Min_Clamp(pitchArmTwistTimer, 0.0f);
		

		Stuff::Scalar lerp_time;
		lerp_time = pitchArmTwistTimer/armPitchTime;

		if (armPitchState == ToForwardPitchState)
		{
			lerp_time = 1.0f - lerp_time;
		}


		Scalar side_rotation;
		side_rotation = -45.0f*Stuff::Radians_Per_Degree;

		Scalar anim_rotation;
		anim_rotation = pitchValue;
	
		armPitch = Lerp(side_rotation, anim_rotation, lerp_time);


		if (pitchArmTwistTimer == 0.0f)
		{
			if (armPitchState == ToForwardTwistState)
			{
				armPitchState = ForwardPitchState;
			}
			else
			{
				armPitchState = SurrenderPitchState;
			}
		}
	}
	else if (armPitchState == SurrenderPitchState)
	{
		armPitch = -45.0f*Stuff::Radians_Per_Degree;
	}
	else
	{
		armPitch = pitchValue;
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Torso::SetHeatMultiplier(Stuff::Scalar value)
{
	Check_Object(this);
	Clamp(value, 0.0f, 1.0f);
	
	heatMultiplier = value;
}