extends BTAction
class_name BTLookAtPath
func tick(tick: Tick) -> int:
	var npc = tick.actor
	var bb = tick.blackboard
	var point = npc.get_meta("path_point")
#	print("look path point: ", point)
#	var space_state = npc.get_world().direct_space_state
	var result
	var correction = 0
	var xf = Transform()
#	var right = npc.orientation.basis[0] * 0.2
#	var front = - npc.orientation.basis[2]* 1.2
#	var up = npc.orientation.basis[1] * 0.9
	var front_result = bb.get("front_result")
	if front_result.has("collider"):
		correction |= (1 << 0)
	result = bb.get("front_right_result")
	if result.has("collider"):
		correction |= (1 << 1)
	result = bb.get("front_left_result")
	if result.has("collider"):
		correction |= (1 << 2)
#	if correction != 0:
#		npc.do_stop()
	# 1 = FRONT
	# 2 = RIGHT
	# 4 = LEFT
	if !npc.has_meta("unp_c"):
		npc.set_meta("unp_c", (PI/1.5 + PI / 4.0 * randf()) * sign(0.5 - randf()))
	var c_angle = npc.get_meta("unp_c")
	match(correction):
		1, 6, 7:
			xf = npc.orientation.rotated(Vector3(0, 1, 0), c_angle)
#			npc.orientation.basis = xf.basis
			if randf() > 0.6:
				npc.smart_obj([["set", "parameters/unstuck1/active", "true"]])
			elif randf() > 0.4:
				npc.smart_obj([["set", "parameters/unstuck2/active", "true"]])
			elif randf() > 0.2:
				npc.smart_obj([["set", "parameters/turn_right/active", "true"]])
			elif randf() > 0.1:
				npc.smart_obj([["set", "parameters/turn_left/active", "true"]])
			print("correction: ", correction)

#			npc.do_stop()
			npc.set_meta("walk_cooldown", 2.0)
#			return FAILED
				
		2, 3:
			npc.smart_obj([["set", "parameters/turn_left/active", "true"]])
			npc.set_meta("walk_cooldown", 0.5)
#			return FAILED
		4, 5:
			npc.smart_obj([["set", "parameters/turn_right/active", "true"]])
			npc.set_meta("walk_cooldown", 0.5)
#			return FAILED
#		1, 2, 3, 4, 5, 6, 7:
#			npc.orientation.basis = npc.orientation.interpolate_with(xf, tick.blackboard.get("delta")).basis
#	if !npc.has_meta("path_point"):
#		npc.do_stop()
	if correction in [0, 1, 2, 3, 4, 5]:
		var path = npc.get_meta("path")
		var npc_pos = npc.global_transform.origin
		point.y = npc_pos.y
		var dir = Vector3()
		if path.size() > 2:
			var p1 = path[0]
			var p2 = path[1]
			p1.y = npc_pos.y
			p2.y = npc_pos.y
			var dir1 = (point - npc_pos).normalized()
			var dir2 =  (p1 - npc_pos).normalized()
			var dir3 =  (p2 - npc_pos).normalized()
			dir = (dir1 * 0.9 + dir2 * 0.5 + dir3 * 0.5).normalized()
		else:
			dir = (point - npc_pos).normalized()
		var npc_xform = npc.global_transform
	#	print("dir: ", dir)
		if front_result.has("collider"):
			var steer = front_result.normal * 1.5
			steer.y = 0.0
			dir = (dir + steer).normalized()
		
		if dir.length() > 0:
			xf = npc_xform.looking_at(npc_pos + dir, Vector3(0, 1, 0))
			npc.orientation.basis = npc.orientation.basis.slerp(xf.basis, tick.blackboard.get("delta"))
#		for k in get_tree().get_nodes_in_group("debug_cube"):
#			k.queue_free()
#		var cube = CubeMesh.new()
#		cube.size = Vector3(0.5, 0.5, 0.5)
#		var mi = MeshInstance.new()
#		mi.mesh = cube
#		var r = get_node("/root")
#		r.add_child(mi)
#		mi.global_transform.origin = point
#		mi.add_to_group("debug_cube")
#	if !correction in [0, 2, 4]:
#		npc.remove_meta("path")
#		npc.remove_meta("path_valid")
#		npc.remove_meta("target_loc")
#		npc.remove_meta("target_group")
#		npc.set_meta("path_cooldown", 1.0)
#		print("correction: ", correction)
	return OK