extends "res://addons/pinjoint-ropephysics/path_3d_rope.gd"

## Tether variant of the pinjoint rope.
##
## Adds two things the base addon does not do:
##  - endpoints can be any PhysicsBody3D (the addon only exports RigidBody3D,
##    and its `rigidbody_attached_to_start` path overwrites node_b instead of
##    setting node_a, which pins the body to world space rather than to the rope)
##  - every segment is locked to the X-Y plane so the rope behaves as a 2D rope
##
## Expects to be a direct child of an untransformed parent: the base addon bakes
## its own local `position` into the segment/joint positions and zeroes the node.

## Bodies the two rope ends pin to. NodePath rather than a typed node export so
## the value resolves reliably when set from a .tscn instance override.
@export var attach_start_path : NodePath
@export var attach_end_path : NodePath

var attach_start : PhysicsBody3D
var attach_end : PhysicsBody3D

## Extra rope length as a fraction of the gap between the two endpoints.
## 0.0 is dead taut (pin joints will fight); ~0.15 gives a natural sag.
@export var slack := 0.15

## Reel the rope in and out so its length keeps tracking the endpoint gap,
## instead of staying fixed at whatever it was when the level loaded.
@export var dynamic_length := true

## Stops the rope collapsing into a stub when the players stand on top of each
## other.
@export var min_length := 2.0

## Rest length of the leash, in metres. Past this the tether pulls the endpoints
## back together with a spring.
##
## This is deliberately NOT a cap on the rope geometry. The endpoints are
## CharacterBody3D, which is kinematic and therefore infinitely massive to the
## solver: a pin joint anchored to one pulls on the rope and never on the player.
## Cap the chain and the terminal segment gets whipped by correction impulses
## nothing ever absorbs, which is the violent wiggle. So the chain is kept slack
## at all times and the leash is an explicit force instead.
@export var max_length := 20.0

## Leash spring: metres per second squared of pull per metre of stretch, plus
## damping on the separation speed. Applied as acceleration, so it behaves the
## same whether an endpoint is a CharacterBody3D or a RigidBody3D.
## Stiffness has to beat Player.friction (which zeroes horizontal velocity when
## there is no input) before a grounded player will slide at all.
@export var elastic_stiffness := 60.0
@export var elastic_damping := 8.0

## Safety valve so a runaway stretch cannot fling a body across the level.
@export var max_pull_accel := 200.0

## Drag on the rate the two endpoints separate at while the rope is taut, per
## second, so towing the other player feels heavy. Ramps in over the same
## taut_range as the tautening. This is the knob for how much a player is slowed
## by dragging their partner; elastic_stiffness is the leash itself.
@export var haul_drag := 10.0

## How hard the rope is drawn straight once the leash engages. 0 leaves it
## hanging; 1 snaps it onto the line between the endpoints.
##
## Needed because the chain can never be dead taut on its own. Its length has to
## stay above the gap or the pin joints are over-constrained, and a hanging chain
## sags by roughly L*sqrt(3*excess/8) — even a 2% excess drapes ~9% of the span,
## which reads as a slack rope no matter how hard the players pull. So tension is
## faked: the segments are moved onto the straight line directly. That
## configuration satisfies every pin joint exactly, so unlike a real tension load
## it costs the solver nothing.
@export_range(0.0, 1.0, 0.01) var taut_pull := 0.5

## Stretch past max_length, in metres, at which the rope is drawn fully taut.
@export var taut_range := 1.0

## Hard backstop, as a multiple of max_length. The spring cannot win against code
## that writes velocity outright — a launcher, a moving platform, a bug — and an
## endpoint dragged far enough turns the rope into a handful of enormous capsules
## that thrash. Past this distance the endpoints get moved back directly. Normal
## play never reaches it.
@export var hard_stretch := 2.0

## How fast the rope reels *in*, in metres per second. Low values feel like a
## winch, high values like the rope is weightless. Reeling out is not rate
## limited — see _reel.
@export var reel_speed := 6.0

## Constrain segments to the X-Y plane (linear Z, angular X and Y).
@export var plane_lock_z := true

@export_flags_3d_physics var segment_collision_layer := 4
@export_flags_3d_physics var segment_collision_mask := 1

# Fraction of the endpoint gap the rope keeps in hand at minimum. `slack` is the
# resting sag; this is only the floor that keeps the chain off dead taut when
# slack is turned down to nothing.
const TAUT_MARGIN := 0.02

# The base script zeroes `position` and bakes it into child positions, so keep
# our own copy to place the end joint we add.
var _origin_offset := Vector3.ZERO

