fix: z-axis issues, lock, telemetry

Author: tmcarmichael

src/brain/scoring_phases.py

@@ -74,7 +74,15 @@ def _is_rule_blocked(
         diag_results.append(f"{r.name}=OPEN")
         rule_times[r.name] = 0.0
         return True
-    if not r.condition(state):
+    try:
+        cond = r.condition(state)
+    except Exception as exc:
+        log.warning("[DECISION] Rule %s condition raised %s -- skipping", r.name, exc)
+        rule_eval[r.name] = "ERROR"
+        diag_results.append(f"{r.name}=ERROR")
+        rule_times[r.name] = 0.0
+        return True
+    if not cond:
         rule_eval[r.name] = "cond=F"
         diag_results.append(f"{r.name}=cond_F")
         rule_times[r.name] = 0.0
@@ -114,6 +122,14 @@ def compute_divergence(brain: Brain, state: GameState, now: float, binary_winner
         if breaker and not breaker.allow():
             scores[r.name] = -2.0  # circuit-broken
             continue
+        # Check condition so telemetry doesn't report ineligible "winners"
+        try:
+            if not r.condition(state):
+                scores[r.name] = -3.0  # condition false
+                continue
+        except Exception:
+            scores[r.name] = -4.0  # condition error
+            continue
         s = _safe_score(r.score_fn, state)
         scores[r.name] = s
         if s > best_score:

src/brain/transitions.py

@@ -35,13 +35,15 @@ def handle_transition(
     if brain._ctx and brain._ctx.combat.engaged:
         maybe_clear_stale_engagement(brain, state, selected, now)
 
-    if selected is brain._active:
-        return
-
-    # Check lock timeout before honoring the lock
+    # Check lock timeout even when the active routine re-selects itself,
+    # otherwise max_lock_seconds is bypassed whenever the locked routine
+    # keeps winning selection (the early return below would skip the check).
     if brain._active is not None and brain._active.locked and brain._active_start_time > 0:
         maybe_force_lock_exit(brain, state, now)
 
+    if selected is brain._active:
+        return
+
     # Locked routines can only be interrupted by emergency rules
     if brain._active is not None and brain._active.locked and not selected_emergency:
         lock_key = f"{brain._active_name}:{selected_name}"

src/nav/pathfinding.py

@@ -245,8 +245,10 @@ def find_path(
 
         if current == goal_node:
             path = _jps_reconstruct(came_from, current, terrain, near_z)
-            path = _simplify_path(path, terrain)
-            path = vary_path(path, terrain, jitter_range=jitter)
+            path = _simplify_path(path, terrain, wb_override=wb, wbc_override=wbc)
+            path = vary_path(
+                path, terrain, jitter_range=jitter, near_z=near_z, wb_override=wb, wbc_override=wbc
+            )
             _log_path_found(path, terrain, t0, explored, start.x, start.y, goal.x, goal.y)
             return path
 
@@ -468,8 +470,10 @@ def _find_path_astar(
 
         if cc == gc and cr == gr:
             path = _jps_reconstruct(came_from, current, terrain, near_z)
-            path = _simplify_path(path, terrain)
-            path = vary_path(path, terrain, jitter_range=jitter)
+            path = _simplify_path(path, terrain, wb_override=wb, wbc_override=wbc)
+            path = vary_path(
+                path, terrain, jitter_range=jitter, near_z=near_z, wb_override=wb, wbc_override=wbc
+            )
             elapsed = time.perf_counter() - t0
             log.info("[NAV] A* fallback: path %d waypoints, %d nodes in %.2fs", len(path), explored, elapsed)
             return path
@@ -780,7 +784,12 @@ def _reconstruct(
     return waypoints
 
 
-def _simplify_path(path: list[Point], terrain: ZoneTerrain) -> list[Point]:
+def _simplify_path(
+    path: list[Point],
+    terrain: ZoneTerrain,
+    wb_override: bytearray | None = None,
+    wbc_override: int | None = None,
+) -> list[Point]:
     """Two-phase path smoothing: greedy LOS + clearance-aware funnel.
 
     Phase 1 (greedy forward): scan forward from each anchor, skip
@@ -790,6 +799,9 @@ def _simplify_path(path: list[Point], terrain: ZoneTerrain) -> list[Point]:
     try the FARTHEST remaining point first (reverse scan). Prefer paths
     with clearance margin; fall back to center-only for narrow passages.
     Finds shortcuts that Phase 1 misses due to its early-break heuristic.
+
+    When *wb_override*/*wbc_override* are provided, LOS checks use the
+    Z-filtered bitfield so smoothing respects multi-level terrain.
     """
     if len(path) <= 2:
         return path
@@ -800,21 +812,25 @@ def _simplify_path(path: list[Point], terrain: ZoneTerrain) -> list[Point]:
     while i < len(path) - 1:
         best_j = i + 1
         for j in range(i + 2, len(path)):
-            if _clear_line(terrain, path[i][0], path[i][1], path[j][0], path[j][1]):
+            if _clear_line(
+                terrain, path[i][0], path[i][1], path[j][0], path[j][1], wb_override, wbc_override
+            ):
                 best_j = j
             else:
                 break
         phase1.append(path[best_j])
         i = best_j
 
     # Phase 2: reverse-scan funnel with clearance
-    return _funnel_smooth(phase1, terrain)
+    return _funnel_smooth(phase1, terrain, wb_override=wb_override, wbc_override=wbc_override)
 
 
 def _funnel_smooth(
     path: list[Point],
     terrain: ZoneTerrain,
     clearance: float = 2.0,
+    wb_override: bytearray | None = None,
+    wbc_override: int | None = None,
 ) -> list[Point]:
     """Clearance-aware reverse-scan funnel smoothing.
 
@@ -840,14 +856,18 @@ def _funnel_smooth(
 
         # Try farthest point first -- with clearance
         for j in range(len(path) - 1, i + 1, -1):
-            if _clear_line_wide(terrain, path[i][0], path[i][1], path[j][0], path[j][1], clearance):
+            if _clear_line_wide(
+                terrain, path[i][0], path[i][1], path[j][0], path[j][1], clearance, wb_override, wbc_override
+            ):
                 best_j = j
                 break
 
         # Fallback: center-only for narrow passages
         if best_j == i + 1 and i + 2 < len(path):
             for j in range(len(path) - 1, i + 1, -1):
-                if _clear_line(terrain, path[i][0], path[i][1], path[j][0], path[j][1]):
+                if _clear_line(
+                    terrain, path[i][0], path[i][1], path[j][0], path[j][1], wb_override, wbc_override
+                ):
                     best_j = j
                     break
 
@@ -857,16 +877,39 @@ def _funnel_smooth(
     return result
 
 
-def vary_path(path: list[Point], terrain: ZoneTerrain, jitter_range: float = 3.0) -> list[Point]:
+def vary_path(
+    path: list[Point],
+    terrain: ZoneTerrain,
+    jitter_range: float = 3.0,
+    near_z: float = float("nan"),
+    wb_override: bytearray | None = None,
+    wbc_override: int | None = None,
+) -> list[Point]:
     """Add small perpendicular offsets to intermediate waypoints.
 
     Applies a uniform lateral drift (1-3u) to each interior point.
     Start/end waypoints are never modified. All offset positions are
     validated against terrain before use.
+
+    When *near_z* and a Z-filtered bitfield are provided, jitter
+    validation uses the bitfield (respecting multi-level terrain) and
+    Z lookup uses ``get_level_z`` to stay on the correct floor.
     """
     if len(path) <= 2:
         return path
 
+    # Pre-fetch grid constants for bitfield walkability checks
+    _use_zbit = wb_override is not None and wbc_override is not None
+    if _use_zbit:
+        _cs = terrain.cell_size
+        _mx = terrain._min_x
+        _my = terrain._min_y
+        _cols = terrain._cols
+        _rows = terrain._rows
+        _bw = _bit_walkable
+
+    _has_near_z = not math.isnan(near_z)
+
     result: list[Point] = [path[0]]
 
     for i in range(1, len(path) - 1):
@@ -882,21 +925,44 @@ def vary_path(path: list[Point], terrain: ZoneTerrain, jitter_range: float = 3.0
                 offset = random.uniform(-jitter_range, jitter_range)
                 jx = px + perp_x * offset
                 jy = py + perp_y * offset
-                if terrain.is_walkable(jx, jy):
+                # Z-aware walkability: use the filtered bitfield when available
+                if _use_zbit:
+                    col = int((jy - _mx) / _cs)
+                    row = int((jx - _my) / _cs)
+                    walkable = _bw(wb_override, wbc_override, col, row, _cols, _rows)
+                else:
+                    walkable = terrain.is_walkable(jx, jy)
+                if walkable:
                     px, py = jx, jy
 
-        _z = terrain.get_z(px, py)
+        # Z-aware height: use get_level_z to stay on the correct floor
+        if _has_near_z:
+            _z = terrain.get_level_z(px, py, near_z)
+        else:
+            _z = terrain.get_z(px, py)
         result.append(Point(px, py, _z if _z == _z else 0.0))
 
     result.append(path[-1])
     return result
 
 
-def _clear_line(terrain: ZoneTerrain, x1: float, y1: float, x2: float, y2: float) -> bool:
+def _clear_line(
+    terrain: ZoneTerrain,
+    x1: float,
+    y1: float,
+    x2: float,
+    y2: float,
+    wb_override: bytearray | None = None,
+    wbc_override: int | None = None,
+) -> bool:
     """Check if a straight line between two game-coord points is walkable.
 
     Uses precomputed bitfield when available (~10x faster than
     _cell_walkable per sample). Falls back to _cell_walkable otherwise.
+
+    When *wb_override*/*wbc_override* are provided (e.g. the Z-filtered
+    bitfield from ``build_walk_bits_z``), they take precedence over the
+    cached Z-agnostic ``terrain._walk_bits``.
     """
     dx, dy = x2 - x1, y2 - y1
     dist = math.hypot(dx, dy)
@@ -907,9 +973,9 @@ def _clear_line(terrain: ZoneTerrain, x1: float, y1: float, x2: float, y2: float
     steps = max(1, int(dist / (cs * 0.5)))
 
     # Fast path: bitfield-accelerated (no coord conversion overhead)
-    wb = terrain._walk_bits
+    wb = wb_override if wb_override is not None else terrain._walk_bits
     if wb:
-        wbc = terrain._walk_byte_cols
+        wbc = wbc_override if wbc_override is not None else terrain._walk_byte_cols
         cols = terrain._cols
         rows = terrain._rows
         mx = terrain._min_x
@@ -936,14 +1002,21 @@ def _clear_line(terrain: ZoneTerrain, x1: float, y1: float, x2: float, y2: float
 
 
 def _clear_line_wide(
-    terrain: ZoneTerrain, x1: float, y1: float, x2: float, y2: float, clearance: float = 2.0
+    terrain: ZoneTerrain,
+    x1: float,
+    y1: float,
+    x2: float,
+    y2: float,
+    clearance: float = 2.0,
+    wb_override: bytearray | None = None,
+    wbc_override: int | None = None,
 ) -> bool:
     """Check if a line with clearance margin is fully walkable.
 
     Tests center line plus two parallel lines offset by clearance.
     Ensures the agent has breathing room and won't wall-hug.
     """
-    if not _clear_line(terrain, x1, y1, x2, y2):
+    if not _clear_line(terrain, x1, y1, x2, y2, wb_override, wbc_override):
         return False
 
     dx, dy = x2 - x1, y2 - y1
@@ -955,8 +1028,8 @@ def _clear_line_wide(
     px = -dy / dist * clearance
     py = dx / dist * clearance
 
-    if not _clear_line(terrain, x1 + px, y1 + py, x2 + px, y2 + py):
+    if not _clear_line(terrain, x1 + px, y1 + py, x2 + px, y2 + py, wb_override, wbc_override):
         return False
-    if not _clear_line(terrain, x1 - px, y1 - py, x2 - px, y2 - py):
+    if not _clear_line(terrain, x1 - px, y1 - py, x2 - px, y2 - py, wb_override, wbc_override):
         return False
     return True

tests/test_pathfinding_algo.py

@@ -1063,10 +1063,13 @@ def test_ground_agent_avoids_bridge_cells(self):
             path = find_path(t, start, goal, jitter=0.0)
 
         assert path is not None
-        # No waypoint should be in the bridge columns (8-12)
+        # No waypoint should be in an actual bridge cell (cols 8-12, rows 5-24).
+        # The path may pass through cols 8-12 at rows outside the bridge
+        # (the gap at top/bottom) -- that's correct routing around the bridge.
         for wp in path:
             col, row = t._game_to_grid(wp.x, wp.y)
-            assert col < 8 or col > 12, f"ground path crossed bridge at col={col}"
+            in_bridge = 8 <= col <= 12 and 5 <= row <= 24
+            assert not in_bridge, f"ground path crossed bridge at col={col} row={row}"
 
     def test_bridge_agent_crosses_bridge(self):
         """Agent at Z=50 should path straight across the bridge."""