millimeters-of-aluminum/src/scenes/character/eva_movement_component.gd

# eva_suit_controller.gd
extends Node # Or Node3D if thrusters need specific positions later
class_name EVAMovementComponent

## References (Set automatically in _ready)
var pawn: CharacterPawn3D

## EVA Parameters (Moved from ZeroGPawn)
@export var orientation_speed: float = 1.0 # Used for orienting body to camera
@export var linear_acceleration: float = 1.0
@export var roll_torque_acceleration: float = 0.25
@export var angular_damping: float = 0.95 # Base damping applied by pawn, suit might add more?
@export var inertia_multiplier: float = 1.0 # How much the suit adds to pawn's base inertia (placeholder)
@export var stabilization_kp: float = 5.0
@export var stabilization_kd: float = 1.0

var _auto_orient_target: Basis = Basis() # Stores the target orientation
var _is_auto_orienting: bool = false # Flag to signal the pawn
@export var auto_orient_stop_velocity_threshold: float = 0.01 # (in rad/s)

## State
var stabilization_target: Node3D = null
var stabilization_enabled: bool = false

func _ready():
	pawn = get_parent() as CharacterPawn3D
	if not pawn:
		printerr("EVAMovementComponent must be a child of a CharacterBody3D pawn.")
		return

## Called by Pawn's _integrate_forces when suit equipped
func process_eva_movement(state: PhysicsDirectBodyState3D, move_input: Vector2, vertical_input: float, roll_input: float, orienting_input: PlayerController3D.KeyInput):
	# --- 1. Handle Orient Input ---
	if orienting_input.pressed or orienting_input.held:
		_set_auto_orient_target(state)

	_process_auto_orientation(state) # [Function 2] Run the controller

	# Check if stabilization is active and handle it first
	if stabilization_enabled and is_instance_valid(stabilization_target):
		_apply_stabilization_torques(state)
	else:
		# Apply regular movement/torque only if not stabilizing
		_apply_floating_movement(state, move_input, vertical_input, roll_input)

## Called by Pawn when entering FLOATING state with suit
func on_enter_state():
	print("EVA Suit Engaged")
	# Any specific setup needed when activating the suit

## Called by Pawn when exiting FLOATING state with suit
func on_exit_state():
	print("EVA Suit Disengaged")
	# Any cleanup needed when deactivating the suit (e.g., stop thruster effects)

# --- Internal Logic ---

func _apply_floating_movement(state: PhysicsDirectBodyState3D, move_input: Vector2, vertical_input: float, roll_input: float):
	# Apply Linear Velocity
	var move_dir_horizontal = (-state.transform.basis.z * move_input.y + state.transform.basis.x * move_input.x)
	var move_dir_vertical = state.transform.basis.y * vertical_input
	var combined_move_dir = move_dir_horizontal + move_dir_vertical

	if combined_move_dir != Vector3.ZERO:
		state.apply_central_force(combined_move_dir.normalized() * linear_acceleration)
	
	# --- Apply Roll Torque ---
	# Calculate torque magnitude based on input
	if roll_input != 0.0:
		_is_auto_orienting = false # Cancel auto-orientation if rolling manually

		var roll_acceleration = state.transform.basis.z * (-roll_input) * roll_torque_acceleration

		# Apply the global torque vector using the pawn's helper function
		state.apply_torque(roll_acceleration)

func _set_auto_orient_target(state: PhysicsDirectBodyState3D):
	# Set the target to where the camera is currently looking
	var target_forward = - pawn.camera_anchor.global_basis.z # Look where camera looks
	var target_up = state.transform.basis.y
	_auto_orient_target = Basis.looking_at(target_forward, target_up)
	_is_auto_orienting = true # Start the orientation process

