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_range(0.0, 1.0, 0.01) 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

## Bounds on total rope length, in metres. min_length stops the rope collapsing
## into a stub when the players stand on top of each other; max_length is the
## leash that eventually drags them back together.
@export var min_length := 2.0
@export var max_length := 20.0

## How fast total length changes, in metres per second. Low values feel like a
## winch, high values like the rope is weightless.
@export var reel_speed := 8.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


# --- Pull / tension / snap ---------------------------------------------------
#
# Ported from the 2D Verlet rope. That rope integrated its own points, so it
# could resolve stretch by moving them directly; here the rope body is solved by
# the physics server, so only the *anchor* half of that logic carries over: the
# players get pulled, damped and finally launched. `max_length` is shared with
# the reel, which is exactly right — the reel stops paying out rope there, so
# that is the gap at which the rope runs out and starts pulling back.

## Note: the reference script's `reel_speed` is this — how fast the rope hauls
## the players together. Ours was already taken by the rope's own length change,
## so the anchor-pulling rate is `pull_speed`.
@export_group("Tension")

## Gap at which strain starts registering for visuals. Below max_length, so the
## rope reads as straining before it actually starts pulling.
@export var tension_start := 16.0

## Fraction of its nominal length the rope gives before it is genuinely straight,
## soaked up by compliant pin joints and the two end anchors.
##
## 0.12 is the measured sweet spot on a 6m/12-segment rope: sag at the moment
## pull engages falls from 0.73m at 0.02 to 0.41m here. Past ~0.18 it gets worse
## again — the extra span opens the joints further, which adds back the path
## length it was meant to take up. Retune if segment count or joint softness
## changes; those move the curve.
@export_range(0.0, 0.5, 0.005) var taut_margin := 0.12

## Metres per second the rope hauls the players together while over-stretched.
@export var pull_speed := 6.0

## How the pull is split. 0.5 moves both equally; 0.0 moves only attach_end.
@export_range(0.0, 1.0, 0.01) var pull_bias := 0.5

## Fraction of velocity-away-from-the-rope killed per tick while taut.
@export_range(0.0, 1.0, 0.01) var damping := 0.2

## Hard break distance, and how fast sustained over-stretch accumulates toward a
## break. The rope snaps on whichever arrives first.
@export var snap_length := 34.0
@export var snap_tension := 1.0
@export var tension_rise := 1.5
@export var tension_fall := 2.0

## Speed kicked into each player when the rope lets go.
@export var snap_impulse := 19.0

## Seconds the two dead halves dangle and fade before being freed.
@export var snap_fade := 0.6

## Off makes the rope an unbreakable leash: it still pulls, never snaps.
@export var can_snap := true

signal became_taut
signal became_slack
signal rope_snapped
signal tension_changed(amount)

## True while the gap exceeds max_length and the rope is actively hauling.
var taut := false
## True once the rope has broken. It stays broken until reset().
var snapped := false
## Accumulated over-stretch. Reaching snap_tension breaks the rope.
var tension := 0.0
## 0..1 strain for visuals and camera shake. Driven by whichever of raw distance
## or accumulated tension is further along.
var strain_amt := 0.0

var _emitted_strain := -1.0
var _break_time := 0.0
var _halves : Array[Path3D] = []

# 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, eased toward _target_length() at reel_speed.
var _rope_length := 0.0


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


func _physics_process(delta: float) -> void:
	if snapped:
		_update_break(delta)
		return
	if dynamic_length:
		_reel(delta)
	_constrain(delta)
	# Base script redraws the CSG curve from the segment transforms and capsule
	# heights, so it has to run after the resize.
	super(delta)


## Pull the players together while the gap exceeds the rope, accumulate strain,
## and break the rope when it has taken too much.
func _constrain(delta: float) -> void:
	if attach_start == null or attach_end == null:
		return
	var v := attach_end.global_position - attach_start.global_position
	if plane_lock_z:
		v.z = 0.0
	var dist := v.length()
	if dist < 0.001:
		return
	var stretch := dist - _taut_distance()

	var now_taut := stretch > 0.0
	if now_taut != taut:
		taut = now_taut
		if taut:
			became_taut.emit()
		else:
			became_slack.emit()

	# Tension only builds while genuinely over-stretched, and bleeds off the rest
	# of the time, so a rope that is repeatedly yanked breaks but one held at a
	# steady hard stretch does not break instantly.
	if stretch > 0.0:
		var s := clampf(stretch / maxf(snap_length - max_length, 0.001), 0.0, 1.0)
		tension += tension_rise * s * delta
	else:
		tension = move_toward(tension, 0.0, tension_fall * delta)

	var dist_strain := clampf(
		(dist - tension_start) / maxf(snap_length - tension_start, 0.001), 0.0, 1.0)
	strain_amt = maxf(dist_strain, tension / maxf(snap_tension, 0.001))
	_emit_strain()

	if now_taut:
		var dir := v / dist
		var pull := minf(stretch, pull_speed * delta)
		_haul(attach_start, dir * pull * pull_bias)
		_haul(attach_end, -dir * pull * (1.0 - pull_bias))
		if damping > 0.0:
			_damp(attach_start, -dir)
			_damp(attach_end, dir)

	if can_snap and (tension >= snap_tension or dist >= snap_length):
		_snap()


