extends Node
## Shared planar A* map for static office collisions; no runtime navmesh bake.
const CELL := 0.5
const CLEARANCE := 0.60 # capsule radius .35 + conservative cell/edge margin
var grid := AStarGrid2D.new()
var obstacles: Array[Rect2] = []

func _ready() -> void:
	name = "RoomNavigation"
	grid.region = Rect2i(-17, -17, 35, 35)
	grid.cell_size = Vector2(CELL, CELL)
	grid.diagonal_mode = AStarGrid2D.DIAGONAL_MODE_ONLY_IF_NO_OBSTACLES
	grid.default_compute_heuristic = AStarGrid2D.HEURISTIC_OCTILE
	grid.default_estimate_heuristic = AStarGrid2D.HEURISTIC_OCTILE
	grid.update() # Must precede solid flags; update clears them.
	for prop in get_tree().get_nodes_in_group("office_props"):
		for collider in prop.get_children():
			if not collider is CollisionShape3D or not collider.shape is BoxShape3D:
				continue
			var half: Vector3 = collider.shape.size * 0.5
			var minimum := Vector2(INF, INF)
			var maximum := Vector2(-INF, -INF)
			for x in [-half.x, half.x]:
				for z in [-half.z, half.z]:
					var p: Vector3 = collider.global_transform * Vector3(x, 0, z)
					minimum = minimum.min(Vector2(p.x, p.z))
					maximum = maximum.max(Vector2(p.x, p.z))
			obstacles.append(Rect2(minimum, maximum - minimum).grow(CLEARANCE))
	for x in range(-17, 18):
		for y in range(-17, 18):
			var id := Vector2i(x, y)
			var p := Vector2(id) * CELL
			for rect in obstacles:
				if rect.has_point(p):
					grid.set_point_solid(id)
					break

func _free_id(point: Vector3) -> Vector2i:
	var id := Vector2i(roundi(point.x / CELL), roundi(point.z / CELL))
	id.x = clampi(id.x, -17, 17)
	id.y = clampi(id.y, -17, 17)
	if not grid.is_point_solid(id):
		return id
	var best := id
	var distance := INF
	for x in range(-17, 18):
		for y in range(-17, 18):
			var candidate := Vector2i(x, y)
			if not grid.is_point_solid(candidate):
				var d := Vector2(candidate).distance_squared_to(Vector2(point.x, point.z) / CELL)
				if d < distance:
					best = candidate
					distance = d
	return best

func route(from: Vector3, to: Vector3) -> PackedVector3Array:
	var result := PackedVector3Array()
	for p in grid.get_point_path(_free_id(from), _free_id(to)):
		result.append(Vector3(p.x, 0, p.y))
	return result

func direct_clear(from: Vector3, to: Vector3) -> bool:
	var a := Vector2(from.x, from.z)
	var b := Vector2(to.x, to.z)
	var count := maxi(1, ceili(a.distance_to(b) / 0.15))
	for i in range(count + 1):
		var p := a.lerp(b, float(i) / count)
		if absf(p.x) > 8.6 or absf(p.y) > 8.6:
			return false
		for obstacle in obstacles:
			if obstacle.has_point(p):
				return false
	return true