class_name Grab
extends Grabber


## Grab Class
##
## This class encodes information about an active grab. Additionally it applies
## hand poses and collision exceptions as appropriate.


## Priority for grip poses
const GRIP_POSE_PRIORITY := 100

## Priority for grip targeting
const GRIP_TARGET_PRIORITY := 100


## Grab target
var what : XRToolsPickable

## Grab point information
var point : XRToolsGrabPoint

## Hand grab point information
var hand_point : XRToolsGrabPointHand

## Grab transform
var transform : Transform3D

## Position drive strength
var drive_position : float = 1.0

## Angle drive strength
var drive_angle : float = 1.0

## Aim drive strength
var drive_aim : float = 0.0

## Has target arrived at grab point
var _arrived : bool = false

## Collision exceptions we manage
var _collision_exceptions : Array[PhysicsBody3D]


## Initialize the grab
func _init(
	p_grabber : Grabber,
	p_what : XRToolsPickable,
	p_point : XRToolsGrabPoint,
	p_precise : bool) -> void:

	# Copy the grabber information
	by = p_grabber.by
	pickup = p_grabber.pickup
	controller = p_grabber.controller
	hand = p_grabber.hand
	collision_hand = p_grabber.collision_hand

	# Set the point
	what = p_what
	point = p_point
	hand_point = p_point as XRToolsGrabPointHand

	# Calculate the grab transform
	if p_point:
		transform = p_point.transform
	elif p_precise:
		transform = p_what.global_transform.inverse() * by.global_transform
	else:
		transform = Transform3D.IDENTITY

	# Set the drive parameters
	if hand_point:
		drive_position = hand_point.drive_position
		drive_angle = hand_point.drive_angle
		drive_aim = hand_point.drive_aim

	# Apply collision exceptions
	if collision_hand:
		collision_hand.max_distance_reached.connect(_on_max_distance_reached)
		_add_collision_exceptions(what)


## Set the target as arrived at the grab-point
func set_arrived() -> void:
	# Ignore if already arrived
	if _arrived:
		return

	# Set arrived and apply any hand pose
	print_verbose("%s> arrived at %s" % [what.name, point])
	_arrived = true
	_set_hand_pose()

	# Report the grab
	print_verbose("%s> grabbed by %s", [what.name, by.name])
	what.grabbed.emit(what, by)


## Set the grab point
func set_grab_point(p_point : XRToolsGrabPoint) -> void:
	# Skip if no change
	if p_point == point:
		return

	# Remove any current pose override
	_clear_hand_pose()

	# Update the grab point
	point = p_point
	hand_point = point as XRToolsGrabPointHand

	# Update the transform
	if point:
		transform = point.transform

	# Apply the new hand grab-point settings
	if hand_point:
		drive_position = hand_point.drive_position
		drive_angle = hand_point.drive_angle
		drive_aim = hand_point.drive_aim

	# Apply any pose overrides
	if _arrived:
		_set_hand_pose()

	# Report switch
	print_verbose("%s> switched grab point to %s", [what.name, point.name])
	what.released.emit(what, by)
	what.grabbed.emit(what, by)


## Release the grip
func release() -> void:
	# Clear any hand pose
	_clear_hand_pose()

	# Remove collision exceptions with a small delay
	if is_instance_valid(collision_hand) and not _collision_exceptions.is_empty():
		# We need to make a copy of our array else it will be passed by reference.
		var copy : Array[PhysicsBody3D]
		for exc in _collision_exceptions:
			copy.push_back(exc)
		_collision_exceptions.clear()

		# Delay removing our exceptions to give the object time to fall away
		collision_hand.get_tree().create_timer(0.5).timeout \
			.connect(_remove_collision_exceptions \
				.bind(copy) \
				.bind(collision_hand))

	# Report the release
	print_verbose("%s> released by %s", [what.name, by.name])
	what.released.emit(what, by)


# Hand has moved too far away from object, can no longer hold on to it.
func _on_max_distance_reached() -> void:
	pickup.drop_object()


# Set hand-pose overrides
func _set_hand_pose() -> void:
	# Skip if not hand
	if not is_instance_valid(hand) or not is_instance_valid(hand_point):
		return

	# Apply the hand-pose
	if hand_point.hand_pose:
		hand.add_pose_override(hand_point, GRIP_POSE_PRIORITY, hand_point.hand_pose)

	# Apply hand snapping
	if hand_point.snap_hand:
		hand.add_target_override(hand_point, GRIP_TARGET_PRIORITY)


# Clear any hand-pose overrides
func _clear_hand_pose() -> void:
	# Skip if not hand
	if not is_instance_valid(hand) or not is_instance_valid(hand_point):
		return

	# Remove hand-pose
	hand.remove_pose_override(hand_point)

	# Remove hand snapping
	hand.remove_target_override(hand_point)


# Add collision exceptions for the grabbed object and any of its children
func _add_collision_exceptions(from : Node):
	if not is_instance_valid(collision_hand):
		return

	if not is_instance_valid(from):
		return

	# If this is a physics body, add an exception
	if from is PhysicsBody3D:
		# Make sure we don't collide with what we're holding
		_collision_exceptions.push_back(from)
		collision_hand.add_collision_exception_with(from)
		from.add_collision_exception_with(collision_hand)

	# Check all children
	for child in from.get_children():
		_add_collision_exceptions(child)


# Remove the exceptions in our passed array. We call this with a small delay
# to give an object a chance to drop away from the hand before it starts
# colliding.
# It is possible that another object is picked up in the meanwhile
# and we thus fill _collision_exceptions with new content.
# Hence using a copy of this list at the time of dropping the object.
#
# Note, this is static because our grab object gets destroyed before this code gets run.
static func _remove_collision_exceptions( \
	on_collision_hand : XRToolsCollisionHand, \
	exceptions : Array[PhysicsBody3D]):
	if not is_instance_valid(on_collision_hand):
		return

	# This can be improved by checking if we're still colliding and only
	# removing those objects from our exception list that are not.
	# If any are left, we can restart a new timer.
	# This will also allow us to use a much smaller timer interval

	# For now we'll remove all.

	for body : PhysicsBody3D in exceptions:
		if is_instance_valid(body):
			on_collision_hand.remove_collision_exception_with(body)
			body.remove_collision_exception_with(on_collision_hand)