//===========================================================================//
// File:	torso.cpp                                                           //
// Project: BattleTech           Brick: Mech subsystems                       //
// Contents: Torso -- torso twist / weapon elevation                        //
//---------------------------------------------------------------------------//
// Copyright (C) 1995, Virtual World Entertainment, Inc.                     //
// All Rights reserved worldwide                                             //
// This unpublished sourcecode is PROPRIETARY and CONFIDENTIAL               //
//===========================================================================//

#include <bt.hpp>
#pragma hdrstop

#if !defined(TORSO_HPP)
#	include <torso.hpp>
#endif

#if !defined(MECH_HPP)
#	include <mech.hpp>
#endif

#if !defined(JOINT_HPP)
#	include <joint.hpp>
#endif

Derivation
	Torso::ClassDerivations(
		PowerWatcher::ClassDerivations,
		"Torso"
	);

//
// The full 13-entry authentic table (ids 3..15) -- donor torso.cpp:152 with
// binary offsets, cross-confirmed by BTL4.RES's streamed control mappings
// binding ids 12/13 on subsystem 17.  The five command members and
// analogTwistAxis are DIRECT WRITE DESTINATIONS for device polls
// (L4CTRL.CPP:1936), which is why every id from 9 up must exist and be the
// right kind of storage: id 9 short of this table landed the RIO's analog
// stick on the motionState StateIndicator.
//
// SpeedOf* still point at the base rates rather than the donor's live
// velocity accumulators (elevationVelocity/twistVelocity @0x1EC/0x1E8) --
// those members arrive with TorsoSimulation; the authored audio watcher on
// SpeedOfTorsoHorizontal just reads a constant until then.
//
const Torso::IndexEntry
	Torso::AttributePointers[]=
{
	ATTRIBUTE_ENTRY(Torso, RotationOfTorsoVertical,		currentElevation),
	ATTRIBUTE_ENTRY(Torso, RotationOfTorsoHorizontal,	currentTwist),
	ATTRIBUTE_ENTRY(Torso, HorizontalLimitRight,		horizontalLimitRight),
	ATTRIBUTE_ENTRY(Torso, HorizontalLimitLeft,			horizontalLimitLeft),
	ATTRIBUTE_ENTRY(Torso, SpeedOfTorsoVertical,		baseElevationRate),
	ATTRIBUTE_ENTRY(Torso, SpeedOfTorsoHorizontal,		baseTwistRate),
	ATTRIBUTE_ENTRY(Torso, StickPosition,				analogTwistAxis),
	ATTRIBUTE_ENTRY(Torso, TorsoUp,						elevateUpCommand),
	ATTRIBUTE_ENTRY(Torso, TorsoDown,					elevateDownCommand),
	ATTRIBUTE_ENTRY(Torso, TorsoLeft,					twistLeftCommand),
	ATTRIBUTE_ENTRY(Torso, TorsoRight,					twistRightCommand),
	ATTRIBUTE_ENTRY(Torso, TorsoCenter,					centerCommand),
	ATTRIBUTE_ENTRY(Torso, MotionState,					motionState)
};

Torso::AttributeIndexSet
	Torso::AttributeIndex(
		ELEMENTS(Torso::AttributePointers),
		Torso::AttributePointers,
		MechSubsystem::AttributeIndex
	);

Torso::SharedData
	Torso::DefaultData(
		Torso::ClassDerivations,
		Subsystem::MessageHandlers,
		Torso::AttributeIndex,
		Subsystem::StateCount
	);

