//===========================================================================//
// 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);


	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;
	//
	// ELEMENTS(), never a literal.  This loop said 22 while the array was
	// shrunk to 16 by carving the command members out of its front (5.3.68 /
	// 5.3.71) -- 24 bytes written past the end of every Torso, on the heap,
	// every mission.  It compiled and ran; C++ does not check.  The macro
	// makes the next carve harmless.
	//
	int
		i;
	for (i = 0; i < (int)ELEMENTS(dynamicsState); ++i)
	{
		dynamicsState[i] = 0.0f;
	}

	//
	// Twist-replication state (binary @0x218/@0x21C/@0x238/@0x254) -- see the
	// header note.  Zero until an update record stamps it.
	//
	targetTwist		= 0.0f;
	twistAtUpdate	= 0.0f;
	twistRate		= 0.0f;
	lastUpdateTime	= 0L;

	//
	// Install the per-frame Performance -- the binary's gate @004b6b0c
	// verbatim: (flags & 0xC) == 0 && (flags & 0x100) != 0, which decodes to
	// the engine's own MasterInstance + DynamicFlag (InstanceBits=2 puts the
	// instance field at mask 0xC with ReplicantInstance=4; DynamicBit=8 is
	// 0x100).  Entity::DefaultFlags = DynamicFlag|MasterInstance and the
	// engine seeds simulationFlags from the MakeMessage, so the gate is live
	// with no stream-builder dependency.  [T2 RETIRED 5.3.119.]
	//
	if (
		(owner->simulationFlags & Entity::InstanceMask) == Entity::MasterInstance &&
		(owner->simulationFlags & Entity::DynamicFlag) != 0
	)
	{
		isDamagedCopy = 0;
		SetPerformance(&Torso::TorsoSimulation);
	}
	else
	{
		isDamagedCopy = 1;
		SetPerformance(&Torso::TorsoCopySimulation);	// binary PTR @00510c1c
	}

	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;

	//
	// DEV HOOK BT_FORCE_TORSO=1: sweep the twist BUTTONS left/right without a
	// hand on the RIO.  This drives the exact members the streamed direct
	// mappings write (twistLeftCommand / twistRightCommand), so it exercises
	// the real button path end to end -- table binding, this integrator, the
	// skeleton joint, and the cockpit eye that rides the twist chain -- and
	// makes the result visible on the render bridge.  The donor carries the
	// same hook for the same reason (torso.cpp:583).
	//
	// Held for ~90 frames each way, matching the donor's period.
	//
	if (getenv("BT_FORCE_TORSO") != NULL)
	{
		static int
			sweep = 0;

		sweep++;
		twistLeftCommand  = ((sweep / 90) & 1) ? 1 : 0;
		twistRightCommand = ((sweep / 90) & 1) ? 0 : 1;
	}

	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;
			}
		}

		if (getenv("BT_TORSO_LOG") != NULL)
		{
			static int
				torso_tick = 0;

			if ((torso_tick++ % 240) == 0)
			{
				DEBUG_STREAM << "[torso] twist=" << currentTwist
					<< " elev=" << currentElevation
					<< " cmdL=" << twistLeftCommand
					<< " cmdR=" << twistRightCommand
					<< " axis=" << analogTwistAxis
					<< " rate=" << baseTwistRate
					<< " limits=(" << horizontalLimitRight
					<< ".." << horizontalLimitLeft << ")"
					<< " joint=" << (horizontalJointNode != NULL ? 1 : 0)
					<< endl << flush;
			}
		}

		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();
}

//
//#############################################################################
// ComputeTargetTwist -- binary @004b6510 (232 bytes), verified against the
// raw decomp (part_013.c).
//
// Extrapolate where the master's torso should be by now.  Two time bases:
//
//   settled  (not a copy, or the command stamp is not ahead of the clock):
//            elapsed = lastPerformance - lastUpdate     (this frame's window)
//   slewing  (a copy with a command stamp still in the future):
//            elapsed = lastUpdateTime - lastUpdate      (the command window)
//
// then targetTwist = twistAtUpdate + twistRate * elapsed, clamped to the
// twist limits (left = upper bound, right = lower -- same convention as the
// master sim).  Returns True while slewing.  Time-Time subtraction is the
// engine's operator- (ticks delta / SystemClock::ticksPerSecond) -- the
// decomp's DAT_0052140c divide is that operator inlined.
//#############################################################################
//
Logical
	Torso::ComputeTargetTwist()
{
	Check(this);

	Scalar
		elapsed;
	Logical
		slewing;

	if (isDamagedCopy == 0 || lastUpdateTime <= lastPerformance)
	{
		elapsed = lastPerformance - lastUpdate;
		slewing = False;
	}
	else
	{
		elapsed = lastUpdateTime - lastUpdate;
		slewing = True;
	}

	targetTwist = twistAtUpdate + twistRate * elapsed;

	if (targetTwist > horizontalLimitLeft)
	{
		targetTwist = horizontalLimitLeft;
	}
	if (targetTwist < horizontalLimitRight)
	{
		targetTwist = horizontalLimitRight;
	}

	return slewing;
}

//
//#############################################################################
// TorsoCopySimulation -- binary @004b65f8 (188 bytes), verified against the
// raw decomp.  The damaged-copy Performance (replicant registration in the
// ctor): no commands, no dynamics -- just ease currentTwist onto the
// extrapolated target.
//
//   settled: snap  currentTwist = targetTwist
//   slewing: ease  currentTwist = Lerp(current, target,
//                      dt / ((lastUpdateTime - lastPerformance) + dt))
//            -- the remaining-time form: the ease lands exactly when the
//            clock reaches the command stamp.
//
// The binary then re-clamps targetTwist (not currentTwist) with the same
// limit pair.  NOTE two donor drifts corrected here against the decomp: the
// ease's age term reads lastPerformance (+0x10), not lastUpdate (+0x14); and
// the binary does NOT call the joint-write helper (@004b67ec) from the copy
// sim -- that call in the BT411 port is a port-era addition, not 1995 code.
//#############################################################################
//
void
	Torso::TorsoCopySimulation(Scalar time_slice)
{
	Check(this);

	if (!ComputeTargetTwist())
	{
		currentTwist = targetTwist;
	}
	else
	{
		currentTwist =
			Lerp(
				currentTwist,
				targetTwist,
				time_slice / ((lastUpdateTime - lastPerformance) + time_slice)
			);
	}

	if (targetTwist > horizontalLimitLeft)
	{
		targetTwist = horizontalLimitLeft;
	}
	if (targetTwist < horizontalLimitRight)
	{
		targetTwist = horizontalLimitRight;
	}

	Check_Fpu();
}