extends KinematicBody

const MOTION_INTERPOLATE_SPEED = 10

var orientation = Transform()
var root_motion = Transform()
var fixup = Transform()
var motion = Vector2()
var motion_target = Vector2()
var velocity = Vector3()
var disable_gravity = false
const p1 = "parameters/main/playback"
const p2 = "parameters/main/Motion/playback"


func _ready():
#	var _ap = $AnimationTree.get_node($AnimationTree.get_animation_player())
	orientation = global_transform
	orientation.origin = Vector3()
	add_to_group("characters")
	do_ungrab()
#	feet_ik_init()


func _physics_process(delta):
#	motion = motion.linear_interpolate(motion_target, MOTION_INTERPOLATE_SPEED * delta)
	assert(has_meta("grabbed"))
	var in_smart_obj = get_meta("smart_object")
	if get_meta("grabbed"):
		orientation.basis = get_parent().global_transform.basis
	root_motion = $AnimationTree.get_root_motion_transform()
	orientation *= root_motion
	var h_velocity = orientation.origin / delta
	velocity.x = h_velocity.x
	velocity.z = h_velocity.z
	velocity.y = h_velocity.y
	if !get_meta("grabbed") && !in_smart_obj && !disable_gravity:
		velocity.y += -9.8
	velocity = move_and_slide(velocity,Vector3(0,1,0))
	orientation.origin = Vector3()
	fixup.origin = Vector3()
	orientation *= Transform().interpolate_with(fixup, delta)
	orientation = orientation.orthonormalized()
	global_transform.basis = orientation.basis
	if get_meta("grabbed"):
		global_transform.origin = get_parent().global_transform.origin
#		global_transform = get_parent().global_transform
	if attacking >= 0.0:
		attacking -= delta
	rpg.update_stats(self)
#	feet_ik()
var skel
func get_skeleton():
	if skel:
		return get_node(skel)
	var queue = [self]
	while queue.size() > 0:
		var item = queue.pop_front()
		if item is Skeleton:
			skel = get_path_to(item)
			return get_node(skel)
		for k in item.get_children():
			queue.push_back(k)
func get_anim_player():
	return $male_2018/male_g_2018/AnimationPlayer
var attachment_start
var attachment_end
var mod_p = false
func get_front_neck_target():
	if !mod_p:
		$male_2018/male_g_2018/neck02BoneAttachment/grab_neck_front.transform.basis = Basis()
		mod_p = true
	return $male_2018/male_g_2018/neck02BoneAttachment/grab_neck_front
func add_attachment(bname, xform):
	var ba: BoneAttachment = BoneAttachment.new()
	ba.bone_name = bname
	var sk = get_skeleton()
	sk.add_child(ba)
	var on1 = Spatial.new()
	ba.add_child(on1)
	var on2 = Spatial.new()
	ba.add_child(on2)
	on1.transform = Transform()
#	on1.global_transform.basis = Basis()
	on2.transform = xform
#	on2.rotate_y(PI)
	attachment_start = ba
	attachment_end = on2
	return on2

static func calc_offset(c2, bone2):
	var skel2 = c2.get_skeleton()
	var id2 = skel2.find_bone(bone2)
	assert(skel2 != null)
	assert(id2 >= 0)
	var offt2 = skel2.get_bone_global_pose(id2).origin
	var offset = Vector3(-offt2.x, -offt2.y, -offt2.z)
#	print("offset: ", offset)
	return offset

var foot_ik_l = {
	"foot": "foot_L",
	"bone1": "upperleg02_L",
	"bone2": "lowerleg01_L",
	"h": -0.3
}

var foot_ik_r = {
	"foot": "foot_R",
	"bone1": "upperleg02_R",
	"bone2": "lowerleg01_R",
	"h": -0.3
}
func foot_ik_reset(sk: Skeleton, data: Dictionary, obj: Spatial):
	var bone1 = sk.fine_bone(data.bone1)
	var bone2 = sk.fine_bone(data.bone2)
	var foot = sk.find_bone(data.foot)
	assert(foot >= 0 && bone1 >= 0 && bone2 >= 0)
	var xf = sk.get_bone_global_pose(foot)
	var xf1 = sk.get_bone_global_pose(bone1)
	var xf2 = sk.get_bone_global_pose(bone2)
	var l1 = xf1.origin.distance_to(xf2.origin)
	var l2 = xf2.origin.distance_to(xf.origin)
	var lt = xf1.origin.distance_squared_to(xf.origin)
	var cosa = l1 * (l1 + l2 * l2 + lt) / (2 * l1 * l2)
	var v1 = xf2.origin - xf1.origin
	var v2 = xf.origin - xf1.origin
	var rot_axis = v1.cross(v2).normalized()
	var space_state = get_world().direct_space_state
	var xform = global_transform
	var f = (xform * xf).origin
	var result = space_state.intersect_ray(f, f + Vector3(0, data.h, 0), [self], 0xffff)
	if result.has("position"):
		print(data.foot, result.position)
		obj.global_transform.origin = result.position

