millimeters-of-aluminum/scenes/character/zero_g_movement_component.gd

# zero_g_move_controller.gd
extends Node
class_name ZeroGMovementComponent

## References
var pawn: CharacterPawn3D

## State & Parameters
var current_grip: GripArea3D = null # Use GripArea3D type hint
var nearby_grips: Array[GripArea3D] = []

# --- Grip damping parameters ---
@export var gripping_linear_damping: float = 6.0 # How quickly velocity stops
@export var gripping_linear_kd: float = 2 * sqrt(gripping_linear_damping) # How quickly velocity stops
@export var gripping_angular_damping: float = 3.0 # How quickly spin stops
@export var gripping_orient_speed: float = 2 * sqrt(gripping_angular_damping) # How quickly pawn rotates to face grip

var _target_basis: Basis # The orientation the PD controller is currently seeking
var _manual_roll_timer: Timer
@export var manual_roll_reset_delay: float = 3.0 # Time in seconds to wait before auto-aligning
@export var manual_roll_speed: float = 2.0 # How fast (rad/s) to rotate the target

# --- Climbing parameters ---
@export var climb_speed: float = 2.0
@export var grip_handover_distance: float = 1 # How close to next grip to initiate handover
@export var climb_acceleration: float = 1.0 # How quickly pawn reaches climb_speed
@export var climb_angle_threshold_deg: float = 120.0 # How wide the forward cone is
@export var release_past_grip_threshold: float = 0.4 # How far past the grip origin before releasing
var next_grip_target: GripArea3D = null # The grip we are trying to transition to

# --- Seeking Climb State ---
var _seeking_climb_input: Vector2 = Vector2.ZERO # The move_input held when seeking started

# --- Launch Parameters ---
@export var launch_charge_rate: float = 1.5
@export var max_launch_speed: float = 4.0
var launch_direction: Vector3 = Vector3.ZERO
var launch_charge: float = 0.0

# Enum for internal state
enum MovementState {
	IDLE,
	REACHING,
	GRIPPING,
	CLIMBING,
	SEEKING_CLIMB,
	CHARGING_LAUNCH
}
var movement_state: MovementState = MovementState.IDLE:
	set(new_state):
		if new_state == movement_state: return
		_on_exit_state(movement_state) # Call exit logic for old state
		movement_state = new_state
		_on_enter_state(movement_state) # Call enter logic for new state

func _ready():
	pawn = get_parent() as CharacterPawn3D
	if not pawn: printerr("ZeroGMovementComponent must be child of CharacterPawn3D")

	_manual_roll_timer = Timer.new()
	_manual_roll_timer.one_shot = true
	_manual_roll_timer.wait_time = manual_roll_reset_delay
	add_child(_manual_roll_timer)

# --- Standardized Movement API ---

## Called by Pawn when relevant state is active (e.g., GRABBING_GRIP, REACHING_MOVE)
func process_movement(physics_state: PhysicsDirectBodyState3D, move_input: Vector2, vertical_input: float, roll_input: float, reach_input: PlayerController3D.KeyInput, release_input: PlayerController3D.KeyInput):
	if not is_instance_valid(pawn): return

	_update_state(move_input, reach_input, release_input)

	match movement_state:
		MovementState.IDLE:
			_process_idle(physics_state, move_input, vertical_input, roll_input, release_input)
		MovementState.REACHING:
			_process_reaching(physics_state)
		MovementState.GRIPPING:
			_process_grip_physics(physics_state, move_input, roll_input)
		MovementState.CLIMBING:
			_process_climb_physics(physics_state, move_input)
		MovementState.SEEKING_CLIMB:
			_process_seeking_climb(physics_state, move_input)
		MovementState.CHARGING_LAUNCH:
			_process_launch_charge(physics_state, move_input, reach_input)


# === STATE MACHINE ===
func _on_enter_state(movement_state: MovementState):
	print("ZeroGMovementComponent activated for movement_state: ", MovementState.keys()[movement_state])
func _on_exit_state(movement_state: MovementState):
	print("ZeroGMovementComponent deactivated for movement_state: ", MovementState.keys()[movement_state])

	# Ensure grip is released if state changes unexpectedly
	# if movement_state == MovementState.GRIPPING:
	# 	_release_current_grip()

