//===========================================================================//
// File:	gyro.cpp                                                            //
// Project: BattleTech           Brick: Mech subsystems                       //
// Contents: Gyroscope -- eye/body stabilisation                             //
//---------------------------------------------------------------------------//
// 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(GYRO_HPP)
#	include <gyro.hpp>
#endif

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

#if !defined(RANDOM_HPP)
#	include <random.hpp>
#endif

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

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

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

	exageration				= r->exageration;
	maxAnimationNoise		= r->maxAnimationNoise;
	minAnimationNoise		= r->minAnimationNoise;
	rotationPerSecond		= r->rotationPerSecond;
	percentageOnNormal		= r->percentageOnNormal;
	percentageOnDestruction	= r->percentageOnDestruction;
	percentageOnDegradation	= r->percentageOnDegradation;
	percentageOnFailure		= r->percentageOnFailure;

	springConstant			= r->springConstant;
	dampingConstant			= r->dampingConstant;
	posSpring				= r->posSpring;
	negSpring				= r->negSpring;
	rotationSpringConstant	= r->rotationSpringConstant;
	rotationDampingConstant	= r->rotationDampingConstant;
	rotationPosSpring		= r->rotationPosSpring;
	rotationNegSpring		= r->rotationNegSpring;

	damageMultiplier[0]		= r->collisionDamageMultiplier;
	damageMultiplier[1]		= r->ballisticDamageMultiplier;
	damageMultiplier[2]		= r->explosiveDamageMultiplier;
	damageMultiplier[3]		= r->laserDamageMultiplier;
	damageMultiplier[4]		= r->energyDamageMultiplier;

	damageResponse[0]		= r->collisionDamageResponse;
	damageResponse[1]		= r->ballisticDamageResponse;
	damageResponse[2]		= r->explosiveDamageResponse;
	damageResponse[3]		= r->laserDamageResponse;
	damageResponse[4]		= r->energyDamageResponse;

	//
	// The clamps are ctor-time COPIES of the spring pairs; the body pair is
	// doubled.  Every accumulator zeroes -- the binary leaves nothing to
	// allocator fill (the donor's history records a NaN poison from exactly
	// that omission).
	//
	eyeClampUpper = posSpring;
	eyeClampLower = negSpring;
	bodyClampUpper.Multiply(rotationPosSpring, 2.0f);
	bodyClampLower.Multiply(rotationNegSpring, 2.0f);

	{
		Vector3D
			vzero(0.0f, 0.0f, 0.0f);
		eyePosition = eyeForce = eyeVelocity = eyeWork = vzero;
		bodyOrientation = bodyForce = bodyVelocity = bodyWork = vzero;
		placePos = placeRot = vzero;
		placeQuat[0] = placeQuat[1] = placeQuat[2] = 0.0f;
		placeQuat[3] = 1.0f;

		int i;
		for (i = 0; i < 12; ++i)
		{
			workMatrix[i] = 0.0f;
		}
		workMatrix[0] = workMatrix[5] = workMatrix[10] = 1.0f;

		spare0 = 0.0f;
		externalPitchPtr = &spare0;
	}
	vibrationDirection	= Vector3D(0.0f, 1.0f, 0.0f);
	swayAngle			= 0.0f;
	swayVelocity		= 0.0f;
	swayActive			= 0;
	swayBias			= 0.0f;

	eyeJointNode		= owner->ResolveJoint(r->eyeJoint);
	mechJointNode		= owner->ResolveJoint(r->mechJoint);

	//
	// The master instance runs the gyro per-frame.
	//
	if (owner->GetInstance() != Entity::ReplicantInstance)
	{
		SetPerformance(&Gyroscope::GyroscopeSimulation);
	}

	Check_Fpu();
}

Gyroscope::~Gyroscope()
{
}

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

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

