extends Node
## Sibling FO. Shift registered world roots.
## Move focus only when it is NOT already under a root (capsule = sibling).
## Never zero velocity.

const THRESHOLD: float = 50000.0
const COOLDOWN: float = 0.25

var focus: Node3D = null
var _shift_roots: Array[Node3D] = []
var _cooldown: float = 0.0


func _ready() -> void:
	process_mode = Node.PROCESS_MODE_ALWAYS


func set_focus(node: Node3D) -> void:
	focus = node


func register_root(node: Node3D) -> void:
	if node == null:
		return
	if not _shift_roots.has(node):
		_shift_roots.append(node)
		print("FloatingOrigin registered: ", node.name)


func unregister_root(node: Node3D) -> void:
	_shift_roots.erase(node)


func shift(offset: Vector3) -> void:
	if offset.length_squared() < 1.0:
		return
	_apply_shift(offset)
	_cooldown = COOLDOWN


func _physics_process(delta: float) -> void:
	if _cooldown > 0.0:
		_cooldown -= delta
		return
	if focus == null or not is_instance_valid(focus) or not focus.is_inside_tree():
		return
	if _shift_roots.is_empty():
		return
	var fp: Vector3 = focus.global_position
	if fp.length() < THRESHOLD:
		return
	_apply_shift(-fp)
	_cooldown = COOLDOWN


func _apply_shift(offset: Vector3) -> void:
	for root in _shift_roots:
		if is_instance_valid(root) and root.is_inside_tree():
			root.global_position += offset
	if is_instance_valid(focus) and focus.is_inside_tree() and not _focus_under_a_root():
		focus.global_position += offset
	_sync_focus_physics()
	print("FloatingOrigin shift: ", offset, " focus now: ", focus.global_position if focus else Vector3.ZERO)


func _focus_under_a_root() -> bool:
	var n: Node = focus
	while n != null:
		if n is Node3D and _shift_roots.has(n as Node3D):
			return true
		n = n.get_parent()
	return false


func _sync_focus_physics() -> void:
	if focus == null or not is_instance_valid(focus):
		return
	if not (focus is CharacterBody3D):
		return
	var body := focus as CharacterBody3D
	var saved_vel: Vector3 = body.velocity
	PhysicsServer3D.body_set_state(
		body.get_rid(),
		PhysicsServer3D.BODY_STATE_TRANSFORM,
		body.global_transform
	)
	PhysicsServer3D.body_set_state(
		body.get_rid(),
		PhysicsServer3D.BODY_STATE_LINEAR_VELOCITY,
		saved_vel
	)
	body.velocity = saved_vel
	if body.has_method("reset_physics_interpolation"):
		body.reset_physics_interpolation()