func _update_state(
	move_input: Vector2,
	reach_input: PlayerController3D.KeyInput,
	release_input: PlayerController3D.KeyInput,
	):
	match movement_state:
		MovementState.IDLE:
			# Already handled initiating reach in process_movement
			if reach_input.pressed or reach_input.held:
				movement_state = MovementState.REACHING
		MovementState.REACHING:
			# TODO: If reach animation completes/hand near target -> GRIPPING
			# If interact released during reach -> CANCEL -> IDLE
			if _seeking_climb_input != Vector2.ZERO:
				# We are in a "seek-climb-reach" chain. Cancel if move input stops.
				if move_input == Vector2.ZERO:
					# This will transition state to IDLE
					_cancel_reach()
			elif not (reach_input.pressed or reach_input.held):
				# This was a normal reach initiated by click. Cancel if click is released.
				_cancel_reach()

		MovementState.GRIPPING:
			# print("ZeroGMovementComponent: Gripping State Active")
			if release_input.pressed or release_input.held or not is_instance_valid(current_grip):
				_release_current_grip(move_input)
				return
			
			# Check for launch charge *before* checking for climb, as it's a more specific action.
			if (reach_input.pressed or reach_input.held) and move_input != Vector2.ZERO:
				_start_charge(move_input)
				return
			elif move_input != Vector2.ZERO and is_instance_valid(current_grip):
				movement_state = MovementState.CLIMBING
		MovementState.CLIMBING:
			if reach_input.pressed or reach_input.held:
				_start_charge(move_input)
				return
			if release_input.pressed or release_input.held or not is_instance_valid(current_grip):
				_stop_climb(true) # Release grip and stop
				return
			if move_input == Vector2.ZERO: # Player stopped giving input
				_stop_climb(false) # Stop moving, return to GRIPPING
				return
			# Continue climbing logic (finding next grip) happens in _process_climbing
		MovementState.CHARGING_LAUNCH:
			if move_input == Vector2.ZERO: # Cancel charge while holding interact
				movement_state = MovementState.GRIPPING
				print("ZeroGMovementComponent: Cancelled Launch Charge")


# === MOVEMENT PROCESSING ===
func _process_idle(_physics_state: PhysicsDirectBodyState3D, _move_input: Vector2, _vertical_input: float, _roll_input: float, _release_input: PlayerController3D.KeyInput):
	# TODO: Implement free-floating auto orientation against bulkheads to maintain orientation with ship
	pass
	
func _process_reaching(physics_state: PhysicsDirectBodyState3D):
	# TODO: Drive IK target towards current_grip.get_grip_transform().origin
	# TODO: Monitor distance / animation state
	# For now, _we just instantly grip.
	if _seeking_climb_input != Vector2.ZERO:
		_attempt_grip(physics_state, next_grip_target) # Complete the seek-reach
	else:
		_attempt_grip(physics_state, _find_best_grip())

func _process_grip_physics(physics_state: PhysicsDirectBodyState3D, _move_input: Vector2, roll_input: float):
	if not is_instance_valid(pawn) or not is_instance_valid(current_grip):
		_release_current_grip(); return
	
	# TODO: Later, replace step 2 and 3 with IK driving the hand bone to the target_transform.origin,
	# while the physics/orientation logic stops the main body's momentum.

	# --- 2. Calculate Target Transform ---	
	if not is_zero_approx(roll_input):
		# User is rolling. Stop the reset timer.
		_manual_roll_timer.stop()
		
		# Rotate the current target basis around the grip's Z-axis
		var grip_z_axis = current_grip.global_basis.z
		_target_basis = _target_basis.rotated(grip_z_axis, -roll_input * manual_roll_speed * physics_state.step)

		# Restart the timer
		_manual_roll_timer.start()
	elif _manual_roll_timer.wait_time < 0.0:
		_on_manual_roll_timeout(physics_state) # Immediate reset if delay is negative

	# --- 3. Apply Linear Force (PD Controller) ---
	physics_state.apply_central_force(_get_hold_force(physics_state))

	_apply_orientation_torque(physics_state, _target_basis)