void
	Gyroscope::ResetToInitialState()
{
	Check(this);
	{
		Vector3D
			vzero(0.0f, 0.0f, 0.0f);
		eyePosition = eyeForce = eyeVelocity = eyeWork = vzero;
		bodyOrientation = bodyForce = bodyVelocity = bodyWork = vzero;
		swayAngle = 0.0f;
	}
}

//
//#############################################################################
// @004b275c -- the per-frame gyro.  Chain the power watch, slew the sway
// angle toward the powered or impaired percentage, and run both spring
// integrators.  THE PERFORMANCE ENDS THERE: the joint writes are called from
// the MECH master performance tail after the gait, not from here.
//#############################################################################
//
void
	Gyroscope::GyroscopeSimulation(Scalar time_slice)
{
	Check(this);

	PowerWatcher::Simulation(time_slice);

	//
	// The impaired test reads the gyro's OWN mirrored alarms -- the two
	// UpdateWatch just drove: the voltage watchdog and the heat watch.
	//
	Logical
		impaired =
			watchdogAlarm.GetLevel() != PoweredSubsystem::Ready ||
			heatAlarm.GetLevel() == HeatSink::FailureHeat;

	Scalar
		target =
			(impaired ? percentageOnDestruction : percentageOnNormal)
			+ swayBias;

	Scalar
		step = rotationPerSecond * time_slice;
	if (target < swayAngle)
	{
		step = -step;
	}
	swayAngle += step;

	if (step <= 0.0f)
	{
		if (step < 0.0f && swayAngle < target)
		{
			swayAngle = target;
		}
	}
	else
	{
		if (swayAngle > target)
		{
			swayAngle = target;
		}
	}

	if (swayAngle > maxAnimationNoise)
	{
		swayAngle = maxAnimationNoise;
	}
	if (swayAngle < minAnimationNoise)
	{
		swayAngle = minAnimationNoise;
	}

	IntegrateEyeJoint(time_slice);
	IntegrateBody(time_slice);

	Check_Fpu();
}

//
//#############################################################################
// @004b2ec0 -- the eye TRANSLATION spring, byte-exact quirks preserved: both
// spring terms use springConstant; the damping step OVERWRITES the force
// accumulator (it carries last frame's damping + damage impulses by design);
// and the position step has NO dt.  Equilibrium is (posSpring+negSpring)/2
// per axis -- the authentic steady eye offset.
//#############################################################################
//
void
	Gyroscope::IntegrateEyeJoint(Scalar time_slice)
{
	Vector3D
		toward_negative,
		toward_positive,
		force;

	toward_negative.Subtract(eyePosition, negSpring);
	toward_positive.Subtract(eyePosition, posSpring);

	force.Multiply(springConstant, toward_negative);
	eyeForce += force;
	force.Multiply(springConstant, toward_positive);
	eyeForce += force;

	eyeWork = eyeForce;
	force.Multiply(eyeWork, time_slice);
	eyeVelocity += force;

	eyeForce.Multiply(dampingConstant, eyeVelocity);
	eyeWork = eyeForce;
	force.Multiply(eyeWork, time_slice);
	eyeVelocity += force;

	eyePosition += eyeVelocity;

	if (eyePosition.x > eyeClampUpper.x)	eyePosition.x = eyeClampUpper.x;
	if (eyePosition.y > eyeClampUpper.y)	eyePosition.y = eyeClampUpper.y;
	if (eyePosition.z > eyeClampUpper.z)	eyePosition.z = eyeClampUpper.z;
	if (eyePosition.x < eyeClampLower.x)	eyePosition.x = eyeClampLower.x;
	if (eyePosition.y < eyeClampLower.y)	eyePosition.y = eyeClampLower.y;
	if (eyePosition.z < eyeClampLower.z)	eyePosition.z = eyeClampLower.z;
}