# --- Auto-Orientation Logic ---
func _process_auto_orientation(state: PhysicsDirectBodyState3D):
	# This function runs every physics frame
	if not _is_auto_orienting:
		return # Not orienting, do nothing

	# 2. Calculate Torque using PD Controller
	var torque = MotionUtils.calculate_pd_rotation_torque(
		_auto_orient_target,
		state.transform.basis,
		state.angular_velocity, # Read from state
		orientation_speed, # Kp
		2 * sqrt(orientation_speed) # Kd (Critically Damped)
	)

	# 2. Apply the torque to the physics state
	state.apply_torque(torque)
	
	# 3. Check for stop condition
	var ang_vel_mag = state.angular_velocity.length()
	var axis = state.angular_velocity.normalized()

	# If we are close enough AND slow enough, stop.
	if ang_vel_mag < auto_orient_stop_velocity_threshold:
		_is_auto_orienting = false
		_auto_orient_target = pawn.global_basis # Set target to current for next time

	if axis.is_normalized():
		var physics_rotation = Basis().rotated(axis, ang_vel_mag * state.step)
		
		pawn.camera_anchor.transform.basis = physics_rotation.inverse() * pawn.camera_anchor.transform.basis


# --- Add new function placeholder ---
# TODO: Implement Rotation Stabilization Logic
func _apply_stabilization_torques(_state: PhysicsDirectBodyState3D):
	if not is_instance_valid(stabilization_target):
		stabilization_enabled = false
		return

	# TODO: Get the angular velocity from suit readings
	var angular_velocity = Vector3.ZERO
	# 1. Calculate Relative Angular Velocity:
	#    - Get the target's angular velocity (if it rotates) or assume zero.
	#    - Find the difference between our angular_velocity and the target's.
	var relative_angular_velocity = angular_velocity # - target_angular_velocity (if applicable)

	# 2. Calculate Target Orientation (Optional - for full orientation lock):
	#    - Determine the desired orientation relative to the target (e.g., face towards it).
	#    - Calculate the rotational error (e.g., using Quaternions or angle differences).
	var rotational_error = Vector3.ZERO # Placeholder for difference from desired orientation

	# 3. Calculate Corrective Torque (PD Controller):
	#    - Proportional Term (based on orientation error): P = rotational_error * stabilization_kp
	#    - Derivative Term (based on relative spin): D = relative_angular_velocity * stabilization_kd
	#    - Required Torque = -(P + D) # Negative to counteract error/spin
	var required_torque = - (rotational_error * stabilization_kp + relative_angular_velocity * stabilization_kd)

	print("Applying stabilization torque: ", required_torque)
	# 4. Convert Required Torque into Thruster Actions:
	#    - This is the complex part. Need to know thruster positions, directions, and forces.
	#    - Determine which thrusters (likely RCS on the jetpack) can produce the required_torque.
	#    - Calculate firing times/intensities for those thrusters.
	#    - Apply the forces/torques (similar to how _apply_floating_movement applies roll torque).
	#    Example (highly simplified, assumes direct torque application possible):
	

# --- Old logic for feet trailing (commented out) ---
	# --- THE FIX: Adjust how target_up is calculated ---
	# Calculate velocity components relative to camera orientation
	# var _forward_velocity_component = pawn.velocity.dot(target_forward)
	# var _right_velocity_component = pawn.velocity.dot(pawn.camera_anchor.global_basis.x)

	# Only apply strong "feet trailing" if significant forward/backward movement dominates
	# and we are actually moving.
	#if abs(forward_velocity_component) > abs(right_velocity_component) * 0.5 and velocity.length_squared() > 0.1:
		#target_up = -velocity.normalized()
		## Orthogonalize to prevent basis skew
		#var target_right = target_up.cross(target_forward).normalized()
		## If vectors are parallel, cross product is zero. Fallback needed.
		#if target_right.is_zero_approx():
			#target_up = transform.basis.y # Fallback to current up
		#else:
			#target_up = target_forward.cross(target_right).normalized()
	#else:
		## If primarily strafing or stationary relative to forward,
		## maintain the current body's roll orientation (its local Y-axis).

# --- Add methods for enabling/disabling stabilization, setting target etc. ---
func set_stabilization_enabled(enable: bool):
	if enable and is_instance_valid(stabilization_target):
		stabilization_enabled = true
		print("EVA Suit Stabilization Enabled")
	else:
		stabilization_enabled = false
		print("EVA Suit Stabilization Disabled")

func set_stabilization_target(target: Node3D):
	stabilization_target = target

func toggle_stabilization():
	set_stabilization_enabled(not stabilization_enabled)