extends Node2D

const movement_speed = 100

var single = NavigationPolygon.new()
var double = NavigationPolygon.new()




func _ready():
	single.agent_radius = 0.75
	double.agent_radius = 1.5
	var root_node: Node3D = get_node("NavigationMeshBakingRootNode")
	
	NavigationServer2D.parse_source_geometry_data(single, $nav.navigation_polygon, root_node)	
	
	for s in [single, double]:
		NavigationServer2D.bake_from_source_geometry_data(single, $nav.navigation_polygon)
		NavigationServer2D.bake_from_source_geometry_data(double, $nav.navigation_polygon)
		
	var single_map: RID = NavigationServer2D.map_create()
	var double_map: RID = NavigationServer2D.map_create()

	NavigationServer2D.map_set_active(single_map, true)
	NavigationServer2D.map_set_active(double_map, true)
	
	var single_reg: RID = NavigationServer2D.region_create()
	var double_reg: RID = NavigationServer2D.region_create()
	
	NavigationServer2D.region_set_map(single_reg, single_map)
	NavigationServer2D.region_set_map(double_reg, double_map)
		
	NavigationServer3D.region_set_navigation_mesh(single_reg, single)
	NavigationServer3D.region_set_navigation_mesh(double_reg, double)

	$small/nav.path_desired_distance = 4.0
	$small/nav.target_desired_distance = 4.0
	$large/nav.path_desired_distance = 4.0
	$large/nav.target_desired_distance = 4.0

	call_deferred("actor_setup")
	
func actor_setup():
	print("actor_setup awaiting...")
	await get_tree().physics_frame
	print("actor_setup finished awaiting")
	set_movement_target($target.position)

func set_movement_target(p):
	print("target: ", p)
	$small/nav.target_position = p
	$large/nav.target_position = p

func _process(delta):
	pass

func do_path(a: CharacterBody2D, map: NavigationPolygon):
	var n = a.get_node("nav")
	if n.is_navigation_finished():
		return
	var p: Vector2 = a.global_position
	var path = NavigationServer2D.map_get_path(map, p, $target.global_position, true)
	print(path)
	a.velocity = p.direction_to(path[0]) * movement_speed
	a.move_and_slide()
	print(path[0])
	
func _physics_process(delta):
	do_path($small, single)
	do_path($large, double)