//
//#############################################################################
// @004b30ec -- the body ROTATION spring.  Same shape with three verified
// quirks: the spring force is X/Z-CROSSED (k.x*d.z, k.y*d.y, k.z*d.x); the
// damping is componentwise UNcrossed; and the orientation step crosses AGAIN
// (x += v.z, z += v.x).
//#############################################################################
//
void
	Gyroscope::IntegrateBody(Scalar time_slice)
{
	Vector3D
		toward_negative,
		toward_positive,
		force;

	toward_negative.Subtract(bodyOrientation, rotationNegSpring);
	toward_positive.Subtract(bodyOrientation, rotationPosSpring);

	force.x = rotationSpringConstant.x * toward_negative.z;
	force.y = rotationSpringConstant.y * toward_negative.y;
	force.z = rotationSpringConstant.z * toward_negative.x;
	bodyForce += force;
	force.x = rotationSpringConstant.x * toward_positive.z;
	force.y = rotationSpringConstant.y * toward_positive.y;
	force.z = rotationSpringConstant.z * toward_positive.x;
	bodyForce += force;

	bodyWork = bodyForce;
	force.Multiply(bodyWork, time_slice);
	bodyVelocity += force;

	bodyForce.Multiply(rotationDampingConstant, bodyVelocity);
	bodyWork = bodyForce;
	force.Multiply(bodyWork, time_slice);
	bodyVelocity += force;

	bodyOrientation.x += bodyVelocity.z;
	bodyOrientation.y += bodyVelocity.y;
	bodyOrientation.z += bodyVelocity.x;

	if (bodyOrientation.y > bodyClampUpper.y)	bodyOrientation.y = bodyClampUpper.y;
	if (bodyOrientation.x > bodyClampUpper.x)	bodyOrientation.x = bodyClampUpper.x;
	if (bodyOrientation.z > bodyClampUpper.z)	bodyOrientation.z = bodyClampUpper.z;
	if (bodyOrientation.y < bodyClampLower.y)	bodyOrientation.y = bodyClampLower.y;
	if (bodyOrientation.x < bodyClampLower.x)	bodyOrientation.x = bodyClampLower.x;
	if (bodyOrientation.z < bodyClampLower.z)	bodyOrientation.z = bodyClampLower.z;
}

//
//#############################################################################
// @004b33e0 / @004b34ec -- the joint writes, called from the MECH master
// performance tail after the gait pass.
//
// WriteEyeJoint scales the EyeJoint's own channel by swayAngle -- the idle
// wander.  WriteMechJoint pushes the integrated state onto 'jointeye'
// (BallTranslation ONLY): TRANSLATION <- eyePosition, ROTATION <-
// bodyOrientation, each write gated on a real change.
//#############################################################################
//

static const Scalar
	GyroQuantiseEps = 0.0001f;

void
	Gyroscope::WriteEyeJoint()
{
	Check(this);

	if (eyeJointNode == NULL)
	{
		return;
	}

	switch (eyeJointNode->GetJointType())
	{
	case Joint::HingeXJointType:
	case Joint::HingeYJointType:
	case Joint::HingeZJointType:
		{
			Scalar
				base = (Scalar)eyeJointNode->GetRadians(),
				swayed = swayAngle * base;
			if (
				base - swayed > GyroQuantiseEps ||
				swayed - base > GyroQuantiseEps
			)
			{
				eyeJointNode->SetRotation(Radian(swayed));
			}
		}
		break;

	case Joint::BallJointType:
	case Joint::BallTranslationJointType:
		{
			EulerAngles
				base = eyeJointNode->GetEulerAngles();
			EulerAngles
				swayed(
					Radian((Scalar)base.pitch * swayAngle),
					Radian((Scalar)base.yaw * swayAngle),
					Radian((Scalar)base.roll * swayAngle));
			eyeJointNode->SetRotation(swayed);
		}
		break;

	default:
		break;
	}
}

