extends PathService

# Pre-optimization algorithm, retained as an exact route-order oracle.
func _run_a_star(
	start: Vector3i,
	goal: Vector3i,
	allowed_layers: Array[int],
	allowed_access_ids: Array[StringName]
) -> Array[Vector3i]:
	if start == goal:
		return [start]

	var open_set: Dictionary = {start: true}
	var closed_set: Dictionary = {}
	var came_from: Dictionary = {}
	var g_score: Dictionary = {start: 0}

	while not open_set.is_empty():
		var current := _select_best_open_node(open_set, g_score, goal)
		if current == goal:
			return _reconstruct_path(came_from, current)

		open_set.erase(current)
		closed_set[current] = true
		var edges: Array = _adjacency.get(current, [])
		for edge_value: Variant in edges:
			var edge: Dictionary = edge_value
			var neighbor: Vector3i = edge[&"to"]
			if neighbor.z not in allowed_layers \
				or _cell_is_temporarily_blocked(neighbor) \
				or not _edge_meets_vertical_policy(current, edge) \
				or not _edge_is_accessible(edge, allowed_access_ids):
				continue
			var tentative_cost: int = int(g_score[current]) + _edge_total_cost(edge)
			var known_cost: int = int(g_score.get(neighbor, _INFINITE_COST))
			var better_parent := false
			if tentative_cost == known_cost and came_from.has(neighbor):
				better_parent = _coordinate_less(current, came_from[neighbor])
			if tentative_cost > known_cost or (tentative_cost == known_cost and not better_parent):
				continue

			came_from[neighbor] = current
			g_score[neighbor] = tentative_cost
			if closed_set.has(neighbor):
				closed_set.erase(neighbor)
			open_set[neighbor] = true

	return []


func _select_best_open_node(open_set: Dictionary, g_score: Dictionary, goal: Vector3i) -> Vector3i:
	var best := Vector3i.ZERO
	var has_best := false
	var best_score := _INFINITE_COST
	for candidate_value: Variant in open_set:
		var candidate: Vector3i = candidate_value
		var score: int = int(g_score[candidate]) + _heuristic(candidate, goal)
		if not has_best or score < best_score:
			best = candidate
			best_score = score
			has_best = true
		elif score == best_score and _coordinate_less(candidate, best):
			best = candidate
	return best