func _process_climb_physics(physics_state: PhysicsDirectBodyState3D, move_input: Vector2):
	if not is_instance_valid(pawn) or not is_instance_valid(current_grip):
		_stop_climb(true); return

	# 1. Calculate Climb Direction: For climbing we interpret W as up from the pawns perspective instead of forward
	var climb_direction = move_input.y * physics_state.transform.basis.y + move_input.x * physics_state.transform.basis.x
	climb_direction = climb_direction.normalized()

	# 2. Find Next Grip
	next_grip_target = _find_best_grip(climb_direction, INF, climb_angle_threshold_deg)

	# 3. Check for Handover: This should be more eager to mark a new grip as current than below check is to release when climbing past
	var performed_handover = _attempt_grip(physics_state, next_grip_target)

	# 4. Check for Release Past Grip (if no handover)
	if not performed_handover:
		var current_grip_pos = current_grip.global_position
		var vector_from_grip_to_pawn = physics_state.transform.origin - current_grip_pos
		var distance_along_climb_dir = vector_from_grip_to_pawn.dot(climb_direction)
		if distance_along_climb_dir > release_past_grip_threshold: # Release threshold
			_release_current_grip(move_input)
			return # State changed to IDLE

	# 5. Apply Combined Forces for Climbing & Holding

	# --- Force 1: Positional Hold (From _process_grip_physics) ---
	# Calculate the force needed to stay at that position
	var force_hold = _get_hold_force(physics_state)

	# --- Force 2: Climbing Movement ---
	var target_velocity = climb_direction * climb_speed
	var error_vel = target_velocity - physics_state.linear_velocity
	var force_climb = error_vel * climb_acceleration # Kp = climb_acceleration
	
	# Find the part of the "hold" force that is parallel to our climb direction
	var force_hold_parallel = force_hold.project(climb_direction)
	
	# Check if that parallel part is pointing *against* our climb
	if force_hold_parallel.dot(climb_direction) < 0:
		# If it is, remove it from the hold force.
		# This leaves only the perpendicular (offset-correcting) force.
		force_hold = force_hold - force_hold_parallel

	# --- Combine and Apply ---
	# We apply *both* forces. The hold force will manage the offset,
	# while the climb force will overpower it in the climb direction.
	var total_force = force_hold + force_climb
	physics_state.apply_central_force(total_force)

	# 6. Apply Angular Force (Auto-Orient to current grip)
	var target_basis = _choose_grip_orientation(physics_state, current_grip.global_basis)
	_apply_orientation_torque(physics_state, target_basis)


func _process_seeking_climb(physics_state: PhysicsDirectBodyState3D, move_input: Vector2):
	# If the player's input has changed from what initiated the seek, cancel it.
	if not move_input.is_equal_approx(_seeking_climb_input):
		_seeking_climb_input = Vector2.ZERO # Reset for next time
		if _attempt_grip(physics_state, _find_best_grip()):
			# Successfully found and grabbed a grip. The state is now GRIPPING.
			print("Seeking Climb ended, gripped new target.")
		else:
			movement_state = MovementState.IDLE
			# No grip found. Transition to IDLE.
			print("Seeking Climb ended, no grip found. Reverting to IDLE.")

# --- Grip Helpers

## The single, authoritative function for grabbing a grip.
func _attempt_grip(physics_state: PhysicsDirectBodyState3D, target_grip: GripArea3D) -> bool:
	if not is_instance_valid(target_grip):
		return false

	if target_grip.grab(pawn):
		# Successfully grabbed the new grip.
		var old_grip = current_grip
		if is_instance_valid(old_grip) and old_grip != target_grip:
			old_grip.release(pawn)
		
		_manual_roll_timer.stop()
		_target_basis = _choose_grip_orientation(physics_state, target_grip.global_basis)
		
		current_grip = target_grip

		current_grip = target_grip
		next_grip_target = null
		_seeking_climb_input = Vector2.ZERO
		
		# If we weren't already climbing, transition to GRIPPING state.
		if movement_state != MovementState.CLIMBING:
			movement_state = MovementState.GRIPPING
		
		print("Successfully gripped: ", current_grip.get_parent().name)
		return true
	else:
		# Failed to grab the new grip.
		print("Failed to grip: ", target_grip.get_parent().name, " (likely occupied).")
		if movement_state == MovementState.CLIMBING:
			_stop_climb(false) # Stop climbing, return to gripping previous one
		return false