Torso::Torso(
	Mech				*owner,
	int					subsystem_ID,
	SubsystemResource	*r,
	SharedData			&shared_data
):
	PowerWatcher(owner, subsystem_ID, r, shared_data)
{
	Check(owner);
	Check_Pointer(r);

	isDamagedCopy	= (owner->GetInstance() == Entity::ReplicantInstance);

	statusFlags					= 0;
	buttonAccelerationPerSecond	= r->buttonAccelerationPerSecond;
	buttonAccelerationStart		= r->buttonAccelerationStartValue;

	baseTwistRate		= r->horizontalRotationPerSecond * RAD_PER_DEG;
	baseElevationRate	= r->verticalRotationPerSecond   * RAD_PER_DEG;
	horizontalLimitRight= r->horizontalLimitRight * RAD_PER_DEG;
	horizontalLimitLeft	= r->horizontalLimitLeft  * RAD_PER_DEG;
	verticalLimitTop	= r->verticalLimitTop     * RAD_PER_DEG;
	verticalLimitBottom	= r->verticalLimitBottom  * RAD_PER_DEG;

	twistCenterHigh		= verticalLimitTop;
	twistCenterLow		= verticalLimitBottom;
	elevationCenter		= verticalLimitTop;
	elevationHalfBottom	= verticalLimitBottom * 0.5f;

	buttonRampActive	= 0;
	buttonRamp			= 0.0f;
	horizontalEnabled	= r->torsoHorizontalEnabled;

	//
	// The device-poll write destinations (published ids 9..14).  Zero =
	// stick centred, no button held -- their state before the first poll.
	//
	elevateUpCommand	= 0;
	elevateDownCommand	= 0;
	twistLeftCommand	= 0;
	twistRightCommand	= 0;
	centerCommand		= 0;
	recenterActive		= 0;

	//
	// Resolve the named torso-twist skeleton joints (twist body + shadow) from
	// the mech skeleton (now live).  A mech with a fixed torso, or one whose
	// joint name is absent from the skeleton, resolves NULL and simply never
	// twists.
	//
	horizontalJointNode			= owner->ResolveJoint(r->torsoHorizontalJoint);
	horizontalShadowJointNode	= owner->ResolveJoint(r->torsoHorizontalShadowJoint);

	if (getenv("BT_MECH_LOG"))
	{
		DEBUG_STREAM << "[torso] horizJoint '" << r->torsoHorizontalJoint
			<< "' -> " << (void *)horizontalJointNode;
		if (horizontalJointNode != NULL)
		{
			DEBUG_STREAM << " type=" << (int)horizontalJointNode->GetJointType();
		}
		DEBUG_STREAM << " enabled=" << (int)horizontalEnabled << endl << flush;
	}

	currentTwist		= 0.0f;
	currentElevation	= 0.0f;
	analogElevationAxis	= 0.0f;
	analogTwistAxis		= 0.0f;
	for (int i = 0; i < 22; ++i)
	{
		dynamicsState[i] = 0.0f;
	}

	//
	// Install the per-frame twist/elevation Performance (the replicant copy is
	// driven by console updates via TorsoCopySimulation instead, still staged).
	//
	if (owner->GetInstance() != Entity::ReplicantInstance)
	{
		SetPerformance(&Torso::TorsoSimulation);
	}

	Check_Fpu();
}

Torso::~Torso()
{
}

Logical
	Torso::TestClass(Mech &)
{
	return True;
}

Logical
	Torso::TestInstance() const
{
	return IsDerivedFrom(ClassDerivations);
}

