extends Node
class_name StateGrapple

## Swing grapple with active rope control:
##   - the rope acts as a hard pendulum constraint at its current length
##   - holding forward reels in (rope shortens, converts to speed)
##   - holding back pays rope out (up to the original latch length)
##   - strafe input steers tangentially around the swing sphere
##   - jump detaches with a boost along your current motion

var machine: MovementStateMachine
var params: MovementParams:
	get: return machine.params

var _rope_length: float = 0.0


func enter(_data: Dictionary = {}) -> void:
	machine.on_ground = false
	machine.wall_normal = Vector3.ZERO
	machine.wall_side = 0.0
	machine.register_chain_mechanic("grapple")
	_rope_length = machine.grapple_length


func exit() -> void:
	pass


func update(delta: float) -> void:
	var player := machine.player
	var vel: Vector3 = player.velocity

	# If player releases the button, disconnect
	if not machine.input_grapple:
		machine.switch_to("air")
		return

	var grapple_pos: Vector3 = machine.grapple_point
	var to_point: Vector3 = grapple_pos - player.global_position
	var current_dist: float = to_point.length()
	if current_dist < 0.01:
		machine.switch_to("air")
		return
	var dir_to_point: Vector3 = to_point / current_dist

	# Jump to detach and get a boost
	if machine.input_jump_just_pressed and machine.jump_cooldown_timer <= 0.0:
		vel.y = maxf(vel.y, params.jump_velocity)
		# Boost along current motion so the release keeps the swing's energy
		var h_vel := Vector3(vel.x, 0.0, vel.z)
		if h_vel.length_squared() > 0.1:
			var boost := h_vel.normalized() * params.grapple_jump_boost * 0.5
			vel.x += boost.x
			vel.z += boost.z
		# Plus a steer boost toward held direction
		var h_look = machine.wish_dir_world
		if h_look.length_squared() > 0.1:
			var h_boost = h_look.normalized() * params.grapple_jump_boost * 0.5
			vel.x += h_boost.x
			vel.z += h_boost.z

		player.velocity = vel
		machine.jump_cooldown_timer = params.jump_cooldown
		machine.register_chain_mechanic("grapple_jump")
		machine.switch_to("air")
		if player.jump_player:
			player.jump_player.play()
		return

	# Apply gravity
	vel.y -= params.gravity * delta

	# Base reel: constant gentle pull toward the hook
	vel += dir_to_point * params.grapple_pull_force * delta

	# ── Active rope control ───────────────────────────────────────────────
	var fwd_input := -machine.input_dir.y  # +1 holding forward, -1 back
	if fwd_input > 0.1:
		# Reel in: shorten rope and pull hard — converts to swing speed
		vel += dir_to_point * params.grapple_reel_force * fwd_input * delta
		_rope_length = maxf(_rope_length - 8.0 * fwd_input * delta, 2.0)
	elif fwd_input < -0.1:
		# Pay out rope back toward the original latch length
		_rope_length = minf(_rope_length + 8.0 * -fwd_input * delta, machine.grapple_length)

	# Keep the working length taut to where we actually are (swinging inside
	# the sphere shortens the constraint, giving crisp Tarzan arcs)
	_rope_length = minf(_rope_length, maxf(current_dist, 2.0))

	# ── Pendulum constraint at the current rope length ────────────────────
	if current_dist > _rope_length:
		# Kill outward radial velocity (the rope is taut and inextensible)
		var radial_vel = vel.project(dir_to_point)
		if radial_vel.dot(dir_to_point) < 0:
			vel -= radial_vel
		# Spring correction toward the sphere surface
		var diff = current_dist - _rope_length
		vel += dir_to_point * diff * params.grapple_spring_strength * delta

	# ── Tangential steering (strafe around the swing sphere) ──────────────
	var strafe_input := machine.input_dir.x
	if absf(strafe_input) > 0.1:
		var right := player.global_transform.basis.x
		var tangent_steer := right - right.project(dir_to_point)
		if tangent_steer.length_squared() > 0.01:
			vel += tangent_steer.normalized() * params.grapple_air_control * strafe_input * delta

	player.velocity = vel
	player.move_and_slide()