//===========================================================================//
// File:	seqctl.cpp                                                          //
// Project: BattleTech           Brick: Entity Manager                        //
// Contents: SequenceController -- the BT gait keyframe animation player       //
//           (Mech::legAnimation @0x65c / bodyAnimation @0x6bc).               //
//---------------------------------------------------------------------------//
// Copyright (C) 1995, Virtual World Entertainment, Inc. All Rights reserved //
//                      PROPRIETARY AND CONFIDENTIAL                         //
//===========================================================================//
//
// RECONSTRUCTED from the shipped binary (part_003.c): ctor @00427768,
// SelectSequence @004277a8, Advance @0042790c, Reset @004283b8, dtor @004278d4.
// This is a BT-specific keyframe player NOT present in the RP411 engine (chosen
// path A -- faithful reconstruction).  It selects a gait clip resource, walks its
// keyframes, interpolates each animated joint's rotation, writes it through the
// engine Joint API (Joint::SetHinge / SetRotation / SetTranslation -- the same
// primitives the gyro/torso use), and returns the root-translation distance
// advanced this frame (the gait's forward step, consumed by IntegrateMotion).
//
// Clip resource layout (at ResourceDescription::resourceAddress):
//   int   frameCount              (hdr[0])
//   int   jointCount              (hdr[1])
//   int   hdr[2]                  (metadata; skipped)
//   int   jointIndices[jointCount]         -- keyframe slot -> skeleton joint index
//   float frameTimes[frameCount]           -- keyframe timestamps (seconds)
//   <pose data>                            -- per frame, per joint: hinge 8B (Hinge) /
//                                             ball 0xC (EulerAngles) / balltrans 0x18
//                                             (EulerAngles + Point3D)
//   Vector3D rootTranslations[frameCount]  -- .z (== Keyframe.stride) is the forward step
//
// Engine-helper map (== the binary FUN_ addresses): joint iter -> JointSubsystem::
// GetJoint; FUN_0041cfc8 = Joint::SetHinge; FUN_0041d0a8 = SetRotation(Radian);
// FUN_0041d020 = SetRotation(EulerAngles); FUN_0041d11c = SetTranslation; FUN_0041cfa0
// = GetEulerAngles; FUN_00406fd4 = ResourceFile::SearchList; FUN_00406cd0 = Lock.
//
#include <bt.hpp>
#pragma hdrstop

#if !defined(MECH_HPP)
#	include <mech.hpp>			// Mech (owner) + SequenceController (via mechrecon.hpp)
#endif
#include <JOINT.hpp>			// Joint, JointSubsystem
#include <ROTATION.hpp>			// EulerAngles, Radian, Hinge, Quaternion
#include <POINT3D.hpp>			// Point3D
#if !defined(APP_HPP)
#	include <app.hpp>			// application, ResourceFile, ResourceDescription
#endif

// Animation clip resource type (CLAUDE.md: AnimationResourceType == 16 / 0x10).
enum { AnimationResourceType = 16 };

// _DAT_00427d88 etc. -- the per-channel "changed enough to re-write" epsilon.
static const Scalar QuantiseEps = 0.0001f;		// _DAT_004xxxxx

//---------------------------------------------------------------------------//
// Local helpers -- resolve the Nth animated joint + read the clip's keyframe
// pose entries.  The keyframe cursor walks a packed pose stream (per the DOF
// sizes above); Scalar-typed for the pointer arithmetic.
//---------------------------------------------------------------------------//
static Joint *
	SeqJoint(void *joint_subsystem, int joint_index)
{
	return (joint_subsystem != 0)
		? ((JointSubsystem *)joint_subsystem)->GetJoint(joint_index)
		: 0;
}

// Per-frame pose byte size for a joint type (hinge<3: 8, ball==4: 0xC, bt==5: 0x18).
static int
	PoseSize(int joint_type)
{
	if (joint_type < 3)		return 8;
	if (joint_type == 4)	return 0xC;
	if (joint_type == 5)	return 0x18;
	return 0;
}

