extends AIScriptModule


# Declare member variables here. Examples:
# var a = 2
# var b = "text"


# Called when the node enters the scene tree for the first time.

var animtree: AnimationTree
var root_motion_track
func init(tick):
#	process_priority = 0
	pass

func can_walk_there(tick: AITick, dest: Vector3) -> bool:
	var root = get_character(tick)
	var space: PhysicsDirectSpaceState = root.get_world().get_direct_space_state()
	var result = space.intersect_ray(root.global_transform.origin, dest, [root])
	if result.has("collider"):
		return false
	else:
		return true

func update_physics(tick, delta):
	var root = get_character(tick)
	var orientation: Transform
	assert(root.has_meta("skeleton"))

	if root.has_meta("orientation"):
		orientation = root.get_meta("orientation")
	if !animtree:
		animtree = root.get_meta("animation_tree")
		if animtree:
			root_motion_track = animtree.root_motion_track
		else:
			return FAILED
	var skel = root.get_meta("skeleton")
	if !skel:
		return FAILED
	if characters.handle_cmdq(root):
		return ERR_BUSY
	if root.has_meta("cmdqueue"):
		var cmds = root.get_meta("cmdqueue")
		var cmd = cmds.pop_front()
		match cmd[0]:
#			"disable_physics":
#				var owners = get_shape_owners()
#				for e in owners:
#					shape_owner_set_disabled(e, true)
#				$CollisionShape.disabled = true
			"vehicle_mode_on":
				if root.has_meta("vehicle"):
					var v = root.get_meta("vehicle")
					assert(v is VehicleBody)
					if root is PhysicsBody:
						root.add_collision_exception_with(v)
					if v is PhysicsBody:
						v.add_collision_exception_with(root)
			"vehicle_mode_off":
				if root.has_meta("vehicle"):
					var v = root.get_meta("vehicle")
					assert(v is VehicleBody)
					if root is PhysicsBody:
						root.remove_collision_exception_with(v)
					if v is PhysicsBody:
						v.remove_collision_exception_with(root)
					root.remove_meta("seat")
					root.remove_meta("vehicle")
					root.remove_meta("orig_parent")
					v.remove_meta("driver")

			"jumpto":
				root.global_transform = cmd[1]
				orientation.basis = root.global_transform.basis
				if root.has_meta("vehicle"):
					var v = root.get_meta("vehicle")
					var et = root.global_transform
					var vt = v.global_transform
					var offset = vt.affine_inverse() * et
					root.set_meta("vehicle_offset", offset)
				root.set_meta("orientation", orientation)
#			"jump_to_obj":
#				var obj = cmd[1]
#				root.global_transform = obj.global_transform
#				orientation.basis = root.global_transform.basis
#				root.set_meta("orientation", orientation)
#			"jump_to_obj_rot":
#				var obj = cmd[1]
#				var xform = Transform()
#				xform.origin = obj.global_transform.origin
#				xform.basis = obj.global_transform.basis.rotated(Vector3(0, 1, 0), PI)
#				root.global_transform = xform
#				orientation.basis = root.global_transform.basis
#				root.set_meta("orientation", orientation)

#			"anim_state":
#				characters.animation_node_travel(root, cmd[1])
			"parent_to_vehicle":
				pass
#				if has_meta("vehicle"):
#					var v = get_meta("vehicle")
#					var et = global_transform.origin
#					var vt = v.global_transform.origin
#					var offset = et - vt
#					set_meta("vehicle_offset", offset)
			"col_ex_add", "add_col_exception":
				root.add_collision_exception_with(cmd[1])
			"col_ex_rm", "remove_col_exception":
				root.remove_collision_exception_with(cmd[1])
			"until_end":
				var check = cmd[1]
				var cur = characters.get_animation_node(root)
				var p = animtree["parameters/state/playback"].get_current_play_position()
				var l = animtree["parameters/state/playback"].get_current_length()
				if check == cur && p < l - 0.05:
					cmds.push_front(["until_end", cmd[1]])
#			"delay":
#				var d = cmd[1]
#				if d > 0:
#					d = d - 1
#					cmds.push_front(["delay", d])
			"disable_collision":
				if root is PhysicsBody:
					var mask = root.collision_mask
					root.set_meta("orig_mask", mask)
					root.collision_mask = 0
					root.set_meta("physics", false)
					animtree.root_motion_track = ""
			"enable_collision":
				if root is PhysicsBody:
					var mask = root.get_meta("orig_mask")
					root.collision_mask = mask
					root.remove_meta("orig_mask")
					root.set_meta("physics", true)
					animtree.root_motion_track = root_motion_track
			"show_marker":
				if skel.has_node(cmd[1]):
					skel.get_node(cmd[1]).show()
			"hide_marker":
				if skel.has_node(cmd[1]):
					skel.get_node(cmd[1]).hide()
			"register_marker":
				if skel.has_node(cmd[2]):
					markers.init_marker(cmd[1], skel.get_node(cmd[2]), cmd[3])
