# May only be attached to PhysicsBody3Ds as a direct child
# CanBeGrabbed may only be attached to CollisionObject3Ds (as a direct child)
extends Node

# T is parent's global transform inverse times node's global transform at the time of grab initiation
# Array<{node: Node3D, iniParentTInv: Transform, iniNodeT, collision_layer: int}>
var grabbedNodes: Array
var parent: Node3D
var lastParentTransform: Transform3D
var collision: KinematicCollision3D = KinematicCollision3D.new()

func _ready():
	parent = get_parent()
	Signals.connect("ForceRelease", release)

func _process(_delta):
	# Process grabbed nodes
	for node in grabbedNodes:
		node.node.global_transform = parent.global_transform * node.iniParentTInv * node.iniNodeT

	lastParentTransform = parent.global_transform

func grab():
	if (grabbedNodes.size() > 0):
		return
	if parent.test_move(
		lastParentTransform,
		parent.global_transform * Vector3() - lastParentTransform * Vector3(),
		collision,
		0.001,
		false,
		INF
	):
		for i in range(collision.get_collision_count()):
			var node = collision.get_collider(i)
			if not node.find_child("CanBeGrabbed", false):
				continue
			grabbedNodes.append({
				"node": node,
				"iniParentTInv": parent.global_transform.inverse(),
				"iniNodeT": node.global_transform,
				"collision_layer": node.collision_layer
			})
			node.collision_layer = 0

func release():
	for node in grabbedNodes:
		node.node.collision_layer = node.collision_layer
	grabbedNodes = []
