class_name XRToolsGrabDriver
extends RemoteTransform3D


## Grab state
enum State {
	LERP,
	SNAP,
}


## Drive state
var state : State = State.SNAP

## Target pickable
var target : XRToolsPickable

## Primary grab information
var primary : Grab = null

## Secondary grab information
var secondary : Grab = null

## Lerp start position
var lerp_start : Transform3D

## Lerp total duration
var lerp_duration : float = 1.0

## Lerp time
var lerp_time : float = 0.0


# Called every frame. 'delta' is the elapsed time since the previous frame.
func _physics_process(delta : float) -> void:
	# Skip if no primary node
	if not is_instance_valid(primary):
		return

	# Set destination from primary grab
	var destination := primary.by.global_transform * primary.transform.inverse()

	# If present, apply secondary-node contributions
	if is_instance_valid(secondary):
		# Calculate lerp coefficients based on drive strengths
		var position_lerp := _vote(primary.drive_position, secondary.drive_position)
		var angle_lerp := _vote(primary.drive_angle, secondary.drive_angle)

		# Calculate the transform from secondary grab
		var x1 := destination
		var x2 := secondary.by.global_transform * secondary.transform.inverse()

		# Independently lerp the angle and position
		destination = Transform3D(
			x1.basis.slerp(x2.basis, angle_lerp),
			x1.origin.lerp(x2.origin, position_lerp))

		# Test if we need to apply aiming
		if secondary.drive_aim > 0.0:
			# Convert destination from global to primary-local
			destination = primary.by.global_transform.inverse() * destination

			# Calculate the from and to vectors in primary-local space
			var secondary_from := destination * secondary.transform.origin
			var secondary_to := primary.by.to_local(secondary.by.global_position)

			# Build shortest arc
			secondary_from = secondary_from.normalized()
			secondary_to = secondary_to.normalized()
			var spherical := Quaternion(secondary_from, secondary_to)

			# Build aim-rotation
			var rotate := Basis.IDENTITY.slerp(Basis(spherical), secondary.drive_aim)
			destination = Transform3D(rotate, Vector3.ZERO) * destination

			# Convert destination from primary-local to global
			destination = primary.by.global_transform * destination

	# Handle update
	match state:
		State.LERP:
			# Progress the lerp
			lerp_time += delta
			if lerp_time < lerp_duration:
				# Interpolate from lerp_start to destination
				destination = lerp_start.interpolate_with(
					destination,
					lerp_time / lerp_duration)
			else:
				# Lerp completed
				state = State.SNAP
				if primary: primary.set_arrived()
				if secondary: secondary.set_arrived()

	# Apply the destination transform
	global_transform = destination
	force_update_transform()
	if is_instance_valid(target):
		target.force_update_transform()


## Set the secondary grab point
func add_grab(p_grab : Grab) -> void:
	# Set the secondary grab
	if p_grab.hand_point and p_grab.hand_point.mode == XRToolsGrabPointHand.Mode.PRIMARY:
		print_verbose("%s> new primary grab %s" % [target.name, p_grab.by.name])
		secondary = primary
		primary = p_grab
	else:
		print_verbose("%s> new secondary grab %s" % [target.name, p_grab.by.name])
		secondary = p_grab

	# If snapped then report arrived at the new grab
	if state == State.SNAP:
		p_grab.set_arrived()


## Get the grab information for the grab node
func get_grab(by : Node3D) -> Grab:
	if primary and primary.by == by:
		return primary

	if secondary and secondary.by == by:
		return secondary

	return null


func remove_grab(p_grab : Grab) -> void:
	# Remove the appropriate grab
	if p_grab == primary:
		# Remove primary (secondary promoted)
		print_verbose("%s> %s (primary) released" % [target.name, p_grab.by.name])
		primary = secondary
		secondary = null
	elif p_grab == secondary:
		# Remove secondary
		print_verbose("%s> %s (secondary) released" % [target.name, p_grab.by.name])
		secondary = null


# Discard the driver
func discard():
	remote_path = NodePath()
	queue_free()


# Create the driver to lerp the target from its current location to the
# primary grab-point.
static func create_lerp(
	p_target : Node3D,
	p_grab : Grab,
	p_lerp_speed : float) -> XRToolsGrabDriver:

	print_verbose("%s> lerping %s" % [p_target.name, p_grab.by.name])

	# Construct the driver lerping from the current position
	var driver := XRToolsGrabDriver.new()
	driver.name = p_target.name + "_driver"
	driver.top_level = true
	driver.process_physics_priority = -80
	driver.state = State.LERP
	driver.target = p_target
	driver.primary = p_grab
	driver.global_transform = p_target.global_transform

	# Calculate the start and duration
	var end := p_grab.by.global_transform * p_grab.transform
	var delta := end.origin - p_target.global_position
	driver.lerp_start = p_target.global_transform
	driver.lerp_duration = delta.length() / p_lerp_speed

	# Add the driver as a neighbor of the target as RemoteTransform3D nodes
	# cannot be descendands of the targets they drive.
	p_target.get_parent().add_child(driver)
	driver.remote_path = driver.get_path_to(p_target)

	# Return the driver
	return driver


# Create the driver to instantly snap to the primary grab-point.
static func create_snap(
	p_target : Node3D,
	p_grab : Grab) -> XRToolsGrabDriver:

	print_verbose("%s> snapping to %s" % [p_target.name, p_grab.by.name])

	# Construct the driver snapped to the held position
	var driver := XRToolsGrabDriver.new()
	driver.name = p_target.name + "_driver"
	driver.top_level = true
	driver.process_physics_priority = -80
	driver.state = State.SNAP
	driver.target = p_target
	driver.primary = p_grab
	driver.global_transform = p_grab.by.global_transform * p_grab.transform.inverse()

	# Snapped to grab-point so report arrived
	p_grab.set_arrived()

	# Add the driver as a neighbor of the target as RemoteTransform3D nodes
	# cannot be descendands of the targets they drive.
	p_target.get_parent().add_child(driver)
	driver.remote_path = driver.get_path_to(p_target)

	# Return the driver
	return driver


# Calculate the lerp voting from a to b
static func _vote(a : float, b : float) -> float:
	if a == 0.0 and b == 0.0:
		return 0.0

	return b / (a + b)