//###########################################################################
//					Init  (ctor logic @00427768)
//###########################################################################
//
// Store the owner and resolve its joint subsystem (the binary resolves owner+0x31c
// == JointedMover::jointSubsystem).  Called from the Mech ctor for legAnimation /
// bodyAnimation (they are embedded, default-constructed members).
//
void
	SequenceController::Init(Mech *owner_mech)
{
	Check(owner_mech);
	owner          = owner_mech;
	jointSubsystem = owner_mech->GetJointSubsystem();		// FUN_00417ab4(owner+0x31c)
	clipResource   = 0;
}

//###########################################################################
//					~SequenceController  (@004278d4)
//###########################################################################
//
// Release the locked clip resource (the binary decrements its ref count).
//
SequenceController::~SequenceController()
{
	if (clipResource != 0)
	{
		((ResourceDescription *)clipResource)->Unlock();
		clipResource = 0;
	}
}

//###########################################################################
//					SelectSequence  (@004277a8)
//###########################################################################
//
// Select a gait clip: store the finished-callback, reset playback, release the old
// clip, find + lock the new clip resource and parse its keyframe layout (frame/joint
// counts, joint-index map, frame times, pose-data base, and the root-translation
// table used for the forward step + by mech3's MeasureClipStride/LoadLocomotionClips).
//
void
	SequenceController::SelectSequence(int clip_id, void *cb, unsigned a2, unsigned a3)
{
	cbCode = cb; cbArg2 = a2; cbArg3 = a3;		// finished-callback @0x48/0x4c/0x50
	currentTime  = 0.0f;						// @0x54
	currentFrame = 0;							// @0x3c

	// release the previously-selected clip
	if (clipResource != 0)
	{
		((ResourceDescription *)clipResource)->Unlock();
		clipResource = 0;
	}

	ResourceFile *rf = application->GetResourceFile();
	Check(rf);
	// Direct by-ID fetch (clip_id is already the resolved animation resource ID from
	// Mech::ResolveAnimationClip).  This mirrors the engine's own clip load
	// AnimationInstance::SetAnimation (JMOVER.cpp:1406) EXACTLY -- FindResourceDescription
	// then Lock then read resourceAddress.  (NOT SearchList: that treats its arg as a
	// resource LIST and walks the clip bytes as garbage IDs -> crash.)
	ResourceDescription *desc = rf->FindResourceDescription(clip_id);
	clipResource = desc;
	if (desc == 0)								// absent clip -> empty (guarded playback)
	{
		keyframeCount = 0; jointCount = 0;
		return;
	}
	desc->Lock();								// FUN_00406cd0 (load-on-first-lock)

	int *hdr = (int *)desc->resourceAddress;	// resource data (binary: *(desc+0x3c))
	keyframeCount = hdr[0];						// @0x14 frameCount
	jointCount    = hdr[1];						// @0x18 jointCount

	int *p = hdr + 3;							// jointIndices follow hdr[0..2]
	jointIndices  = p;							// @0x1c (jointCount ints)
	p += jointCount;
	keyframeTimes = (Scalar *)p;				// @0x24 frameTimes[frameCount]
	p += keyframeCount;
	keyframeBase   = p;							// @0x28 pose-data base
	keyframeCursor = p;							// @0x2c current pose cursor

	// rootTranslations (@0x34) sit past all the packed per-frame pose data; walk the
	// DOF sizes to locate them (binary SelectSequence 6673-6693).
	char *kf = (char *)p;
	for (int f = 0; f < keyframeCount; ++f)
		for (int j = 0; j < jointCount; ++j)
		{
			Joint *jt = SeqJoint(jointSubsystem, jointIndices[j]);
			kf += PoseSize(jt ? (int)jt->GetJointType() : 0);
		}
	keyframeData = (Keyframe *)kf;				// @0x34 rootTranslations[] (.z == stride)

	Check_Fpu();
}