void
	Gyroscope::WriteMechJoint()
{
	Check(this);

	if (
		mechJointNode == NULL ||
		mechJointNode->GetJointType() != Joint::BallTranslationJointType
	)
	{
		return;
	}

	{
		Point3D
			current = mechJointNode->GetTranslation();
		if (
			current.x - eyePosition.x > GyroQuantiseEps ||
			eyePosition.x - current.x > GyroQuantiseEps ||
			current.y - eyePosition.y > GyroQuantiseEps ||
			eyePosition.y - current.y > GyroQuantiseEps ||
			current.z - eyePosition.z > GyroQuantiseEps ||
			eyePosition.z - current.z > GyroQuantiseEps
		)
		{
			mechJointNode->SetTranslation(
				Point3D(eyePosition.x, eyePosition.y, eyePosition.z));
		}
	}
	{
		EulerAngles
			current = mechJointNode->GetEulerAngles();
		if (
			(Scalar)current.pitch - bodyOrientation.x > GyroQuantiseEps ||
			bodyOrientation.x - (Scalar)current.pitch > GyroQuantiseEps ||
			(Scalar)current.yaw - bodyOrientation.y > GyroQuantiseEps ||
			bodyOrientation.y - (Scalar)current.yaw > GyroQuantiseEps ||
			(Scalar)current.roll - bodyOrientation.z > GyroQuantiseEps ||
			bodyOrientation.z - (Scalar)current.roll > GyroQuantiseEps
		)
		{
			mechJointNode->SetRotation(
				EulerAngles(
					Radian(bodyOrientation.x),
					Radian(bodyOrientation.y),
					Radian(bodyOrientation.z)));
		}
	}
}

//
//#############################################################################
// The hit-bounce kicks (@004b2d8c / @004b2de4 / @004b2e50): negate the hit
// direction, scale by exageration, land in the force accumulators.  The
// torque zeroes its .y then negates the WHOLE vector; the vertical kick
// keeps only the pitch component and mirrors it into .y.
//#############################################################################
//
void
	Gyroscope::ApplyDamageImpulse(Scalar x, Scalar y, Scalar z, Scalar magnitude)
{
	Check(this);

	Vector3D
		direction(-x, -y, -z);
	magnitude *= exageration;
	direction *= magnitude;
	eyeForce += direction;
}

void
	Gyroscope::ApplyDamageTorque(Scalar x, Scalar y, Scalar z, Scalar magnitude)
{
	Check(this);

	Vector3D
		direction(-x, -y, -z);
	magnitude *= exageration;
	direction *= magnitude;
	bodyForce += direction;
	bodyForce.y = 0.0f;
	bodyForce.Negate(bodyForce);
}

void
	Gyroscope::ApplyVerticalImpulse(Scalar pitch, Scalar magnitude)
{
	Check(this);

	Vector3D
		direction(pitch, 0.0f, 0.0f);
	magnitude *= exageration;
	direction *= magnitude;
	bodyForce += direction;
	bodyForce.y = bodyForce.x;
}