func feet_ik():
	var sk: Skeleton = get_skeleton()
	var foot1 = sk.find_bone("foot_L")
	var foot2 = sk.find_bone("foot_R")
	assert(foot1 >= 0 && foot2 >= 0)
	var xf1 = sk.get_bone_global_pose(foot1)
	var xf2 = sk.get_bone_global_pose(foot2)
	var space_state = get_world().direct_space_state
	var xform = global_transform
	var f1 = (xform * xf1).origin
	var f2 = (xform * xf2).origin
	var result1 = space_state.intersect_ray(f1, f1 + Vector3(0, -0.3, 0), [self], 0xffff)
	var result2 = space_state.intersect_ray(f2, f2 + Vector3(0, -0.3, 0), [self], 0xffff)
	if result1.has("position"):
		print("L: ", result1.position)
		foot_ik_target_l.global_transform.origin = result1.position
	if result2.has("position"):
		print("R: ", result2.position)
		foot_ik_target_r.global_transform.origin = result2.position
var foot_ik_target_l
var foot_ik_target_r
func feet_ik_init():
	var s1 = Spatial.new()
	var s2 = Spatial.new()
	var sk: Skeleton = get_skeleton()
	var foot1 = sk.find_bone("foot_L")
	var foot2 = sk.find_bone("foot_R")
	var xf1 = sk.get_bone_global_pose(foot1)
	var xf2 = sk.get_bone_global_pose(foot2)
	s1.transform = xf1
	s2.transform = xf2
	foot_ik_target_l = s1
	foot_ik_target_r = s2
#	var ik1 : = SkeletonIK.new()
#	ik1.root_bone = "upperleg02_L"
#	ik1.tip_bone = "foot_L"
#	ik1.target_node = get_path_to(foot_ik_target_l)
#	ik1.interpolation = 0.1
#	sk.add_child(ik1)
#	var ik2 = SkeletonIK.new()
#	ik2.root_bone = "upperleg02_R"
#	ik2.tip_bone = "foot_R"
#	ik2.target_node = get_path_to(foot_ik_target_r)
#	ik2.interpolation = 0.1
#	sk.add_child(ik2)
#	ik1.start(true)
#	ik2.start(true)
var act = {
	"stop":[
		["travel", p1, "Motion"],
		["travel", p2, "stand"],
	],
	"walk":[
		["travel", p1, "Motion"],
		["travel", p2, "walk"],
	],
	"grabbed":[
		["set", "parameters/free_grabbed/current", 1],
		["set", "parameters/grabbed_seek/seek_position", 0.0]
	],
	"ungrabbed":[
		["set", "parameters/free_grabbed/current", 0]
	],
	"grab":[
		["set", "parameters/grab/blend_amount", 1.0],
		["set", "parameters/grab_seek/seek_position", 0.0]
	],
	"grab_attack1_on":[
		["set", "parameters/grab_attack/active", true],
	],
	"grabbed_attacked1_on":[
		["set", "parameters/grabbed_attacked/active", true],
	],
	"ungrab":[
		["set", "parameters/grab/blend_amount", 0.0]
	],
	"throw_projectile":[
		["set", "parameters/throw/active", true]
	]
}
func smart_obj(sm):
	for k in sm:
		if k[0] == "travel":
			$AnimationTree[k[1]].travel(k[2])
		if k[0] == "set":
			$AnimationTree[k[1]] = k[2]
func do_stop():
#	if name != "player":
#		print(name, " stop")
	smart_obj(act.stop)
func do_walk():
#	if name != "player":
#		print(name, "walk")
	smart_obj(act.walk)
func set_walk_speed(x):
	var s = [["set", "parameters/main/Motion/walk/walk_speed/scale", x]]
	smart_obj(s)
var grab_bone = "grabber_L"
func do_grab():
	smart_obj(act.grab)
var attacking = 0.0
func do_attack():
	if attacking > 0.0:
		return
	if get_meta("grabbing"):
		smart_obj(act.grab_attack1_on)
		var s = grabbing.get_grabbed(self)
		s.do_attacked()
		rpg.melee_attack(self, s)
		attacking += 1.0
	else:
		smart_obj(act.throw_projectile)
func do_attacked():
	if get_meta("grabbed"):
		smart_obj(act.grabbed_attacked1_on)
func do_ungrab():
	smart_obj(act.ungrab)
func do_grabbed():
	smart_obj(act.grabbed)
func do_ungrabbed():
	smart_obj(act.ungrabbed)