//###########################################################################
//					Advance  (@0042790c)
//###########################################################################
//
// Advance playback by time_slice.  Snap through every keyframe whose timestamp has
// passed (writing each animated joint's pose when move_joints), then interpolate the
// partial frame.  Returns the forward distance covered = sum over the advanced span
// of (dt * rootTranslation.z).  At end-of-clip the finished-callback re-arms the clip
// (loop) and its carryover distance is folded in.
//
Scalar
	SequenceController::Advance(Scalar time_slice, int move_joints)
{
	Scalar t        = currentTime + time_slice;		// local_c
	Scalar distance = 0.0f;							// local_10
	char  *cursor   = (char *)keyframeCursor;		// local_8 = *(this+0x2c)

	// --- snap through the whole keyframes reached this frame ---
	while (currentFrame < keyframeCount && keyframeTimes[currentFrame] <= t)
	{
		for (int j = 0; j < jointCount; ++j)
		{
			Joint *jt = SeqJoint(jointSubsystem, jointIndices[j]);
			int    ty = jt ? (int)jt->GetJointType() : 0;
			if (ty < 3)								// hinge
			{
				// task #59 discriminator (BT_HIP_LOG): log hinge writes so the
				// jointhip walk-lean can be seen.  EXTERIOR clips ramp jointhip
				// to ~-8deg on stand->walk and hold it; INTERIOR ('i') clips
				// never bind jointhip, so it stays silent.  Sampled by joint
				// slot so the stream stays readable.
				if (jt && move_joints)
				{
					static const int s_hipLog = getenv("BT_HIP_LOG") ? 1 : 0;
					if (s_hipLog)
					{
						const Radian ang = ((const Hinge *)cursor)->rotationAmount;
						if (fabsf((float)ang) > 0.02f)		// only the leaning ones
							DEBUG_STREAM << "[hip] jointIdx=" << jointIndices[j]
								<< " deg=" << ((float)ang * 57.2958f) << "\n" << std::flush;
					}
				}
				if (jt && move_joints)	jt->SetHinge(*(const Hinge *)cursor);		// FUN_0041cfc8
				cursor += 8;
			}
			else if (ty == 4)						// ball
			{
				if (jt && move_joints)	jt->SetRotation(*(const EulerAngles *)cursor);	// FUN_0041d020
				cursor += 0xC;
			}
			else if (ty == 5)						// ball + translation
			{
				if (jt && move_joints)	jt->SetRotation(*(const EulerAngles *)cursor);
				cursor += 0xC;
				if (jt && move_joints)	jt->SetTranslation(*(const Point3D *)cursor);	// FUN_0041d11c
				cursor += 0xC;
			}
		}
		distance    += (keyframeTimes[currentFrame] - currentTime) * keyframeData[currentFrame].stride;
		currentTime  = keyframeTimes[currentFrame];
		++currentFrame;
	}

	if (currentFrame == keyframeCount)
	{
		// clip finished: invoke the stored finished-callback @0x48 EXACTLY as the binary
		// (FUN_0042790c:6815): (owner, cbArg2, carryover, move_joints) -> distance, folded in.
		// The callback (Mech::BodyClipFinished == FUN_004a6d8c) picks the next gait state,
		// re-arms THIS controller via SetBodyAnimation (SelectSequence resets it to frame 0),
		// and recursively advances the carryover -- so on return our state is already the next
		// clip's.  A null callback (leg channel during load) just leaves the clip at its end.
		if (cbCode != 0 && keyframeCount > 0)
		{
			Scalar carryover = t - keyframeTimes[keyframeCount - 1];
			typedef Scalar (*FinishedCallback)(Mech *, unsigned, Scalar, int);
			distance += ((FinishedCallback)cbCode)(owner, cbArg2, carryover, move_joints);
		}
	}
	else
	{
		// partial frame: interpolate each joint from its current pose toward the next
		// keyframe by the fractional ratio, and advance the cursor to that keyframe.
		keyframeCursor = cursor;
		Scalar span  = keyframeTimes[currentFrame] - currentTime;
		Scalar ratio = (span > 0.0f) ? (t - currentTime) / span : 0.0f;
		for (int j = 0; j < jointCount; ++j)
		{
			Joint *jt = SeqJoint(jointSubsystem, jointIndices[j]);
			int    ty = jt ? (int)jt->GetJointType() : 0;
			if (ty < 3)								// hinge: lerp the scalar angle
			{
				const Hinge *target = (const Hinge *)cursor;
				if (jt && move_joints)
				{
					Scalar a = (Scalar)jt->GetRadians();
					Scalar b = (Scalar)target->rotationAmount;
					jt->SetRotation(Radian(a + (b - a) * ratio));	// FUN_0041d0a8
				}
				cursor += 8;
			}
			else if (ty == 4)						// ball: lerp the euler angles
			{
				const EulerAngles *target = (const EulerAngles *)cursor;
				if (jt && move_joints)
				{
					EulerAngles a = jt->GetEulerAngles();
					EulerAngles out(
						(Scalar)a.pitch + ((Scalar)target->pitch - (Scalar)a.pitch) * ratio,
						(Scalar)a.yaw   + ((Scalar)target->yaw   - (Scalar)a.yaw)   * ratio,
						(Scalar)a.roll  + ((Scalar)target->roll  - (Scalar)a.roll)  * ratio);
					jt->SetRotation(out);							// FUN_0041d020
				}
				cursor += 0xC;
			}
			else if (ty == 5)						// ball + translation: lerp both
			{
				const EulerAngles *tRot = (const EulerAngles *)cursor;
				if (jt && move_joints)
				{
					EulerAngles a = jt->GetEulerAngles();
					EulerAngles out(
						(Scalar)a.pitch + ((Scalar)tRot->pitch - (Scalar)a.pitch) * ratio,
						(Scalar)a.yaw   + ((Scalar)tRot->yaw   - (Scalar)a.yaw)   * ratio,
						(Scalar)a.roll  + ((Scalar)tRot->roll  - (Scalar)a.roll)  * ratio);
					jt->SetRotation(out);
				}
				cursor += 0xC;
				const Point3D *tPos = (const Point3D *)cursor;
				if (jt && move_joints)
				{
					Point3D c = jt->GetTranslation();
					Point3D out(c.x + (tPos->x - c.x) * ratio,
								c.y + (tPos->y - c.y) * ratio,
								c.z + (tPos->z - c.z) * ratio);
					jt->SetTranslation(out);						// FUN_0041d11c
				}
				cursor += 0xC;
			}
		}
		distance   += (t - currentTime) * keyframeData[currentFrame].stride;
		currentTime = t;
	}

	Check_Fpu();
	return distance;
}

