extends NavigationAgent3D

@export var MAX_SPEED : float = 3.0

var points : Array[Marker3D]
var parent : CharacterBody3D

var current_point : int

func _ready() -> void:
	parent = get_parent()
	for child in get_children():
		if child is Marker3D:
			points.append(child)
	if points.size() > 0:
		current_point = 0
		set_target_position(points[current_point].global_position)
		
func _physics_process(delta: float) -> void:
	var next_path_position = get_next_path_position()
	var dir = parent.global_position.direction_to(next_path_position)
	var new_velocity = dir*MAX_SPEED
	#parent.look_at(next_path_position)
	#parent.rotate(Vector3.UP, parent.basis.z.angle_to(next_path_position)/2)
	parent.global_transform.basis = parent.global_transform.basis.slerp(parent.global_transform.looking_at(next_path_position, Vector3.UP).basis, delta * 10.0)
	parent.velocity = new_velocity
	parent.move_and_slide()
	if parent.global_position.distance_to(points[current_point].global_position) < 1:
		current_point = (current_point + 1)%points.size()
		set_target_position(points[current_point].global_position)