millimeters-of-aluminum/scenes/tests/3d/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: CharacterBody3D
var camera_pivot: Node3D
var camera: Camera3D

## EVA Parameters (Moved from ZeroGPawn)
@export var orientation_speed: float = 2.0 # Used for orienting body to camera
@export var move_speed: float = 2.0
@export var roll_torque: float = 2.5
@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

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

func _ready():
	pawn = get_parent() as CharacterBody3D
	if not pawn:
		printerr("EVAMovementComponent must be a child of a CharacterBody3D pawn.")
		return
	# Make sure the paths match your CharacterPawn scene structure
	camera_pivot = pawn.get_node_or_null("CameraPivot")
	if camera_pivot:
		camera = camera_pivot.get_node_or_null("SpringArm/Camera3D") # Adjusted path for SpringArm

	if not camera_pivot or not camera:
		printerr("EVAMovementComponent could not find CameraPivot/SpringArm/Camera3D on pawn.")

# --- Standardized Movement API ---

## Called by Pawn's _physics_process when in FLOATING state with suit equipped
func process_movement(delta: float, move_input: Vector2, vertical_input: float, roll_input: float, orienting_input: PlayerController3D.KeyInput):
	var orienting = orienting_input.held
	if not is_instance_valid(pawn) or not camera: return
	if orienting:
		_orient_pawn(delta)

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

func apply_thrusters(pawn: CharacterPawn3D, delta: float, move_input: Vector2, vertical_input: float, roll_input: float):
	if not is_instance_valid(pawn): return
	
	# Apply Linear Velocity
	var pawn_forward = -pawn.global_basis.z
	var pawn_right = pawn.global_basis.x
	var pawn_up = pawn.global_basis.y
	var move_dir_horizontal = (pawn_forward * move_input.y + pawn_right * move_input.x)
	var move_dir_vertical = pawn_up * vertical_input
	var combined_move_dir = move_dir_horizontal + move_dir_vertical

	if combined_move_dir != Vector3.ZERO:
		pawn.velocity += combined_move_dir.normalized() * move_speed * delta

	# Apply Roll Torque
	var roll_torque_global = -pawn.global_basis.z * (roll_input) * roll_torque # Sign fixed
	pawn.add_torque(roll_torque_global, delta)

## Called by Pawn to handle collision response in FLOATING state
func handle_collision(collision: KinematicCollision3D, collision_energy_loss: float):
	if not is_instance_valid(pawn): return
	var surface_normal = collision.get_normal()
	var reflected_velocity = pawn.velocity.bounce(surface_normal)
	reflected_velocity *= (1.0 - collision_energy_loss)
	pawn.velocity = reflected_velocity # Update pawn's velocity directly

## 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(delta: float, move_input: Vector2, vertical_input: float, roll_input: float):
	# Apply Linear Velocity
	var pawn_forward = -pawn.global_basis.z
	var pawn_right = pawn.global_basis.x # Use pawn's right for consistency
	var pawn_up = pawn.global_basis.y
	var move_dir_horizontal = (pawn_forward * move_input.y + pawn_right * move_input.x)
	var move_dir_vertical = pawn_up * vertical_input
	var combined_move_dir = move_dir_horizontal + move_dir_vertical

	if combined_move_dir != Vector3.ZERO:
		pawn.velocity += combined_move_dir.normalized() * move_speed * delta
	# --- Apply Roll Torque ---
	# Calculate torque magnitude based on input
	var roll_torque_vector = pawn.transform.basis.z * (-roll_input) * roll_torque

	# Apply the global torque vector using the pawn's helper function
	pawn.add_torque(roll_torque_vector, delta)


# --- Auto-Orientation Logic ---
func _orient_pawn(delta: float):
	# 1. Determine Target Orientation Basis
	var initial_cam_basis = camera_pivot.global_basis
	var target_forward = -camera_pivot.global_basis.z # Look where camera looks
	var target_up = Vector3.UP # Default up initially

	# --- 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(camera_pivot.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).
	target_up = pawn.transform.basis.y

	# Create the target basis
	var target_basis = Basis.looking_at(target_forward, target_up)

	# Optional Pitch Offset (Experimental):
	# Apply the desired 70-degree pitch relative to the forward direction
	# var target_pitch_rad = deg_to_rad(target_body_pitch_degrees)
	# target_basis = target_basis.rotated(target_basis.x, target_pitch_rad) # Rotate around the target right vector
	
	# 2. Smoothly Interpolate Towards Target Basis
	var current_basis = pawn.global_basis
	var new_basis = current_basis.slerp(target_basis, delta * orientation_speed)
	
	# Store the body's yaw *before* applying the new basis
	var _old_body_yaw = current_basis.get_euler().y
	var _old_body_pitch = current_basis.get_euler().x
	
	# 3. Apply the new orientation
	pawn.global_basis = new_basis
	
	# 4. Reset camera pivot to rotation to what it was before we rotated the parent
	camera_pivot.global_basis = initial_cam_basis

# --- Add new function placeholder ---
# TODO: Implement Rotation Stabilization Logic
func _apply_stabilization_torques(_delta: float):
	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):
	
	# angular_velocity += (required_torque / inertia) * delta

# --- 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)