# --- Grip Orientation Helper ---
func _choose_grip_orientation(physics_state: PhysicsDirectBodyState3D, grip_basis: Basis) -> Basis:
	# 1. Define the two possible target orientations based on the grip.
	# Both will look away from the grip's surface (-Z).
	var look_at_dir = - grip_basis.z.normalized()
	var target_basis_up = Basis.looking_at(look_at_dir, grip_basis.y.normalized()).orthonormalized()
	var target_basis_down = Basis.looking_at(look_at_dir, -grip_basis.y.normalized()).orthonormalized()

	# 2. Get the pawn's current orientation.
	var current_basis = physics_state.transform.basis
	
	# 3. Compare which target orientation is "closer" to the current one.
	# We can do this by finding the angle of rotation needed to get from current to each target.
	# The quaternion dot product is related to the angle between orientations. A larger absolute dot product means a smaller angle.
	var dot_up = current_basis.get_rotation_quaternion().dot(target_basis_up.get_rotation_quaternion())
	var dot_down = current_basis.get_rotation_quaternion().dot(target_basis_down.get_rotation_quaternion())
	
	# We choose the basis that results in a larger absolute dot product (smaller rotational distance).
	return target_basis_up if abs(dot_up) >= abs(dot_down) else target_basis_down

# --- Grip Selection Logic ---
# Finds the best grip based on direction, distance, and angle constraints
func _find_best_grip(direction := Vector3.ZERO, max_distance_sq := INF, angle_threshold_deg := 120.0) -> GripArea3D:
	var best_grip: GripArea3D = null
	var min_dist_sq = max_distance_sq # Start checking against max allowed distance

	var use_direction_filter = direction != Vector3.ZERO
	var max_allowed_angle_rad = 0.0 # Initialize
	if use_direction_filter:
		# Calculate the maximum allowed angle deviation from the center direction
		max_allowed_angle_rad = deg_to_rad(angle_threshold_deg) / 2.0

	# Iterate through all grips detected by the pawn
	for grip in nearby_grips:
		# Basic validity checks
		if not is_instance_valid(grip) or grip == current_grip or not grip.can_grab(pawn):
			continue

		var grip_pos = grip.global_position
		# Use direction_to which automatically normalizes
		var dir_to_grip = pawn.global_position.direction_to(grip_pos)
		var dist_sq = pawn.global_position.distance_squared_to(grip_pos)

		# Check distance first
		if dist_sq >= min_dist_sq: # Use >= because we update min_dist_sq later
			continue
		
		# If using direction filter, check angle constraint
		if use_direction_filter:
			# Ensure the direction vector we compare against is normalized
			var normalized_direction = direction.normalized()
			# Calculate the dot product
			var dot = dir_to_grip.dot(normalized_direction)
			# Clamp dot product to handle potential floating-point errors outside [-1, 1]
			dot = clamp(dot, -1.0, 1.0)
			# Calculate the actual angle between the vectors in radians
			var angle_rad = acos(dot)

			# Check if the calculated angle exceeds the maximum allowed deviation
			if angle_rad > max_allowed_angle_rad:
				# print("Grip ", grip.get_parent().name, " outside cone. Angle: ", rad_to_deg(angle_rad), " > ", rad_to_deg(max_allowed_angle_rad))
				continue # Skip this grip if it's outside the cone
		
		# If it passes all filters and is closer than the previous best:
		min_dist_sq = dist_sq
		best_grip = grip

	# if is_instance_valid(best_grip):
	# 	print("Best grip found: ", best_grip.get_parent().name, " at distance squared: ", min_dist_sq)

	return best_grip

# --- Reaching Helpers ---
func _get_hold_distance() -> float:
	# Use the pawn.grip_detector.position.length() method if you prefer that:
	if is_instance_valid(pawn) and is_instance_valid(pawn.grip_detector):
		return pawn.grip_detector.position.length()
	else:
		return 0.5 # Fallback distance if detector isn't set up right

func _release_current_grip(move_input: Vector2 = Vector2.ZERO):
	if is_instance_valid(current_grip):
		current_grip.release(pawn)
		current_grip = null

	_manual_roll_timer.stop()

	# If we were climbing and are still holding a climb input, start seeking.
	if move_input != Vector2.ZERO:
		movement_state = MovementState.SEEKING_CLIMB
		_seeking_climb_input = move_input # Store the input that started the seek
		# print("ZeroGMovementComponent: Released grip, now SEEKING_CLIMB.")
	else:
		movement_state = MovementState.IDLE
		# print("ZeroGMovementComponent: Released grip, now IDLE.")


func _cancel_reach():
	# TODO: Logic to stop IK/animation if reach is cancelled mid-way
	_release_current_grip(Vector2.ZERO) # Ensure grip reference is cleared
	print("ZeroGMovementComponent: Reach cancelled.")

