#include "rp.h"
#pragma hdrstop

#include "thruster.h"
#include "vtvmppr.h"
#include "..\munga\boxsolid.h"
#include "vtv.h"

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

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

//#############################################################################
// Attribute Support
//
const Thruster::IndexEntry
	Thruster::AttributePointers[]=
{
	ATTRIBUTE_ENTRY(Thruster, ThrusterOffset,				thrusterOffset),
	ATTRIBUTE_ENTRY(Thruster, ThrusterPosition,			thrusterPosition),
	ATTRIBUTE_ENTRY(Thruster, CurrentAcceleration,		currentAcceleration),
	ATTRIBUTE_ENTRY(Thruster, CurrentHeight,				currentHeight),
	ATTRIBUTE_ENTRY(Thruster, MaxThrusterRotationRate,	maxThrusterRotationRate),
	ATTRIBUTE_ENTRY(Thruster, MomentArm,					momentArm),
	ATTRIBUTE_ENTRY(Thruster, MomentArmLength,			momentArmLength)
};

Thruster::AttributeIndexSet& Thruster::GetAttributeIndex()
{
	static Thruster::AttributeIndexSet attributeIndex(ELEMENTS(Thruster::AttributePointers),
		Thruster::AttributePointers,
		Subsystem::GetAttributeIndex()
	);
	return attributeIndex;
}

