extends Spatial


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


# Called when the node enters the scene tree for the first time.
func _ready():
	pass # Replace with function body.


# Called every frame. 'delta' is the elapsed time since the previous frame.
#func _process(delta):
#	pass

func _physics_process(delta):
	for g in get_tree().get_nodes_in_group("agent"):
		var bb = g
		var space_state = g.get_world().direct_space_state
		var up = Vector3(0, 1, 0)
		var npc_pos = g.global_transform.origin
		var front = -g.global_transform.basis[2]
		var right = g.global_transform.basis[0]
		var front_result = space_state.intersect_ray(npc_pos + up, npc_pos + up + front * 0.6,
			[g], 0xffff)
		bb.set("front_result", front_result)
		var front_right_result = space_state.intersect_ray(npc_pos + up + right * 0.1, npc_pos + up + front * 0.8 + right * 0.3,
			[g], 0xffff)
		bb.set("front_right_result", front_right_result)
		var front_left_result = space_state.intersect_ray(npc_pos + up - right * 0.1, npc_pos + up + front * 0.8 - right * 0.3,
			[g], 0xffff)
		bb.set("front_left_result", front_left_result)
		var low_obstacle_result = space_state.intersect_ray(npc_pos + up * 0.3, npc_pos + up * 0.3 + front * 0.6,
			[g], 0xffff)
		bb.set("low_obstacle_result", low_obstacle_result)
	for g in get_tree().get_nodes_in_group("agent"):
		var bb = g
		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 = bb.get("low_obstacle_result")
		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 left_col && right_col:
#		if low_obstacle:
#			npc.disable_gravity = true
#			var c = [["set", "parameters/climb_low_obstacle/active", true]]
#			npc.smart_obj(c)
		var npc = g
		var rot: Basis = npc.orientation.basis
		if front_col:
			var normal = front_result.normal
			var xnormal = npc.global_transform.xform_inv(normal)
			print("normal ", normal)
			print("xnormal ", xnormal)
#			var ok = false
#			if xnormal.x < 0:
#				rot = rot.rotated(Vector3(0, 1, 0), deg2rad(30 + 0.2 * (0.5 - randf())))
#				ok = true
#			elif xnormal.x > 0:
#				rot = rot.rotated(Vector3(0, 1, 0), deg2rad(-30 + 0.2 * (0.5 - randf())))
#				ok = true
		if left_col && !right_col:
			rot = rot.rotated(Vector3(0, 1, 0), deg2rad(-100 - 15.2 * (0.5 - randf())))
			print("l")
		elif right_col && !left_col:
			rot = rot.rotated(Vector3(0, 1, 0), deg2rad(100 + 15.2 * (0.5 - randf())))
			print("r")
		elif right_col && left_col:
			rot = rot.rotated(Vector3(0, 1, 0), npc.unp_fixup_angle)
#		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