//
//#############################################################################
// @004b2980 -- the damage fan-out: every non-collision hit shakes the
// cockpit.  Direction = the record's damageForce, or a random horizontal
// when it is ~zero; rotated into the yaw-only torso frame and re-normalized;
// per-type scaled amount / multiplier * response (Explosive alone reads
// burstCount); each channel clamped at 1.3 UPPER only; then the four kicks.
//#############################################################################
//
void
	Gyroscope::ApplyDamageResponse(const Damage &damage)
{
	Check(this);

	if (damage.damageAmount <= 0.0f)
	{
		return;
	}
	if (damage.damageType == Damage::CollisionDamageType)
	{
		return;
	}

	Scalar
		trans = 0.0f,
		pitch_roll = 0.0f,
		yaw = 0.0f,
		vibration = 0.0f;

	Vector3D
		direction;
	if (
		damage.damageForce.x < 1.0e-4f && damage.damageForce.x > -1.0e-4f &&
		damage.damageForce.y < 1.0e-4f && damage.damageForce.y > -1.0e-4f &&
		damage.damageForce.z < 1.0e-4f && damage.damageForce.z > -1.0e-4f
	)
	{
		direction.x = ((Scalar)Random >= 0.5f) ? (Scalar)Random : -(Scalar)Random;
		direction.z = ((Scalar)Random >= 0.5f) ? (Scalar)Random : -(Scalar)Random;
		direction.y = 0.0f;
	}
	else
	{
		direction = damage.damageForce;
	}
	direction.Normalize(direction);

	//
	// Rotate the WORLD hit direction into the yaw-only body frame (the
	// torso twist), then re-normalize.
	//
	placeRot = Vector3D(0.0f, *externalPitchPtr, 0.0f);
	placePos = Vector3D(0.0f, 0.0f, 0.0f);
	{
		AffineMatrix
			frame;
		frame = EulerAngles(
			Radian(placeRot.x), Radian(placeRot.y), Radian(placeRot.z));
		int
			i;
		for (i = 0; i < 12; ++i)
		{
			workMatrix[i] = frame.entries[i];
		}
		Vector3D
			world = direction;
		direction.x =
			world.x * workMatrix[0] + world.y * workMatrix[1] +
			world.z * workMatrix[2];
		direction.y =
			world.x * workMatrix[4] + world.y * workMatrix[5] +
			world.z * workMatrix[6];
		direction.z =
			world.x * workMatrix[8] + world.y * workMatrix[9] +
			world.z * workMatrix[10];
	}
	direction.Normalize(direction);

	switch (damage.damageType)
	{
	case Damage::BallisticDamageType:
		trans      = damage.damageAmount / damageMultiplier[1] * damageResponse[1].trans;
		pitch_roll = damage.damageAmount / damageMultiplier[1] * damageResponse[1].pitchRoll;
		yaw        = damage.damageAmount / damageMultiplier[1] * damageResponse[1].yaw;
		vibration  = damage.damageAmount / damageMultiplier[1] * damageResponse[1].vibration;
		break;

	case Damage::ExplosiveDamageType:
		trans      = (Scalar)damage.burstCount * damage.damageAmount / damageMultiplier[2] * damageResponse[2].trans;
		pitch_roll = (Scalar)damage.burstCount * damage.damageAmount / damageMultiplier[2] * damageResponse[2].pitchRoll;
		yaw        = (Scalar)damage.burstCount * damage.damageAmount / damageMultiplier[2] * damageResponse[2].yaw;
		vibration  = (Scalar)damage.burstCount * damage.damageAmount / damageMultiplier[2] * damageResponse[2].vibration;
		break;

	case Damage::LaserDamageType:
		trans      = damage.damageAmount / damageMultiplier[3] * damageResponse[3].trans;
		pitch_roll = damage.damageAmount / damageMultiplier[3] * damageResponse[3].pitchRoll;
		yaw        = damage.damageAmount / damageMultiplier[3] * damageResponse[3].yaw;
		vibration  = damage.damageAmount / damageMultiplier[3] * damageResponse[3].vibration;
		break;

	case Damage::EnergyDamageType:
		trans      = damage.damageAmount / damageMultiplier[4] * damageResponse[4].trans;
		pitch_roll = damage.damageAmount / damageMultiplier[4] * damageResponse[4].pitchRoll;
		yaw        = damage.damageAmount / damageMultiplier[4] * damageResponse[4].yaw;
		vibration  = damage.damageAmount / damageMultiplier[4] * damageResponse[4].vibration;
		break;

	default:
		break;
	}

	if (trans > 1.3f)		trans = 1.3f;
	if (pitch_roll > 1.3f)	pitch_roll = 1.3f;
	if (yaw > 1.3f)			yaw = 1.3f;
	if (vibration > 1.3f)	vibration = 1.3f;

	ApplyDamageImpulse(direction.x, direction.y, direction.z, trans);
	ApplyDamageTorque(direction.x, direction.y, direction.z, pitch_roll);
	ApplyDamageImpulse(
		vibrationDirection.x, vibrationDirection.y, vibrationDirection.z,
		vibration);
	ApplyVerticalImpulse(yaw, yaw);
}