# How far apart the rope can actually hold the players, which is *not* the
# length the reel commands. Pin joints are compliant: measured on the stock
# settings the interior joints open ~0.29m and the two end anchors another
# ~0.14m on a 6m rope, so the rope keeps roughly `taut_margin` in hand past its
# nominal length. Pulling at nominal starts hauling while the rope still droops.
#
# This deliberately does not measure the rope's live path. That was tried and it
# feeds back: compliant joints stretch to meet whatever the gap is, so measured
# reach chases the span, `stretch` never turns positive and the rope never pulls
# at all. The allowance has to be a value the current stretch cannot influence.
func _taut_distance() -> float:
	return _rope_length * (1.0 + taut_margin)


func _emit_strain() -> void:
	if absf(strain_amt - _emitted_strain) > 0.01:
		_emitted_strain = strain_amt
		tension_changed.emit(strain_amt)


# Position is moved rather than velocity added, matching the reference: the pull
# has to survive the player's own _physics_process, which rewrites velocity.x
# from input every tick and would swallow a velocity nudge.
func _haul(body: PhysicsBody3D, delta_pos: Vector3) -> void:
	var rigid := body as RigidBody3D
	if rigid != null:
		rigid.apply_central_impulse(delta_pos / maxf(get_physics_process_delta_time(), 0.001))
	else:
		body.global_position += delta_pos


# Kill only the component of velocity heading away from the rope, so the rope
# resists being stretched without deadening movement along it.
func _damp(body: PhysicsBody3D, away: Vector3) -> void:
	var character := body as CharacterBody3D
	if character != null:
		var along := character.velocity.dot(away)
		if along > 0.0:
			character.velocity -= away * along * damping
		return
	var rigid := body as RigidBody3D
	if rigid != null:
		var along := rigid.linear_velocity.dot(away)
		if along > 0.0:
			rigid.linear_velocity -= away * along * damping


## Length the rope wants to be for the current endpoint gap.
##
## Divided by the compliance allowance rather than set to the gap directly. The
## chain can reach `taut_margin` past whatever length it is told to be, so
## commanding exactly the gap leaves that surplus hanging as droop at every
## distance — which is what made the rope look slack however hard it was pulled.
## Commanding the gap *minus* the stretch it is going to gain means its real
## reach lands on the gap, and the rope hangs straight.
func _target_length() -> float:
	if attach_start == null or attach_end == null:
		return maxf(distance, min_length)
	var span := attach_start.global_position.distance_to(attach_end.global_position)
	return clampf(span / (1.0 + taut_margin), min_length, max_length)


func _reel(delta: float) -> void:
	var previous := _rope_length
	# Both directions are rate limited. Paying out instantly was what made the
	# rope feel permanently slack: the commanded length would sit exactly on the
	# gap, and since the chain can reach `taut_margin` beyond what it is told to
	# be, that surplus had nowhere to go and hung as droop no matter how hard the
	# players pulled. Trailing the gap instead means a pull faster than
	# `reel_speed` eats the surplus and the rope comes up taut.
	_rope_length = move_toward(_rope_length, _target_length(), reel_speed * delta)
	if absf(_rope_length - previous) > 0.0001:
		_set_segment_length(_rope_length / float(number_of_segments))


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


## Break the rope: kick the players apart and split the chain in two.
##
## The reference had to hand-integrate the two dead halves after a break. Here
## the halves are still pin-jointed rigid bodies with one loose end, so the
## physics server drapes them for free — all that is needed is to drop the
## middle joint and give each half its own tube to draw into.
func _snap() -> void:
	var dir := attach_end.global_position - attach_start.global_position
	if plane_lock_z:
		dir.z = 0.0
	dir = dir.normalized() if dir.length() > 0.001 else Vector3.RIGHT
	_kick(attach_start, -dir * snap_impulse)
	_kick(attach_end, dir * snap_impulse)

	_split_chain()

	snapped = true
	taut = false
	tension = 0.0
	strain_amt = 0.0
	_emit_strain()
	rope_snapped.emit()


