extends Node
var grab_queue = []
var ungrab_queue = []
func _ready():
	pass # Replace with function body.
func grab_character(gch, ch):
	if !ch.get_meta("grabbed"):
		ch.set_meta("grabbed", true)
		ch.set_meta("grabbed_time", 20.0)
		if !ch in grab_queue:
			grab_queue.push_back([gch, ch])
		gch.set_meta("grabbing", true)
		gch.do_grab()
func ungrab_character(gch, finisher = null):
	if !grab_data.has(gch):
		return
	var ch = grab_data[gch].item
	ch.set_meta("grabbed", false)
	ch.set_meta("grabbed_time", 0.0)
	if !ch in ungrab_queue:
		if finisher:
			ungrab_queue.push_back([gch, ch, finisher])
		else:
			ungrab_queue.push_back([gch, ch])
	gch.set_meta("grabbing", false)
	gch.do_ungrab()
var grab_data = {}
func get_grabbed(gch):
	if grab_data.has(gch):
		return grab_data[gch].item
	else:
		return null
func get_bone_xform(ch, b):
	var skel = ch.get_skeleton()
	var bone_id = skel.find_bone(b)
	assert(bone_id >= 0)
	var xform = skel.get_bone_global_pose(bone_id)
	return ch.global_transform * xform
func left_arm_ik_enable(m: Spatial, _s: Spatial, target: Spatial):
#	var item_skel = s.get_skeleton()
	var main_skel = m.get_skeleton()
	var ik: = SkeletonIK.new()
#	var xf = get_bone_xform(m, "wrist_L")
	var conv = target
	ik.root_bone = "upperarm01_L"
	ik.tip_bone = "wrist_L"
	main_skel.add_child(ik)
	var tpath = ik.get_path_to(conv)
	ik.target_node = tpath
	ik.min_distance = 0.01
	ik.magnet = Vector3(-8, -10, 0)
	ik.use_magnet = true
	ik.start()
	m.set_meta("left_arm_ik", m.get_path_to(ik))
func left_arm_ik_disable(m):
	if m.has_meta("left_arm_ik"):
		var ik = m.get_node(m.get_meta("left_arm_ik"))
#		print("ik stopped")
		ik.interpolation = 0.0
		ik.use_magnet = false
		ik.stop()
		ik.queue_free()
func left_arm_front_neck_ik_enable(m, s):
	var target = s.get_front_neck_target()
	left_arm_ik_enable(m, s, target)
func left_arm_front_neck_ik_disable(m):
	left_arm_ik_disable(m)
func _physics_process(_delta):
	while grab_queue.size() > 0:
		var characters = grab_queue.pop_front()
		var item: KinematicBody = characters[1]
		var gch = characters[0]
		if grab_data.has(gch):
			continue
		grab_data[gch] = {}
		item.do_stop()
		item.do_grabbed()
		gch.add_collision_exception_with(item)
		item.add_collision_exception_with(gch)
#		item.do_disable_collision()
		item.global_transform = gch.global_transform
#		var grab_xform = Transform()
#		grab_xform.origin = gch.calc_offset(item, "neck02")
		grab_data[gch].item = item
#		grab_data[gch].grab_attachment = gch.add_attachment(gch.grab_bone, grab_xform)
		grab_data[gch].grabbed_mask = item.collision_mask
		grab_data[gch].grabbed_parent = item.get_parent()
		item.collision_mask = 0
		item.get_parent().remove_child(item)
#		grab_data[gch].grab_attachment.add_child(item)
		gch.add_child(item)
		item.transform = Transform()
		item.orientation.basis = Basis()
		var ik_enable = true
		if ik_enable:
			left_arm_front_neck_ik_enable(gch, item)
	while ungrab_queue.size() > 0:
		var characters = ungrab_queue.pop_front()
		var item: KinematicBody = characters[1]
		var gch = characters[0]
		var finisher = null
		if characters.size() >= 3:
			finisher = characters[2]
		var tf = item.global_transform
#		print("ik disable")
		left_arm_front_neck_ik_disable(gch)
#		grab_data[gch].grab_attachment.remove_child(item)
		gch.remove_child(item)
		grab_data[gch].grabbed_parent.add_child(item)
		gch.remove_collision_exception_with(item)
		item.remove_collision_exception_with(gch)
		item.collision_mask = grab_data[gch].grabbed_mask
		item.global_transform = tf
		if !finisher:
			item.do_stop()
			item.do_ungrabbed()
		else:
#			print("finisher: ", finisher)
			item.smart_obj(finisher)
		item.set_meta("grabbed", false)
		grab_data.erase(gch)
	for k in grab_data.keys():
		grab_data[k].item.transform = Transform()