#include "munga.h"
#pragma hdrstop

#include "joint.h"
#include "jmover.h"

//##########################################################################
//########################    BallTranslation   ############################
//##########################################################################

#if defined(USE_SIGNATURE)
	int
		Is_Signature_Bad(const volatile BallTranslation *)
	{
		return False;
	}
#endif

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
BallTranslation::BallTranslation()
{
	ballJoint = EulerAngles::Identity;
	jointTranslation = Point3D::Identity;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
BallTranslation::BallTranslation(const BallTranslation &new_ball)
{
	Check(&new_ball);
	ballJoint = new_ball.ballJoint;
	jointTranslation = new_ball.jointTranslation;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	BallTranslation::TestInstance() const
{
	return True;
}

//##########################################################################
//##########################    Joint        ###############################
//##########################################################################

#if defined(USE_SIGNATURE)
	int
		Is_Signature_Bad(const volatile Joint *)
	{
		return False;
	}
#endif

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Joint::Joint(JointSubsystem *subsystem)
{
	Check(subsystem);

	jointType = StaticJointType;
	jointModified = True;
	ballJoint = NULL;
	jointSubsystem = subsystem;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Joint::Joint(
	JointSubsystem *subsystem,
	JointType joint_type
)
{
	Check(subsystem);

	jointType = joint_type;
	jointModified = True;
	jointSubsystem = subsystem;
	ballJoint = NULL;

	switch(joint_type)
	{
		case Joint::BallJointType:
			ballJoint = new EulerAngles(EulerAngles::Identity);
			Register_Object(ballJoint);
			break;

		case Joint::BallTranslationJointType:
			ballTranslationJoint = new BallTranslation;
			Register_Object(ballTranslationJoint);
			break;

		case Joint::HingeXJointType:
			hingeJoint = new Hinge(X_Axis, 0.0f);
			Register_Object(hingeJoint);
			break;

		case Joint::HingeYJointType:
			hingeJoint = new Hinge(Y_Axis, 0.0f);
			Register_Object(hingeJoint);
			break;

		case Joint::HingeZJointType:
			hingeJoint = new Hinge(Z_Axis, 0.0f);
			Register_Object(hingeJoint);
			break;

		case Joint::StaticJointType:
			break;

#if DEBUG_LEVEL>0
		default:
			Fail("Unsupported jointType!\n");
			break;
#endif
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Joint::Joint(const Joint &new_joint)
{
	jointModified = True;
	jointSubsystem = new_joint.jointSubsystem;
	jointType = new_joint.jointType;
	ballJoint = NULL;

	//
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	// Create a new joint of correct size
	//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
	//
	switch(new_joint.jointType)
	{
		case HingeXJointType:
		case HingeYJointType:
		case HingeZJointType:
			hingeJoint = new Hinge(*new_joint.hingeJoint);
			Register_Object(hingeJoint);
			break;

		case BallJointType:
			ballJoint = new EulerAngles(*new_joint.ballJoint);
			Register_Object(ballJoint);
			break;

		case BallTranslationJointType:
			ballTranslationJoint =
				new BallTranslation(*new_joint.ballTranslationJoint);
			Register_Object(ballTranslationJoint);
			break;

		case StaticJointType:
			break;

#if DEBUG_LEVEL>0
		default:
			Fail("Unsupported jointType!\n");
			break;
#endif
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Joint&
	Joint::operator=(const Joint &new_joint)
{
	Check(this);

	jointModified = True;

	//
	//---------------------------------------------------
	// If not same kind of joint free up old joint memory
	//---------------------------------------------------
	//
	if(new_joint.jointType != jointType)
	{
		switch(jointType)
		{
			case HingeXJointType:
			case HingeYJointType:
			case HingeZJointType:
				Unregister_Object(hingeJoint);
				delete hingeJoint;
				break;

			case BallJointType:
				Unregister_Object(ballJoint);
				delete ballJoint;
				break;

			case BallTranslationJointType:
				Unregister_Object(ballTranslationJoint);
				delete ballTranslationJoint;
				break;

			case StaticJointType:
				break;

#if DEBUG_LEVEL>0
			default:
				Fail("Unsupported jointType!\n");
				break;
#endif
		}
		jointType = new_joint.jointType;

		//
		//-----------------------------------
		// Create a new joint of correct size
		//-----------------------------------
		//
		switch(new_joint.jointType)
		{
			case HingeXJointType:
			case HingeYJointType:
			case HingeZJointType:
				hingeJoint = new Hinge(*new_joint.hingeJoint);
				Register_Object(hingeJoint);
				break;

			case BallJointType:
				ballJoint = new EulerAngles(*new_joint.ballJoint);
				Register_Object(ballJoint);
				break;

			case BallTranslationJointType:
				ballTranslationJoint =
					new BallTranslation(*new_joint.ballTranslationJoint);
				Register_Object(ballTranslationJoint);
				break;

			case StaticJointType:
				break;

#if DEBUG_LEVEL>0
			default:
				Fail("Unsupported jointType!\n");
				break;
#endif
		}
	}

	//
	//-----------------------------------
	// Otherwise simply copy all the data
	//-----------------------------------
	//
	else
	{
		switch(new_joint.jointType)
		{
			case HingeXJointType:
			case HingeYJointType:
			case HingeZJointType:
				hingeJoint = new_joint.hingeJoint;
				break;

			case BallJointType:
				ballJoint = new_joint.ballJoint;
				break;

			case BallTranslationJointType:
				ballTranslationJoint = new_joint.ballTranslationJoint;
				break;

			case StaticJointType:
				break;

#if DEBUG_LEVEL>0
			default:
				Fail("Unsupported jointType!\n");
				break;
#endif
		}
	}
	return (*this);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Joint::~Joint()
{
	switch(jointType)
	{
		case HingeXJointType:
		case HingeYJointType:
		case HingeZJointType:
			Unregister_Object(hingeJoint);
			delete hingeJoint;
			break;

		case BallJointType:
			Unregister_Object(ballJoint);
			delete ballJoint;
			break;

		case BallTranslationJointType:
			Unregister_Object(ballTranslationJoint);
			delete ballTranslationJoint;
			break;

		case StaticJointType:
			break;

#if DEBUG_LEVEL>0
		default:
			Fail("Unsupported jointType!\n");
			break;
#endif
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
const EulerAngles&
	Joint::GetEulerAngles()
{
	Check(this);
	switch(jointType)
	{
		case BallJointType:
			return *ballJoint;
		case BallTranslationJointType:
			return ballTranslationJoint->GetEulerAngles();
#if DEBUG_LEVEL>0
		default:
			Fail("Joint must be a BallJoint of BallTranslationJoint \n");
			break;
#endif
	}
	return *ballJoint;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joint::SetHinge(const Hinge &new_hinge)
{
	Check(this);
	if(*hingeJoint != new_hinge)
	{
		jointModified = True;
		(*hingeJoint) = new_hinge;
		Verify(jointSubsystem);
		jointSubsystem->ModifyJoints();
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joint::SetRotation(const EulerAngles &new_angles)
{
	Check(this);

	switch(jointType)
	{
		case BallJointType:
			if ((*ballJoint) != new_angles)
			{
				(*ballJoint) = new_angles;
Joint_Modified:
				jointModified = True;
				Verify(jointSubsystem);
				jointSubsystem->ModifyJoints();
			}
			break;

		case BallTranslationJointType:
			if (ballTranslationJoint->GetEulerAngles() != new_angles)
			{
				ballTranslationJoint->SetEulerAngles(new_angles);
				goto Joint_Modified;
			}
			break;

#if DEBUG_LEVEL>0
		default:
			Fail("Joint must be a Ball or Ball Translation Joint! \n");
			break;
#endif
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joint::SetRotation(const Radian &new_radian)
{
	Check(this);

	Verify (
		(jointType == HingeXJointType ) ||
		(jointType == HingeYJointType ) ||
		(jointType == HingeZJointType )
	);
	if(hingeJoint->rotationAmount != new_radian)
	{
		hingeJoint->rotationAmount = new_radian;
		jointModified = True;
		Verify(jointSubsystem);
		jointSubsystem->ModifyJoints();
	}
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Joint::SetTranslation(const Point3D &new_trans)
{
	Check(this);
	jointModified = True;

	Verify(jointType == BallTranslationJointType);
	if (ballTranslationJoint->GetTranslation() != new_trans)
	{
		ballTranslationJoint->SetTranslation(new_trans);
		jointModified = True;
		Verify(jointSubsystem);
		jointSubsystem->ModifyJoints();
	}
}

//#############################################################################
// Shared Data Support
//
JointSubsystem::SharedData
	JointSubsystem::DefaultData(
		JointSubsystem::GetClassDerivations(),
		JointSubsystem::GetMessageHandlers(),
		JointSubsystem::GetAttributeIndex(),
		JointSubsystem::StateCount
	);

Derivation* JointSubsystem::GetClassDerivations()
{
	static Derivation classDerivations(Subsystem::GetClassDerivations(), "JointSubsystem");
	return &classDerivations;
}

#if 0
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	JointSubsystem::JointSubsystemSimulation(Scalar )
{
	Check(this);


	Joint *current_joint;
	JointTableIterator iterator(jointTable);

	while ((current_joint = iterator.ReadAndNext() )!= NULL)
	{
		if (current_joint->IsJointModified())
		{
			JointedMover *jointed_mover = Cast_Object(
				JointedMover*,
				GetEntity()
			);
			jointed_mover->ForceSegmentTransformUpdate();
			break;
		}
	}

}
#endif

//#############################################################################
// Constructer/Destructor Support
//

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
JointSubsystem::JointSubsystem(
	Entity *owner,
	int subsystem_ID,
	SharedData &shared_data
):
	Subsystem(
		owner,
		subsystem_ID,
		"Joint Subsystem",
		JointSubsystemClassID,
		shared_data
	),
	jointTable(NULL, False)
{
	damageZone = new DamageZone(this, 0);
	Register_Object(damageZone);
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
JointSubsystem::~JointSubsystem()
{
	JointTableIterator iterator(jointTable);
	iterator.DeletePlugs();
	Unregister_Object(damageZone);
	delete damageZone;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Logical
	JointSubsystem::TestInstance() const
{
	return IsDerivedFrom(*GetClassDerivations());
}