#			"anim_param":
#				animtree["parameters/" + cmd[1]] = cmd[2]
			"equip":
				inventory.equip(skel.get_node(cmd[1]), cmd[2])
			"walkto":
				if !root.has_meta("cmdq_walk"):
					root.set_meta("cmdq_walk", true)
				var can_walk = true
				var fwd_probe = characters.forward_probe(root, 0.5, 0.05, 1.9, 0.2)
				var climb = false
				if fwd_probe < 0.1:
					if !characters.get_animation_node(root) == "climb1":
						characters.animation_node_travel(root, "locomotion");
						climb = false
#						print("NO climb!!!")
				elif fwd_probe < 1.2:
#					print("trying climb!!!")
					climb = true
				else:
					can_walk = false
				if can_walk:
					characters.set_walk_speed(root, 0.4, 0)
				else:
					characters.set_walk_speed(root, 0.0, 0)
#				animtree["parameters/state/locomotion/loc/blend_position"].x = 0.5
#				animtree["parameters/state/locomotion/loc/blend_position"].y = 0
				var gt: Transform = root.global_transform
				var mp: Transform = root.global_transform
				var target: = Vector3()
				if cmd[1] is Spatial:
					target = cmd[1].global_transform.origin
				else:
					target = cmd[1]
				target.y = root.global_transform.origin.y
				var current_dir = root.global_transform.xform(Vector3(0, 0, -1))
				var locnav_probe = characters.locnav_probe(root, 3.0, (target - mp.origin).normalized(), 0.15)
#				var where_to = target.linear_interpolate(gt.origin + locnav_probe, 0.3)
#				var where_to = mp.origin + current_dir * 0.8 + locnav_probe * 0.0 + (target - mp.origin).normalized() * 0.2
				var where_to = mp.origin + (target - mp.origin).normalized()
				if gt.origin != where_to:
					gt = gt.looking_at(where_to, Vector3.UP)
				gt.origin = Vector3()
#				print("target: ", target)
				var timeout = cmd[3]
				timeout -= delta
				orientation.basis = gt.basis
				root.set_meta("orientation", orientation)
				root.global_transform.basis = gt.basis
				var d = root.global_transform.origin.distance_to(target)
				if climb:
					var cmd_new = ["walkto", cmd[1], cmd[2], timeout + delta]
					cmds.push_front(cmd_new)
					cmd_new = ["climb", "1"]
					cmds.push_front(cmd_new)
				elif timeout >= 0.0 && d > cmd[2]:
					if can_walk:
						var cmd_new = ["walkto", cmd[1], cmd[2], timeout]
						cmds.push_front(cmd_new)
				elif d > cmd[2]:
					var dir = target - root.global_transform.origin
					dir = dir.normalized() * (d - cmd[2])
					root.global_transform.origin += dir
					characters.set_walk_speed(root, 0.0, 0.0)
					root.remove_meta("cmdq_walk")
				else:
					characters.set_walk_speed(root, 0.0, 0.0)
					root.remove_meta("cmdq_walk")
			"set_other":
				var cmdq_other = []
				if cmd[1].has_meta("cmdqueue"):
					cmdq_other = cmd[1].get_meta("cmdqueue")
				cmdq_other.push_back(cmd[2])
				cmd[1].set_meta("cmdqueue", cmdq_other)
			"wait_for_completion":
				if !cmd[1].has_meta("complete_" + cmd[2]):
					cmds.push_front([cmd[0], cmd[1], cmd[2]])
				cmd[1].remove_meta("complete_" + cmd[2])
			"complete":
				root.set_meta("complete_" + cmd[1], true)
				
			"set_transforms":
				var master_xform: Transform = cmd[1].global_transform
				var slave_xform = master_xform
				slave_xform.basis = master_xform.basis.rotated(Vector3.UP, PI)
				slave_xform.origin = master_xform.origin - master_xform.basis[2] * 0.375
				var slave_orientation = slave_xform
				slave_orientation.origin = Vector3()
				cmd[2].set_meta("orientation", slave_orientation)
				cmd[2].global_transform = slave_xform
			"climb":
				if !root.has_meta("last_climb"):
					characters.animation_node_travel(root, "climb" + cmd[1])
					root.set_meta("last_climb", "climb" + cmd[1])
				elif root.get_meta("last_climb") == "climb" + cmd[1]:
					characters.animation_node_travel(root, "climb" + cmd[1] + "a")
					root.set_meta("last_climb", "climb" + cmd[1] + "a")
				else:
					characters.animation_node_travel(root, "climb" + cmd[1])
					root.set_meta("last_climb", "climb" + cmd[1])
				cmds.push_front(["climb_wait", cmd[1]])
				root.set_meta("climb", true)
			"climb_wait":
				var cur = characters.get_animation_node(root)
				if cur.begins_with("climb" + cmd[1]):
					cmds.push_front(["climb_wait", cmd[1]])
				else:
					root.remove_meta("climb")

		if cmds.size() > 0:
			root.set_meta("cmdqueue", cmds)
		else:
			root.remove_meta("cmdqueue")
	return ERR_BUSY

func update(tick, delta):
	return ERR_BUSY