func _kick(body: PhysicsBody3D, impulse: Vector3) -> void:
	var character := body as CharacterBody3D
	if character != null:
		character.velocity += impulse
		return
	var rigid := body as RigidBody3D
	if rigid != null:
		rigid.apply_central_impulse(impulse * rigid.mass)


# Free the middle joint so the chain parts, then hand each half its own path and
# tube. The single shared tube cannot be reused: its curve would still run
# through both halves and stretch a band of geometry across the break.
func _split_chain() -> void:
	var mid := int(segments.size() / 2.0)
	# joints[i] bridges segments[i-1] and segments[i], so joints[mid] is the one
	# holding the two halves together.
	if mid > 0 and mid < joints.size():
		var seam := joints[mid]
		joints.remove_at(mid)
		seam.queue_free()

	mesh.visible = false
	_halves.clear()
	_halves.append(_make_half(0, mid - 1))
	_halves.append(_make_half(mid, segments.size() - 1))
	_break_time = 0.0
	_update_break(0.0)


# A Path3D + CSGPolygon3D pair covering segments[lo..hi], mirroring the settings
# of the main tube so the halves look like the rope they came from.
func _make_half(lo: int, hi: int) -> Path3D:
	var path := Path3D.new()
	path.top_level = true
	path.curve = Curve3D.new()
	path.set_meta("lo", lo)
	path.set_meta("hi", hi)
	add_child(path)

	var tube := CSGPolygon3D.new()
	tube.polygon = mesh.polygon
	tube.mode = mesh.mode
	tube.path_interval_type = mesh.path_interval_type
	tube.path_interval = mesh.path_interval
	tube.path_rotation = mesh.path_rotation
	tube.path_local = mesh.path_local
	tube.path_continuous_u = mesh.path_continuous_u
	tube.path_joined = mesh.path_joined
	tube.smooth_faces = mesh.smooth_faces
	tube.calculate_tangents = mesh.calculate_tangents
	# The rope material is an opaque resource shared with anything else using it,
	# so the fade gets its own transparent copy rather than mutating the original.
	if material != null:
		var faded := material.duplicate() as StandardMaterial3D
		if faded != null:
			faded.transparency = BaseMaterial3D.TRANSPARENCY_ALPHA
			tube.material = faded
	path.add_child(tube)
	# path_node resolves relative to the CSG node, so it can only be set once the
	# tube is actually in the tree under the path.
	tube.path_node = tube.get_path_to(path)
	return path


func _update_break(delta: float) -> void:
	_break_time += delta
	var alpha := clampf(1.0 - _break_time / maxf(snap_fade, 0.001), 0.0, 1.0)
	for path in _halves:
		if not is_instance_valid(path):
			continue
		_trace_half(path)
		var tube := path.get_child(0) as CSGPolygon3D
		var mat := tube.material as StandardMaterial3D
		if mat != null:
			mat.albedo_color.a = alpha
	if _break_time >= snap_fade:
		_clear_rope()


# Same endpoint math the base script uses for the main curve: a capsule's +Y end
# is the one nearer the rope start, so the run is every segment's near end plus
# the far end of the last one.
func _trace_half(path: Path3D) -> void:
	var lo : int = path.get_meta("lo")
	var hi : int = path.get_meta("hi")
	var c := path.curve
	c.clear_points()
	for i in range(lo, hi + 1):
		var seg := segments[i]
		var half_h : float = seg.get_child(0).shape.height * 0.5
		c.add_point(seg.position + seg.transform.basis.y * half_h)
	var last := segments[hi]
	var last_half : float = last.get_child(0).shape.height * 0.5
	c.add_point(last.position - last.transform.basis.y * last_half)


# Tear down every body, joint and tube the rope owns. A snapped rope should stop
# costing physics once it has finished falling.
func _clear_rope() -> void:
	for path in _halves:
		if is_instance_valid(path):
			path.queue_free()
	_halves.clear()
	for joint in joints:
		if is_instance_valid(joint):
			joint.queue_free()
	joints.clear()
	for segment in segments:
		if is_instance_valid(segment):
			segment.queue_free()
	segments.clear()
	curve_points.clear()


## Rebuild the rope from scratch after a snap.
func reset() -> void:
	_clear_rope()
	snapped = false
	taut = false
	tension = 0.0
	strain_amt = 0.0
	_emitted_strain = -1.0
	_break_time = 0.0
	if mesh != null:
		mesh.visible = true
	# The base _ready() bakes `position` into the segments and then zeroes it, so
	# the offset has to be put back before rebuilding or the new rope spawns at
	# the parent's origin instead of the rope's.
	position = _origin_offset
	curve = Curve3D.new()
	_fit_curve_to_endpoints()
	super._ready()
	_apply_plane_lock()
	_wire_endpoints()
	_rope_length = _target_length()