//#############################################################################
// Model Support
//
void
	Thruster::PrimeThruster(Scalar time_slice)
{
	Check(this);

	//
	//--------------------------------------------------------------------------
	// Get pointers to the other subsystems we will have to deal with inside the
	// VTV
	//--------------------------------------------------------------------------
	//
	VTV* vtv = GetEntity();
	Check(vtv);
	if (vtv->GetSimulationState() == VTV::BurningState)
	{
		Check_Fpu();
		return;
	}

	VTVControlsMapper* controls =
		Cast_Object(
			VTVControlsMapper*,
			vtv->GetSubsystem(VTV::ControlsMapperSubsystem)
		);
	Check(controls);
	Vector3D power_demand = controls->powerDemand;
	

	//
	//-------------------------------------------------------------------
	// Rotate the engines so that they can match to power demand requests
	//-------------------------------------------------------------------
	//
	Scalar desire,new_rad;
	if (
		controls->controlMode == VTVControlsMapper::BasicMode
		 && Small_Enough(vtv->localVelocity.linearMotion.z, 1.0f)
	)
	{
		desire = 0.0f;
	}
	else if (Small_Enough(power_demand.z) && Small_Enough(power_demand.y))
	{
		desire = 0.0f;
	}
	else
	{
		desire = Arctan(power_demand.z, power_demand.y);
	}

	Check(thrusterPosition);
	if (thrusterPosition->GetRadians() < desire)
	{
		new_rad =
			thrusterPosition->GetRadians()
			 + time_slice * maxThrusterRotationRate;
		Max_Clamp(new_rad, desire);
		vtv->thrusterAngle = new_rad;
	}
	else if (thrusterPosition->GetRadians() > desire)
	{
		new_rad =
			thrusterPosition->GetRadians()
			 - time_slice * maxThrusterRotationRate;
		Min_Clamp(new_rad, desire);
		vtv->thrusterAngle = new_rad;
	}
	thrusterPosition->SetRotation(vtv->thrusterAngle);

	if (!Close_Enough(desire, updatedPosition, 10.0f*RAD_PER_DEG))
	{
		ForceUpdate();
		updatedPosition = desire;
	}

	//
	//-------------------------------------------------------------------
	// Calculate the vertical thrust based upon the height of each engine
	//-------------------------------------------------------------------
	//
	Verify(vtv->GetMoverCollisionRoot());
	BoundingBoxTreeNode *tree = vtv->GetMoverCollisionRoot();

	currentHeight = 0.0f;
	Point3D test_point;
	test_point.Multiply(thrusterOffset, vtv->localToWorld);
	tree->FindBoundingBoxUnder(test_point, &currentHeight);

	Vector3D force;
	force.Multiply(power_demand, currentAcceleration);
	if (power_demand.y > SMALL)
	{
		Vector3D ge = Vector3D::Identity;
		ge.y =
			vtv->groundEffectRange * power_demand.y * powerScale
			/ (1.0f + vtv->groundEffectDomainSquared*currentHeight*currentHeight);
		Check_Fpu();
		Vector3D local_ge;
		local_ge.MultiplyByInverse(ge, vtv->localToWorld);
		force += local_ge;
	}
#if defined(WOOBLY)
	vtv->ApplyLocalAcceleration(force, momentArm);
#else
	vtv->localAcceleration.linearMotion += force;
#endif
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Thruster::SlaveThruster(Scalar)
{
	Check(this);

	//
	//--------------------------------------------------------------------------
	// Get pointers to the other subsystems we will have to deal with inside the
	// VTV
	//--------------------------------------------------------------------------
	//
	VTV* vtv = GetEntity();
	Check(vtv);
	if (vtv->GetSimulationState() == VTV::BurningState)
	{
		Check_Fpu();
		return;
	}

	VTVControlsMapper* controls =
		Cast_Object(
			VTVControlsMapper*,
			vtv->GetSubsystem(VTV::ControlsMapperSubsystem)
		);
	Check(controls);
	Vector3D power_demand = controls->powerDemand;
	

	//
	//-------------------------------------------------------------------
	// Rotate the engines so that they can match to power demand requests
	//-------------------------------------------------------------------
	//
	thrusterPosition->SetRotation(vtv->thrusterAngle);

	//
	//-------------------------------------------------------------------
	// Calculate the vertical thrust based upon the height of each engine
	//-------------------------------------------------------------------
	//
	Verify(vtv->GetMoverCollisionRoot());
	BoundingBoxTreeNode *tree = vtv->GetMoverCollisionRoot();

	currentHeight = 0.0f;
	Point3D test_point;
	test_point.Multiply(thrusterOffset, vtv->localToWorld);
	tree->FindBoundingBoxUnder(test_point, &currentHeight);

	Vector3D force;
	force.Multiply(power_demand, currentAcceleration);
	if (power_demand.y > SMALL)
	{
		Vector3D ge = Vector3D::Identity;
		ge.y =
			vtv->groundEffectRange * power_demand.y * powerScale
			/ (1.0f + vtv->groundEffectDomainSquared*currentHeight*currentHeight);
		Check_Fpu();
		Vector3D local_ge;
		local_ge.MultiplyByInverse(ge, vtv->localToWorld);
		force += local_ge;
	}
#if defined(WOOBLY)
	vtv->ApplyLocalAcceleration(force, momentArm);
#else
	vtv->localAcceleration.linearMotion += force;
#endif
	Check_Fpu();
}

//#############################################################################
// Model Support
//
void
	Thruster::PrimeDeadReckon(Scalar time_slice)
{
	Check(this);

	//
	//--------------------------------------------------------------------------
	// Get pointers to the other subsystems we will have to deal with inside the
	// VTV
	//--------------------------------------------------------------------------
	//
	VTV* vtv = GetEntity();
	Check(vtv);
	if (vtv->GetSimulationState() == VTV::BurningState)
	{
		Check_Fpu();
		return;
	}

	Check(thrusterPosition);
	Scalar new_rad;
	if (thrusterPosition->GetRadians() < updatedPosition)
	{
		new_rad =
			thrusterPosition->GetRadians()
			 + time_slice * maxThrusterRotationRate;
		Max_Clamp(new_rad, updatedPosition);
		vtv->thrusterAngle = new_rad;
	}
	else if (thrusterPosition->GetRadians() > updatedPosition)
	{
		new_rad =
			thrusterPosition->GetRadians()
			 - time_slice * maxThrusterRotationRate;
		Min_Clamp(new_rad, updatedPosition);
		vtv->thrusterAngle = new_rad;
	}
	thrusterPosition->SetRotation(vtv->thrusterAngle);
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Thruster::SlaveDeadReckon(Scalar)
{
	Check(this);

	//
	//--------------------------------------------------------------------------
	// Get pointers to the other subsystems we will have to deal with inside the
	// VTV
	//--------------------------------------------------------------------------
	//
	VTV* vtv = GetEntity();
	Check(vtv);
	if (vtv->GetSimulationState() == VTV::BurningState)
	{
		Check_Fpu();
		return;
	}

	thrusterPosition->SetRotation(vtv->thrusterAngle);
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Thruster::ReadUpdateRecord(Simulation::UpdateRecord *message)
{
	Check(this);
	Check_Pointer(message);
	Subsystem::ReadUpdateRecord(message);
	UpdateRecord* record = (UpdateRecord*) message;

	updatedPosition = record->thrusterRotation;

	Check_Fpu();
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	Thruster::WriteUpdateRecord(Simulation::UpdateRecord *record, int update_model)
{
	Check(this);
	Check_Pointer(record);

	Subsystem::WriteUpdateRecord(record, update_model);
	UpdateRecord* update = (UpdateRecord*) record;

	update->recordLength = sizeof(*update);
	update->thrusterRotation = updatedPosition;
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Thruster::Thruster(
	VTV *owner,
	int subsystem_ID,
	SubsystemResource *subsystem_resource
):
	Subsystem(owner, subsystem_ID, subsystem_resource, DefaultData)
{
	if (owner->GetInstance() != VTV::ReplicantInstance)
	{
		if (subsystem_resource->primeThruster)
		{
			SetPerformance(&Thruster::PrimeThruster);
		}
		else
		{
			SetPerformance(&Thruster::SlaveThruster);
		}
	}
	else if (subsystem_resource->primeThruster)
	{
		SetPerformance(&Thruster::PrimeDeadReckon);
	}
	else
	{
		SetPerformance(&Thruster::SlaveDeadReckon);
	}

	//
	//		Get the offset from the linear matrix by peeliong
	//  	off the translation part
	//
	EntitySegment *segment = owner->GetSegment(segmentIndex);
	momentArm = thrusterOffset = segment->GetBaseOffset();

	thrusterOffset.y = owner->GetCollisionTemplate()->minY;
	momentArm.y = 0.0f;
	momentArmLength = momentArm.Length();
	powerScale = 0.0f;
	//
	//	Get the thruster position from the joint subsystem of the VTV
	//
	JointSubsystem *joint_subsystem = owner->GetJointSubsystem();
	Check(joint_subsystem);

	thrusterPosition = joint_subsystem->GetJoint(subsystem_resource->jointIndex);

	currentAcceleration = 0.0f;
	currentHeight = 0.0f;
	maxThrusterRotationRate = subsystem_resource->maxThrusterRotationRate;
	updatedPosition = 0.0f;
	Check_Fpu();
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Thruster::~Thruster()
{
	Check(this);
	Check_Fpu();
}

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