@tool
extends BTAction

@export var navigation_path_var: StringName = &"navigation_path"
var path_waypoints:StringName = &"path_waypoint_locations"

var _agent_npc:NonPlayerCharacter = null

func _generate_name() -> String:
	return "FindPathWayPoints(\"%s\")  ➜ %s" % [
		LimboUtility.decorate_var(navigation_path_var), LimboUtility.decorate_var(path_waypoints)
		]

func _calc_path_length (navigation_path: PackedVector3Array) -> float:
	var result: float = 0
	for i in range(1, navigation_path.size()):
		result = result + (navigation_path[i] - navigation_path[i - 1]).length()
		
	return result

func _tick(_delta: float) -> Status:
	var path:Path3D = blackboard.get_var(navigation_path_var) as Path3D
	
	var shortest_distance:float = INF
	var closest_point_index:int = 0
	
	_agent_npc = agent as NonPlayerCharacter
	
	var path_baked_points:PackedVector3Array = path.curve.get_baked_points()
	
	for i in range(path_baked_points.size()):
		var point:Vector3 = path_baked_points[i]
		var query_path:PackedVector3Array = _agent_npc.query_navigation_path(point)
		if query_path.size() == 0:
			continue
		
		var point_distance = _calc_path_length(query_path)
		if point_distance < shortest_distance:
			closest_point_index = i
			shortest_distance = point_distance
	
	if shortest_distance == INF:
		push_error("Could not find a navigation path to path '%s'!" % path.name)
		return FAILURE

	var closest_point_path:PackedVector3Array = PackedVector3Array()
	closest_point_path.resize(path_baked_points.size())
	for i in range(path_baked_points.size()):
		closest_point_path[i] = path_baked_points[(closest_point_index + i) % path_baked_points.size()]
	
	#if DebugSystem.debug_npc == _agent_npc: DebugDraw3D.draw_line_path(closest_point_path, Color.ORANGE_RED, 1.0)

	blackboard.set_var(path_waypoints, closest_point_path)

	return SUCCESS