# Total rope length right now: snaps up to _target_length(), eases down to it.
var _rope_length := 0.0

## Fired once, when the rope parts. Carries the world position of the break, for
## whoever wants to put a sound or a puff of frayed cable there.
signal snapped(at_position: Vector3)

var _snapped := false
# Index of the first segment of the tail half, once the rope has parted.
var _split_index := -1
# The tail half gets its own Path3D to draw along. See _split_mesh.
var _tail_path : Path3D


func _ready() -> void:
	attach_start = get_node_or_null(attach_start_path) as PhysicsBody3D
	attach_end = get_node_or_null(attach_end_path) as PhysicsBody3D
	_origin_offset = position
	_fit_curve_to_endpoints()
	super()
	_apply_plane_lock()
	_wire_endpoints()
	_rope_length = _target_length()
	var snap_timer := get_node_or_null("SnapTimer") as Timer
	# Guarded: the signal may also have been wired up in the editor.
	if snap_timer != null and not snap_timer.timeout.is_connected(_on_snap_timer_timeout):
		snap_timer.timeout.connect(_on_snap_timer_timeout)


func _physics_process(delta: float) -> void:
	if _snapped:
		# Nothing left to reel, tension or leash. Both halves just hang off their
		# player, so all that is left is drawing them.
		_redraw_halves()
		return
	if dynamic_length:
		_reel(delta)
	_apply_elastic(delta)
	_apply_tautness(delta)
	_update_timer()
	# Base script redraws the CSG curve from the segment transforms and capsule
	# heights, so it has to run after the resize.
	super(delta)


func _on_snap_timer_timeout() -> void:
	snap()


## Break the rope at its midpoint. Each half stays pinned to its own player and
## falls slack; the leash, the reel and the tautening all stop, so from here the
## players are untethered. Idempotent.
func snap() -> void:
	if _snapped or segments.size() < 2:
		return
	_snapped = true

	# joints[0] pins segment 0 to attach_start, and joints[i] for i in 1..N-1
	# bridges segments i-1 and i. So the joint in the middle of the chain is
	# joints[N/2], and freeing it is what actually parts the rope.
	@warning_ignore("integer_division")
	_split_index = segments.size() / 2
	var broken := joints[_split_index]
	var break_point := broken.global_position
	joints.remove_at(_split_index)
	broken.queue_free()

	# Hand the weight back. _apply_tautness may have left the segments weightless
	# to hold a straight line, and it is never going to run again to undo that.
	for segment in segments:
		segment.gravity_scale = 1.0

	_split_mesh()
	snapped.emit(break_point)


# One Path3D drew the whole chain. Left alone, the CSG would keep bridging the
# two halves with a length of rope stretched across the gap, so the tail gets a
# path of its own and the original curve is cut back to the head.
func _split_mesh() -> void:
	var tail_curve := Curve3D.new()
	for i in segments.size() - _split_index + 1:
		tail_curve.add_point(Vector3.ZERO)
	_tail_path = Path3D.new()
	_tail_path.curve = tail_curve
	add_child(_tail_path)

	# Duplicated rather than built from scratch: the polygon is generated in the
	# base script's _ready, and the CSG carries a dozen path_* settings off the
	# scene that all have to match for the two halves to look like one rope.
	var tail_mesh := mesh.duplicate() as CSGPolygon3D
	_tail_path.add_child(tail_mesh)
	tail_mesh.path_node = tail_mesh.get_path_to(_tail_path)

	# Cut the head back. Curves run one point per segment plus one to cap the end.
	while curve.point_count > _split_index + 1:
		curve.remove_point(curve.point_count - 1)
	_redraw_halves()


func _redraw_halves() -> void:
	_write_curve(curve, 0, _split_index - 1)
	if _tail_path != null:
		_write_curve(_tail_path.curve, _split_index, segments.size() - 1)


# The base script's curve update, over a range instead of the whole chain: a
# point at each segment's +Y cap, plus one more for the last segment's -Y cap.
func _write_curve(target: Curve3D, first: int, last: int) -> void:
	for i in last - first + 1:
		var segment := segments[first + i]
		var half := (segment.get_child(0).shape as CapsuleShape3D).height * 0.5
		target.set_point_position(i, segment.position + segment.transform.basis.y * half)
	var final := segments[last]
	var final_half := (final.get_child(0).shape as CapsuleShape3D).height * 0.5
	target.set_point_position(
		last - first + 1, final.position - final.transform.basis.y * final_half
	)


