extends BTAction
class_name BTWalk
#func npc_stuck(npc, cond):
#	if cond:
#		if npc.has_meta("stuck"):
#			var stuck = npc.get_meta("stuck")
#			stuck += 2.0 * get_process_delta_time()
#			npc.set_meta("stuck", stuck)
#		else:
#			npc.set_meta("stuck", 0.4 * get_process_delta_time())
#		print(npc.name, " stuck ", npc.get_meta("stuck"))
#	else:
#		if npc.has_meta("stuck"):
#			var stuck = npc.get_meta("stuck")
#			stuck -= 0.1 * get_process_delta_time()
#			print(npc.name, " unstucking: ", stuck)
#			if stuck > 0.0:
#				npc.set_meta("stuck", stuck)
#			else:
#				npc.set_meta("stuck", null)
#func is_npc_stuck(npc):
#	if npc.has_meta("stuck"):
#		var stuck = npc.get_meta("stuck")
#		if stuck > 0.0:
#			return true
#	return false
		
func tick(tick: Tick) -> int:
	var npc = tick.actor
	var bb = tick.blackboard
#	npc.disable_gravity = false
	if npc.has_meta("walk_cooldown"):
		var cd = npc.get_meta("walk_cooldown")
		if cd >= 0.0:
			cd -= get_process_delta_time()
			npc.set_meta("walk_cooldown", cd)
			return FAILED
		else:
			npc.remove_meta("walk_cooldown")
	
	if npc.has_meta("action"):
		if npc.get_meta("action") == "run_away":
			var p = [["set", "parameters/main/Motion/walk/walking/blend_position", 1.0]]
			npc.smart_obj(p)
		elif npc.get_meta("action") == "unstuck":
			return OK
		else:
			var p = [["set", "parameters/main/Motion/walk/walking/blend_position", 0.0]]
			npc.smart_obj(p)
#	var front = - npc.orientation.basis[2]* 0.6
#	var up = npc.orientation.basis[1]
#	var npc_pos = npc.global_transform.origin
#	var space_state = npc.get_world().direct_space_state
#	var front_result = space_state.intersect_ray(npc_pos + up, npc_pos + up + front,
#		[npc], 0xffff)
	var front_result = bb.get("front_result")
#	var front_left_result = bb.get("front_left_result")
#	var front_right_result = bb.get("front_right_result")
	var low_obstacle_result: = {}
	for k in ["low_obstacle_result"]:
		low_obstacle_result = bb.get("low_obstacle_result")
		if low_obstacle_result.has("collider"):
			break
	
	var front_col = false
#	var left_col = false
#	var right_col = false
	var low_obstacle = false
	if front_result.has("collider"):
		var col = front_result.collider
		if !col.is_in_group("entry"):
			front_col = true
		front_col = true
#	if front_left_result.has("collider"):
#		var col = front_left_result.collider
#		if !col.is_in_group("entry"):
#			left_col = true
#		left_col = true
#	if front_right_result.has("collider"):
#		var col = front_right_result.collider
#		if !col.is_in_group("entry"):
#			right_col = true
#		right_col = true
	if !front_col:
		if low_obstacle_result.has("collider"):
			var col = low_obstacle_result.collider
			if !col.is_in_group("entry"):
				low_obstacle = true
	if low_obstacle:
#		npc.disable_gravity = true
		var c = [["set", "parameters/climb_low_obstacle/active", true]]
		npc.smart_obj(c)
	else:
		npc.do_walk()
#	if !low_obstacle && !front_col:
##		npc_stuck(npc, false)
#		npc.do_walk()
##		if npc.has_meta("stuck"):
##			npc.set_meta("stuck", null)
#		return OK
#	elif !low_obstacle && front_col:
#		var normal = front_result.normal
#		var xnormal = npc.global_transform.xform_inv(normal)
#		print("xnormal ", xnormal)
#		var rot: Basis = npc.orientation.basis
#		var ok = false
#		if xnormal.z < 0:
#			rot = rot.rotated(Vector3(0, 1, 0), deg2rad(-20 + 3.0 * (0.5 - randf())))
#			ok = true
#		elif xnormal.z > 0:
#			rot = rot.rotated(Vector3(0, 1, 0), deg2rad(20 + 3.0 * (0.5 - randf())))
#			ok = true
##		else:
##			rot = rot.rotated(Vector3(0, 1, 0), deg2rad(10 + 10.0 * (0.5 - randf())))
#		var cur: Basis = npc.orientation.basis
#		cur = cur.slerp(rot, get_process_delta_time())
#		npc.orientation.basis = cur
#		if ok:
#			npc.do_walk()
#		return OK
#	elif !low_obstacle && left_col && !right_col:
#		var rot: Basis = npc.orientation.basis.rotated(Vector3(0, 1, 0), deg2rad(-30))
#		var cur: Basis = npc.orientation.basis
#		cur = cur.slerp(rot, get_process_delta_time())
#		npc.orientation.basis = cur
#		npc_stuck(npc, false)
#		if !is_npc_stuck(npc):
#			npc.do_walk()
#		return OK
#	elif !low_obstacle && !left_col && right_col:
#		var rot: Basis = npc.orientation.basis.rotated(Vector3(0, 1, 0), deg2rad(30))
#		var cur: Basis = npc.orientation.basis
#		cur = cur.slerp(rot, get_process_delta_time())
#		npc.orientation.basis = cur
#		npc_stuck(npc, false)
#		if !is_npc_stuck(npc):
#			npc.do_walk()
#		return OK
#	elif !low_obstacle && left_col && right_col:
#		var rot: Basis = npc.orientation.basis.rotated(Vector3(0, 1, 0), deg2rad(100))
#		var cur: Basis = npc.orientation.basis
#		cur = cur.slerp(rot, get_process_delta_time())
#		npc.orientation.basis = cur
#		npc_stuck(npc, false)
#		print("correction")
#		npc.do_stop()
#		if !is_npc_stuck(npc):
#			npc.do_walk()
#		return OK
#	elif !low_obstacle && left_col && right_col:
#		var rot: Basis = npc.orientation.basis.rotated(Vector3(0, 1, 0), deg2rad(90))
#		var cur: Basis = npc.orientation.basis
#		cur = cur.slerp(rot, get_process_delta_time())
#		npc.orientation.basis = cur
#		npc.do_walk()
#		npc_stuck(npc, true)
#		return FAILED
#	elif !low_obstacle:
#		var rot: Basis = npc.orientation.basis.rotated(Vector3(0, 1, 0), deg2rad(-45))
#		var cur: Basis = npc.orientation.basis
#		cur = cur.slerp(rot, get_process_delta_time())
#		npc.orientation.basis = cur
#		npc.do_walk()
#		if npc.has_meta("stuck"):
#			var stuck = npc.get_meta("stuck")
#			stuck += 2.0 * get_process_delta_time()
#			npc.set_meta("stuck", stuck)
#		else:
#			npc.set_meta("stuck", 0.0)
#		npc_stuck(npc, true)
#		return FAILED
	return OK

#	print(npc.name, " walk")