//
//#############################################################################
// TorsoSimulation -- per-frame torso twist / weapon-elevation integration.
//
// Slews the current elevation/twist toward the analog aim axes (written by
// MechControlsMapper::InterpretControls) at the resource base rates, clamped to
// the resource limits.  Authentic core (torso.cpp @004b6xxx): current += axis *
// baseRate * dt.  APPLYING the resolved angles onto the skeleton torso/gun
// joints (horizontalJointNode / the elevation joint) is deferred with the
// skeleton-link + render wave; here we advance the scalar aim state (read by the
// HUD reticle / weapon aim and, once wired, the joints).
//#############################################################################
//
void
	Torso::TorsoSimulation(Scalar time_slice)
{
	Check(this);

	//
	// The BUTTON COMMANDS (donor torso.cpp:557, binary @004b5cf0).  The five
	// command members are the streamed direct mappings' write destinations --
	// the RIO/TM twist and elevation buttons land in them as raw
	// ControlsButton ints (positive = held).  The donor's ramp machinery is
	// carried verbatim, including its punchline: the shipped build
	// unconditionally overwrites the ramp with 1.0f, so the acceleration
	// ramp is authored dead weight.  Kept because it is the binary's shape.
	//
	Scalar
		twist_step = baseTwistRate * time_slice,
		elev_step = baseElevationRate * time_slice;

	if (buttonRampActive == 0)
	{
		buttonRamp = buttonAccelerationStart;
	}
	else
	{
		buttonRamp += buttonAccelerationPerSecond * time_slice;
		if (buttonRamp > 1.0f)
		{
			buttonRamp = 1.0f;
		}
		buttonRampActive = 0;
	}
	buttonRamp = 1.0f;	// @004b5cf0 unconditionally overwrites with 1.0f

	if (elevateUpCommand > 0)
	{
		currentElevation += elev_step * buttonRamp;
		if (currentElevation > verticalLimitTop)
		{
			currentElevation = verticalLimitTop;
		}
		buttonRampActive = 1;
	}
	if (elevateDownCommand > 0)
	{
		currentElevation -= elev_step * buttonRamp;
		if (currentElevation < verticalLimitBottom)
		{
			currentElevation = verticalLimitBottom;
		}
		buttonRampActive = 1;
	}

	//
	// Elevation (weapon pitch): slew toward the analog axis, clamp to limits.
	//
	currentElevation += analogElevationAxis * baseElevationRate * time_slice;
	{
		Scalar lo = (verticalLimitBottom < verticalLimitTop)
			? verticalLimitBottom : verticalLimitTop;
		Scalar hi = (verticalLimitBottom < verticalLimitTop)
			? verticalLimitTop : verticalLimitBottom;
		if (currentElevation < lo) currentElevation = lo;
		if (currentElevation > hi) currentElevation = hi;
	}

	//
	// Twist (horizontal torso rotation): only mechs with an enabled torso joint.
	//
	if (horizontalEnabled)
	{
		//
		// Digital twist commands, then recenter, then the analog axis --
		// the donor's order.  Twist buttons cancel a pending recenter;
		// the centre button arms it and it slews home across frames
		// (donor Recenter, binary @004b6918).
		//
		if (twistLeftCommand > 0)
		{
			currentTwist += twist_step * buttonRamp;
			if (currentTwist > horizontalLimitLeft)
			{
				currentTwist = horizontalLimitLeft;
			}
			recenterActive = 0;
			buttonRampActive = 1;
		}
		if (twistRightCommand > 0)
		{
			currentTwist -= twist_step * buttonRamp;
			if (currentTwist < horizontalLimitRight)
			{
				currentTwist = horizontalLimitRight;
			}
			recenterActive = 0;
			buttonRampActive = 1;
		}
		if (centerCommand > 0)
		{
			recenterActive = 1;
			buttonRampActive = 0;
		}
		if (recenterActive != 0)
		{
			if (currentTwist > 0.0f)
			{
				currentTwist -= twist_step;
				if (currentTwist < 0.0f) currentTwist = 0.0f;
			}
			else if (currentTwist < 0.0f)
			{
				currentTwist += twist_step;
				if (currentTwist > 0.0f) currentTwist = 0.0f;
			}
			if (currentTwist == 0.0f)
			{
				recenterActive = 0;
			}
		}

		currentTwist += analogTwistAxis * baseTwistRate * time_slice;
		Scalar lo = (horizontalLimitLeft < horizontalLimitRight)
			? horizontalLimitLeft : horizontalLimitRight;
		Scalar hi = (horizontalLimitLeft < horizontalLimitRight)
			? horizontalLimitRight : horizontalLimitLeft;
		if (currentTwist < lo) currentTwist = lo;
		if (currentTwist > hi) currentTwist = hi;

		//
		// Push the twist onto the skeleton torso joint so the upper body rotates
		// on the model.  Hinge joints take the scalar angle about their axis;
		// ball joints take it as the yaw component.
		//
		if (horizontalJointNode != NULL)
		{
			Joint::JointType jt = horizontalJointNode->GetJointType();
			if (jt == Joint::HingeXJointType
				|| jt == Joint::HingeYJointType
				|| jt == Joint::HingeZJointType)
			{
				horizontalJointNode->SetRotation(Radian(currentTwist));
			}
			else if (jt == Joint::BallJointType)
			{
				horizontalJointNode->SetRotation(EulerAngles(0.0f, currentTwist, 0.0f));
			}
		}
	}

	Check_Fpu();
}

void
	Torso::TorsoCopySimulation(Scalar)
{
	Fail("Torso::TorsoCopySimulation -- torso.cpp not yet reconstructed");
}