## Length the rope wants to be for the current endpoint gap. Unbounded above:
## the geometry follows the players wherever they go and _apply_elastic is what
## stops them going far.
func _target_length() -> float:
	if attach_start == null or attach_end == null:
		return maxf(distance, min_length)
	# Give up the resting sag as the leash engages. Straightening the segments is
	# not enough on its own: while the chain is longer than the gap the joints have
	# real error to correct and they push it right back off the line.
	var effective_slack := lerpf(slack, TAUT_MARGIN, _taut_ramp())
	return maxf(_endpoint_span() * (1.0 + effective_slack), min_length)


func _reel(delta: float) -> void:
	var previous := _rope_length
	var target := _target_length()
	if target > _rope_length:
		# Out is instant. Rate-limiting this direction is what lets the gap outrun
		# the chain, and a chain shorter than the gap is the whole failure mode.
		_rope_length = target
	else:
		# In is the winch, so the rope visibly takes up its own slack.
		_rope_length = move_toward(_rope_length, target, reel_speed * delta)
	# Hard floor. Instant reel-out already covers this while `slack` is positive;
	# it is here so that a slack of 0 still cannot produce a chain shorter than the
	# straight-line gap.
	_rope_length = maxf(_rope_length, _endpoint_span() * (1.0 + TAUT_MARGIN))
	if absf(_rope_length - previous) > 0.0001:
		_set_segment_length(_rope_length / float(number_of_segments))


# The leash. See max_length for why this is a force and not a joint constraint.
func _apply_elastic(delta: float) -> void:
	if attach_start == null or attach_end == null:
		return
	var to_end := attach_end.global_position - attach_start.global_position
	var span := to_end.length()
	if span <= max_length or span < 0.001:
		return
	var axis := to_end / span
	# Damp separation only. Damping the closing speed too would fight the spring
	# on the way back in and leave the players stuck at full stretch.
	var separation := maxf(_separation_speed(), 0.0)
	var accel := clampf(
		elastic_stiffness * (span - max_length) + elastic_damping * separation,
		0.0,
		max_pull_accel
	)
	var pull := axis * accel * delta
	_add_velocity(attach_start, pull)
	_add_velocity(attach_end, -pull)

	_apply_haul_drag(axis, delta)

	var limit := max_length * hard_stretch
	if span > limit:
		_clamp_span(axis, span - limit, separation)


# Weight. The spring alone does not read as hauling: Player re-accelerates to
# move_speed every frame at `accel`, so a player towing a planted partner still
# runs at full speed and the rope looks weightless. Drag on the outward part of
# their velocity is what makes the load felt.
#
# Only the outward part, so being towed is never slowed and neither is running
# back toward the other player. And no scaling by who is heavier: the resistance
# comes out of the ramp, which only stays high while the far end is actually
# refusing to follow.
func _apply_haul_drag(axis: Vector3, delta: float) -> void:
	if haul_drag <= 0.0:
		return
	var drag := clampf(haul_drag * _stretch_ramp() * delta, 0.0, 1.0)
	if drag <= 0.0:
		return
	# Braked on their own outward speed, not on the separation rate. Separation is
	# the wrong signal: a hauler towing a partner who is keeping up has no
	# separation at all, and that is the case this exists for. Stretch is the right
	# signal, because stretch is what the rope's tension is proportional to.
	#
	# axis runs start -> end, so outward is -axis for the start and +axis for the
	# end. Only whoever is moving outward pays, which sorts out who is hauling and
	# who is being towed without having to ask.
	var out_start := _velocity_of(attach_start).dot(-axis)
	if out_start > 0.0:
		_add_velocity(attach_start, axis * (out_start * drag))
	var out_end := _velocity_of(attach_end).dot(axis)
	if out_end > 0.0:
		_add_velocity(attach_end, -axis * (out_end * drag))


