extends RefCounted
## World-space focus cone (ADR-0008). Picks the head closest to the camera
## forward vector inside range and angle. No viewport needed, so tests run headless.

const MAX_RANGE := 10.0
const MAX_ANGLE_RAD := 0.175  # ~10 degrees

static func score_head(cam_origin: Vector3, cam_forward: Vector3, head_pos: Vector3, max_range: float = MAX_RANGE, max_angle: float = MAX_ANGLE_RAD) -> float:
	var to := head_pos - cam_origin
	var dist := to.length()
	if dist < 0.05 or dist > max_range:
		return -1.0
	var dir := to / dist
	var dot := cam_forward.dot(dir)
	if dot <= 0.0:
		return -1.0
	var ang := acos(clampf(dot, -1.0, 1.0))
	if ang > max_angle:
		return -1.0
	return ang

static func pick_index(cam_origin: Vector3, cam_forward: Vector3, head_positions: Array, max_range: float = MAX_RANGE, max_angle: float = MAX_ANGLE_RAD) -> int:
	var best_i := -1
	var best_score := 999.0
	for i in head_positions.size():
		var pos: Vector3 = head_positions[i]
		var score := score_head(cam_origin, cam_forward, pos, max_range, max_angle)
		if score < 0.0:
			continue
		if score < best_score:
			best_score = score
			best_i = i
	return best_i