# --- Climbing Helpers ---
func _stop_climb(release_grip: bool):
	# print("ZeroGMoveController: Stopping Climb. Release Grip: ", release_grip)
	# TODO: Implement using forces
	# pawn.velocity = pawn.velocity.lerp(Vector3.ZERO, 0.5) # Apply some braking
	next_grip_target = null
	if release_grip:
		_release_current_grip() # Transitions to IDLE
	else:
		movement_state = MovementState.GRIPPING # Go back to stationary gripping

func _apply_orientation_torque(physics_state: PhysicsDirectBodyState3D, target_basis: Basis):
	var torque = MotionUtils.calculate_pd_rotation_torque(
		target_basis,
		physics_state.transform.basis,
		physics_state.angular_velocity, # Use angular_velocity (from RigidBody3D)
		gripping_orient_speed, # Kp
		gripping_angular_damping # Kd
	)
	
	physics_state.apply_torque(torque)

# --- Launch helpers ---
func _start_charge(move_input: Vector2):
	if not is_instance_valid(current_grip): return # Safety check
	movement_state = MovementState.CHARGING_LAUNCH
	launch_charge = 0.0

	# Calculate launch direction based on input and push-off normal
	# The direction is based on the pawn's current orientation, not the camera or grip.
	# This makes it feel like you're pushing off in a direction relative to your body.
	var pawn_up = pawn.global_basis.y
	var pawn_right = pawn.global_basis.x
	launch_direction = (pawn_up * move_input.y + pawn_right * move_input.x).normalized()

	print("ZeroGMovementComponent: Charging Launch")


func _process_launch_charge(physics_state: PhysicsDirectBodyState3D, move_input: Vector2, reach_input: PlayerController3D.KeyInput):
	if not (reach_input.pressed or reach_input.held):
		_execute_launch(physics_state, move_input)
	
	# hold on to current grip
	physics_state.apply_central_force(_get_hold_force(physics_state))

	launch_charge = min(launch_charge + launch_charge_rate * physics_state.step, max_launch_speed)

func _execute_launch(physics_state: PhysicsDirectBodyState3D, move_input: Vector2):
	if not is_instance_valid(current_grip): return # Safety check

	_release_current_grip(move_input) # Release AFTER calculating direction
	physics_state.apply_impulse(launch_direction * launch_charge)
	launch_charge = 0.0
	
	# Instead of going to IDLE, go to SEEKING_CLIMB to find the next grip.
	# The move_input that started the launch is what we'll use for the seek direction.
	# _seeking_climb_input = (pawn.global_basis.y.dot(launch_direction) * Vector2.UP) + (pawn.global_basis.x.dot(launch_direction) * Vector2.RIGHT)
	# movement_state = MovementState.SEEKING_CLIMB
	print("ZeroGMovementComponent: Launched with speed ", physics_state.linear_velocity.length(), " and now SEEKING_CLIMB")


# --- Force Calculation Helpers ---
func _get_hold_force(state) -> Vector3:
	if not is_instance_valid(pawn) or not is_instance_valid(current_grip):
		return Vector3.ZERO
	
	var grip_base_transform = current_grip.global_transform
	var target_direction = grip_base_transform.basis.z.normalized()
	var hold_distance = _get_hold_distance()
	var target_position = grip_base_transform.origin + target_direction * hold_distance
	
	# Calculate the force needed to stay at that position
	var force_hold = MotionUtils.calculate_pd_position_force(
		target_position,
		state.transform.origin,
		state.linear_velocity,
		gripping_linear_damping, # Kp
		gripping_linear_kd # Kd
	)
	return force_hold

# --- Manual Roll Reset ---
func _on_manual_roll_timeout(physics_state: PhysicsDirectBodyState3D):
	# Timer fired. This means the user hasn't touched roll for [delay] seconds.
	# We smoothly reset the _target_basis back to the closest grip orientation.
	if is_instance_valid(current_grip):
		_target_basis = _choose_grip_orientation(physics_state, current_grip.global_basis)


# --- Signal Handlers ---
func on_grip_area_entered(area: Area3D):
	if area is GripArea3D: # Check if the entered area is actually a GripArea3D node
		var grip = area as GripArea3D
		if not grip in nearby_grips:
			nearby_grips.append(grip)
			# print("Detected nearby grip: ", grip.get_parent().name if grip.get_parent() else "UNKNOWN") # Print parent name for context

func on_grip_area_exited(area: Area3D):
	if area is GripArea3D:
		var grip = area as GripArea3D
		if grip in nearby_grips:
			nearby_grips.erase(grip)
			# print("Grip out of range: ", grip.get_parent().name if grip.get_parent() else "UNKNOWN")