# Draw the chain onto the straight line between the endpoints as the leash takes
# up. See taut_pull for why this is done by hand rather than by the solver.
func _apply_tautness(delta: float) -> void:
	if attach_start == null or attach_end == null:
		return
	var ramp := _taut_ramp()

	# Take the weight off as the rope goes taut. Gravity is the only thing pulling
	# it off the line, and a weightless chain holds the pose for free instead of
	# being dragged back down between frames. Written unconditionally so the rope
	# gets its weight back the moment the leash lets go.
	for segment in segments:
		segment.gravity_scale = 1.0 - ramp
	if ramp <= 0.0:
		return

	var a := attach_start.global_position
	var b := attach_end.global_position
	var span := a.distance_to(b)
	# Per-frame lerp weight, corrected so the pull feels the same off 60 Hz.
	var blend := 1.0 - pow(1.0 - ramp * taut_pull, delta * 60.0)

	# Capsules run along local Y and +Y is the end nearer the rope's start, so the
	# straight-line pose points each segment's local Y from the end back to the
	# start. Only that axis is aimed: the addon bakes a PI/2 X-rotation into the
	# segments at spawn, so their local Z is not world Z, and building a fresh
	# basis instead of swinging the existing one puts the segment in a pose
	# axis_lock_angular_x/y forbid and the solver spends the frame undoing it.
	var y_axis := (a - b) / span

	var count := segments.size()
	for i in count:
		var segment := segments[i]
		var current := Quaternion(segment.global_basis.orthonormalized())
		# Shortest arc onto the line. Both vectors lie in the X-Y plane, so the
		# rotation is about world Z, which is the one axis left unlocked.
		var swing := Quaternion(segment.global_basis.y.normalized(), y_axis)
		# Segment i owns the slice [i, i+1] of the line and sits at its midpoint.
		var ideal := a.lerp(b, (float(i) + 0.5) / float(count))
		segment.global_transform = Transform3D(
			Basis(current.slerp(swing * current, blend)),
			segment.global_position.lerp(ideal, blend)
		)
		# Spin is the segment's own momentum fighting the pose it was just put in.
		segment.angular_velocity *= 1.0 - blend


# Positional backstop. Moves the bodies rather than shortening the chain: a chain
# shorter than the gap is the over-constrained case this whole design exists to
# avoid, so the rope is never the thing that gives.
func _clamp_span(axis: Vector3, excess: float, separation: float) -> void:
	var half := axis * (excess * 0.5)
	_move_endpoint(attach_start, half)
	_move_endpoint(attach_end, -half)
	# Cancel the outward velocity too, or they grind against the backstop and it
	# has to fire again every single frame.
	if separation > 0.0:
		var kill := axis * (separation * 0.5)
		_add_velocity(attach_start, kill)
		_add_velocity(attach_end, -kill)


func _move_endpoint(body: PhysicsBody3D, motion: Vector3) -> void:
	if body is CharacterBody3D:
		# Swept, so the backstop cannot shove a player inside level geometry.
		(body as CharacterBody3D).move_and_collide(motion)
	else:
		body.global_position += motion


## How loaded the tether is, 0 slack to 1 at full stretch. For camera shake, UI
## and audio; read every frame, so keep it cheap.
func tension_ratio() -> float:
	return (clampf(_stretch_ramp(), 0.8, 1) - 0.8) * 5


## 0 while the rope hangs free, reaching 1 once the leash is stretched taut_range
## past max_length. Drives both the tautening and the haul drag.
func _stretch_ramp() -> float:
	# A parted rope pulls on nothing, so it reports no load either — otherwise the
	# players walk away and the camera buzzes forever off a rope that is gone.
	if _snapped or attach_start == null or attach_end == null:
		return 0.0
	var span := _endpoint_span()
	if span <= max_length:
		return 0.0
	return clampf((span - max_length) / maxf(taut_range, 0.001), 0.0, 1.0)


## _stretch_ramp gated on the tautening being switched on at all.
func _taut_ramp() -> float:
	if taut_pull <= 0.0:
		return 0.0
	return _stretch_ramp()


func _endpoint_span() -> float:
	if attach_start == null or attach_end == null:
		return 0.0
	return attach_start.global_position.distance_to(attach_end.global_position)


## Rate the gap is opening at, in metres per second. Negative while closing.
func _separation_speed() -> float:
	if attach_start == null or attach_end == null:
		return 0.0
	var to_end := attach_end.global_position - attach_start.global_position
	if to_end.length() < 0.001:
		return 0.0
	return (_velocity_of(attach_end) - _velocity_of(attach_start)).dot(to_end.normalized())


func _velocity_of(body: PhysicsBody3D) -> Vector3:
	if body is CharacterBody3D:
		return (body as CharacterBody3D).velocity
	if body is RigidBody3D:
		return (body as RigidBody3D).linear_velocity
	return Vector3.ZERO


func _add_velocity(body: PhysicsBody3D, delta_v: Vector3) -> void:
	if body is CharacterBody3D:
		(body as CharacterBody3D).velocity += delta_v
	elif body is RigidBody3D:
		(body as RigidBody3D).linear_velocity += delta_v


