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 := 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

# 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 dynamic_length:
		_reel(delta)
	# Base script redraws the CSG curve from the segment transforms and capsule
	# heights, so it has to run after the resize.
	super(delta)


## Length the rope wants to be for the current endpoint gap.
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 + slack), min_length, max_length)


func _reel(delta: float) -> void:
	var previous := _rope_length
	_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)

#func _physics_process(_delta: float) -> void:
	#curve.bake_interval += 0.01