//###########################################################################
//					Reset  (@004283b8)
//###########################################################################
//
// Reset every animated joint to its neutral pose (identity rotation / zero
// translation), per joint type.  (Binary: SetHinge/SetRotation/SetTranslation from a
// default pose block; the neutral pose is the engine identity for each channel.)
//
void
	SequenceController::Reset(int loop)
{
	if (loop == 0)
		return;

	EulerAngles	zeroEuler(Radian(0.0f), Radian(0.0f), Radian(0.0f));
	Point3D		zeroPoint(0.0f, 0.0f, 0.0f);

	for (int j = 0; j < jointCount; ++j)
	{
		Joint *jt = SeqJoint(jointSubsystem, jointIndices[j]);
		if (jt == 0)
			continue;
		switch (jt->GetJointType())
		{
		case Joint::HingeXJointType:
		case Joint::HingeYJointType:
		case Joint::HingeZJointType:	jt->SetRotation(Radian(0.0f));	break;	// neutral hinge angle
		case Joint::BallJointType:		jt->SetRotation(zeroEuler);		break;	// FUN_0041d020
		case Joint::BallTranslationJointType:
			jt->SetRotation(zeroEuler);								// FUN_0041d020
			jt->SetTranslation(zeroPoint);							// FUN_0041d11c
			break;
		default:						break;
		}
	}
	Check_Fpu();
}