# Resize every capsule and re-anchor the pin joints bracketing it.
#
# Joint3D only derives its anchor points from the node transforms when the joint
# is (re)configured on tree entry, so moving the joint nodes here would do
# nothing — the anchors go straight to the physics server instead.
#
# Capsules run along local Y, and the base script's curve update shows +Y is the
# end nearer the rope's start, so joint i sits at -Y on segment i-1 and +Y on
# segment i.
func _set_segment_length(segment_length: float) -> void:
	# CapsuleShape3D silently clamps height to its diameter; clamp here too so
	# the joint anchors match the shape the physics server actually has.
	var clamped := maxf(segment_length, cable_thickness * 2.0)
	var half := clamped * 0.5
	for segment in segments:
		var shape := segment.get_child(0).shape as CapsuleShape3D
		shape.height = clamped
	for i in joints.size():
		var rid := joints[i].get_rid()
		if i == 0:
			# node_b is the attached body, whose anchor must not move.
			PhysicsServer3D.pin_joint_set_local_a(rid, Vector3(0, half, 0))
		elif i < segments.size():
			PhysicsServer3D.pin_joint_set_local_a(rid, Vector3(0, -half, 0))
			PhysicsServer3D.pin_joint_set_local_b(rid, Vector3(0, half, 0))
		else:
			# Tail joint: node_a is the last segment, node_b the attached body.
			PhysicsServer3D.pin_joint_set_local_a(rid, Vector3(0, -half, 0))


# Rebuild the curve as a sagging arc between the two endpoints so the rope
# always spans wherever the players actually spawn.
func _fit_curve_to_endpoints() -> void:
	if attach_start == null or attach_end == null:
		return
	var a := to_local(attach_start.global_position)
	var b := to_local(attach_end.global_position)
	var span := a.distance_to(b)
	if span < 0.001:
		return
	# Bow the spawn arc *upward*. A downward sag can start inside level geometry
	# (a rope between two grounded players dips below the floor), and segments
	# that spawn embedded tunnel straight through it. Starting high is always
	# clear, and gravity drapes the rope into place within a few frames.
	#
	# Curve points get zero tangents, so the baked path is the straight pair
	# a->mid->b: bowing by h gives a length of 2*sqrt((span/2)^2 + h^2). Solve
	# that for the length the reel is going to ask for anyway, so the rope does
	# not lurch on the first frame.
	var target := _target_length()
	var bow := 0.5 * sqrt(maxf(target * target - span * span, 0.0))
	var mid := (a + b) * 0.5 + Vector3(0, bow, 0)

	var fitted := Curve3D.new()
	# The scene's curve ships with a huge bake_interval, which would make
	# sample_baked() miss the midpoint entirely.
	fitted.bake_interval = maxf(span / float(number_of_segments) * 0.25, 0.05)
	fitted.add_point(a)
	fitted.add_point(mid)
	fitted.add_point(b)
	curve = fitted
	# `distance` is an @onready in the base script; recompute it explicitly so
	# the value is correct regardless of when that initializer runs.
	distance = curve.get_baked_length()

func _apply_plane_lock() -> void:
	for segment in segments:
		segment.collision_layer = segment_collision_layer
		segment.collision_mask = segment_collision_mask
		segment.continuous_cd = true
		if plane_lock_z:
			segment.axis_lock_linear_z = true
			segment.axis_lock_angular_x = true
			segment.axis_lock_angular_y = true
			# The base script offsets look_at by (0.001, 0, -0.001) to dodge a
			# degenerate up vector, which nudges segments off the plane.
			segment.position.z = 0.0


func _wire_endpoints() -> void:
	if attach_start != null:
		# node_b first: assigning node_a while node_b is still segments[0] would
		# briefly join the body to itself.
		joints[0].node_b = attach_start.get_path()
		joints[0].node_a = segments[0].get_path()
	if attach_end != null and not fixed_end_point:
		var end_joint := PinJoint3D.new()
		add_child(end_joint)
		end_joint.position = curve_points[-1] + _origin_offset
		end_joint.node_a = segments[-1].get_path()
		end_joint.node_b = attach_end.get_path()
		end_joint.set_param(PinJoint3D.PARAM_BIAS, joint_bias_or_stiffness)
		end_joint.set_param(PinJoint3D.PARAM_IMPULSE_CLAMP, max_impulse)
		joints.append(end_joint)

func _update_timer() -> void:
	var stretched = is_equal_approx(tension_ratio(), 1.0)
	if stretched and $SnapTimer.is_stopped():
		$SnapTimer.start()
	elif !stretched:
		$SnapTimer.stop()