# Hero exploration controller with committed routes and integrated driving.
# Generated by tools/build_exploration_hero.py and tools/build_driving_hero.py.
import time
import sys
import os
import re
import math
import json
import hashlib
import tempfile
import struct
import copy
from concurrent.futures import CancelledError, ThreadPoolExecutor
from threading import Event
from queue import Empty, Queue
from collections import deque, defaultdict
from itertools import combinations, product
from fractions import Fraction
import numpy as np
from scipy import ndimage as scipy_ndimage
from scipy import sparse as scipy_sparse
from scipy.sparse.csgraph import dijkstra as scipy_dijkstra
from scipy.spatial import cKDTree as scipy_cKDTree
from scipy.spatial.transform import Rotation as scipy_Rotation
import matplotlib.pyplot as plt
plt.rcParams["figure.raise_window"] = False
from matplotlib.collections import LineCollection
from matplotlib.lines import Line2D
sys.path.append("C:\\Program Files\\Webots\\lib\\controller\\python")
from controller import Robot, Keyboard

# Runtime controls. DEBUG=False disables diagnostic work, console summaries and
# normal capture/dump writers (including S/A). SHOW_MAPS=False suppresses
# live windows independently; sensing, mapping, planning and export stay active.
DEBUG = False
SHOW_MAPS = False
# Keep rendering in a separate process; False restores inline drawing.
ASYNC_DRAW = False
# Temporary encounter observation is independent of full DEBUG. It records
# decisions after motor control; it never changes movement or radio packets.
OBSERVE_ENCOUNTERS = False

# BEGIN EMBEDDED RescueVision class
class RescueVision:
    """Paste this whole class before OccupancyMapper in Hero.py.

    Owns the recognizer, physical token memory and embedded calibration, plus
    bounded inspection scheduling through Hero's existing planner and driver.
    Requires NumPy, OpenCV and SciPy, but no companion project files or Webots
    imports. mapper is Hero's existing OccupancyMapper. Motion stays in the driver.
    """
    _CALIBRATION = {'Wheel 1': {'dictName': 'Wheel 1', 'name': 'Wheel', 'customName': 'wheelR', 'type': '', 'x': 170, 'y': 15, 'z': 0, 'rx': 0, 'ry': -1, 'rz': 0, 'a': 1.57, 'custom': []}, 'Wheel 2': {'dictName': 'Wheel 2', 'name': 'Wheel', 'customName': 'wheelL', 'type': '', 'x': -170, 'y': 15, 'z': 0, 'rx': 0, 'ry': -1, 'rz': 0, 'a': 1.57, 'custom': []}, 'camera1': {'dictName': 'camera1', 'name': 'Camera', 'customName': 'camR', 'type': 'add', 'x': 300, 'y': 370, 'z': 0, 'rx': -0.9267025424900825, 'ry': -0.2657276780299868, 'rz': 0.2657276780299868, 'a': 1.6468455624731366, 'custom': [64, 64]}, 'iu': {'dictName': 'iu', 'name': 'InertialUnit', 'customName': 'imu', 'type': 'add', 'x': 0, 'y': 0, 'z': 0, 'rx': -0.58, 'ry': 0.58, 'rz': 0.58, 'a': 2.09, 'custom': []}, 'gps': {'dictName': 'gps', 'name': 'GPS', 'customName': 'gps', 'type': 'add', 'x': 0, 'y': 0, 'z': 0, 'rx': -0.5858, 'ry': 0.5858, 'rz': 0.5858, 'a': 2.09, 'custom': []}, 'colsensor': {'dictName': 'colsensor', 'name': 'Colour sensor', 'customName': 'color', 'type': 'add', 'x': 0, 'y': -100, 'z': -370, 'rx': -0.68, 'ry': 0.28, 'rz': 0.68, 'a': 2.6, 'custom': []}, 'Distance Sensor 1': {'dictName': 'Distance Sensor 1', 'name': 'Distance Sensor', 'customName': 'dist', 'type': '', 'x': 0, 'y': 175, 'z': -370, 'rx': -0.66, 'ry': 0.36, 'rz': 0.66, 'a': 2.46, 'custom': []}, 'lidar': {'dictName': 'lidar', 'name': 'Lidar', 'customName': 'lidar', 'type': 'add', 'x': 0, 'y': 370, 'z': 0, 'rx': -0.58, 'ry': 0.58, 'rz': 0.58, 'a': 2.09, 'custom': []}, 'camera2': {'dictName': 'camera2', 'name': 'Camera', 'customName': 'camL', 'type': 'add', 'x': -300, 'y': 370, 'z': 0, 'rx': 0.1987159956791229, 'ry': -0.6930050335536008, 'rz': -0.6930050335536008, 'a': 2.7492713815778576, 'custom': [64, 64]}}

    def __init__(self, mapper, *, camera_size=(64, 64), camera_fov=1.,
                 debug_enabled=False, capture_dir=None, calibration=None, async_processing=False):
        import base64
        import copy
        import json
        import math
        import os
        import sys
        from collections import Counter, deque
        import cv2
        import numpy as np
        # Only diagnostic output uses this path. Calibration is already embedded.
        __file__ = globals().get('__file__', os.path.join(os.getcwd(), 'Hero.py'))

        class TokenMap:
            def __init__(self, mapper):
                self.mapper = mapper
                self.records = {}
                self._next_id = 1
                self._pose_time = None
                self._pose_displacement = np.zeros(2)
                self._pose_valid = False
                self._boot_sum = np.zeros(2)
                self._boot_count = 0
                self._boot_closed = False
                self._boot_lever = None

            def update_pose(self, data, gps, sigma, source, lever=None):
                """Refine observations still inside Hero's transported GPS window.

        Integrate the same right-endpoint velocity as MappingGPSFilter. Never
        transport across a relocation, missing step or unavailable velocity.
        Non-GPS surface/pixel allowance is retained, never averaged away.
        """
                now=float(data['time']);period=float(data.get('sensor_period_s',.016))
                if now==self._pose_time:
                    return
                velocity=np.asarray(data.get('gps_velocity_mps'),float)
                valid=velocity.shape==(3,) and np.isfinite(velocity).all()
                reset=(self._pose_time is None or data.get('supervisor_relocated')
                    or data.get('gps_filter_reset') or not valid
                    or abs(now-self._pose_time-period)>1e-8)
                if reset:
                    self._pose_displacement[:]=0
                    for r in self.records.values():r.pop('pose_anchor',None)
                else:
                    self._pose_displacement+=velocity[[0,2]]*(now-self._pose_time)
                if not self._boot_closed and lever is not None:
                    # Same start-at-tile-centre convention as Hero. Refine only the
                    # semantic origin; never move occupancy or navigation coordinates.
                    if ((reset and self._pose_time is not None) or not valid
                            or any(data.get('motor_command_radps',()))
                            or np.linalg.norm(velocity[[0,2]])>=.0008):
                        self._boot_closed=True
                    else:
                        if self._boot_lever is None:self._boot_lever=np.asarray(lever)
                        raw=np.asarray(data['position']['raw_xyz_m'])[[0,2]]
                        self._boot_sum+=raw-self._pose_displacement-self._boot_lever
                        self._boot_count+=1
                        self._boot_closed=self._boot_count>=49
                self._pose_time=now;self._pose_valid=valid
                count=data.get('position',{}).get('map_position_samples',0)
                if not valid or source!='hero-physical-gps-filter' or count<49:
                    return
                for r in self.records.values():
                    anchor=r.get('pose_anchor')
                    if anchor is None:continue
                    if now-anchor['time']>(count-1)*period+1e-8:
                        r.pop('pose_anchor',None);continue
                    allowance=3*sigma+anchor['surface_allowance']
                    if allowance<=r['allowance_m']:
                        p=np.asarray(anchor['relative'])+gps-self._pose_displacement
                        r['world_xyz_m'][0],r['world_xyz_m'][2]=p.tolist()
                        r['allowance_m']=allowance

            def xy(self, world):
                m = self.mapper
                if m.origin_x is None or m.origin_y is None:
                    return None
                return np.array([world[0]*1000-m.origin_x,
                                 m.GPS_Y_SIGN*world[2]*1000-m.origin_y])

            def normal(self, world_normal):
                n = np.array([world_normal[0], self.mapper.GPS_Y_SIGN*world_normal[1]], float)
                return n / np.linalg.norm(n)

            def observe(self, row, now, project=None):
                """Associate a unique physical instance; ambiguous views cannot create reports."""
                loc = row['location']
                if loc.get('world_xyz_m') is None:
                    return None, 'rejected or unlocalized observation'
                box=np.asarray(row.get('bbox_px'),float)
                center=np.asarray(row.get('image_center_px'),float)
                if (box.shape!=(4,) or center.shape!=(2,) or not np.isfinite(box).all()
                        or not np.isfinite(center).all() or min(box[2:])<=0):
                    return None, 'missing or invalid image footprint'
                point = np.array(loc['world_xyz_m'], float)
                normal = np.array(loc['surface_normal_xz'], float)
                normal /= np.linalg.norm(normal)
                allowance = float(loc['uncertainty_m'])
                if not np.isfinite(point).all() or not math.isfinite(allowance):
                    return None, 'nonfinite observation'
                tangent = np.array([-normal[1], normal[0]])
                strict, possible, refinements = [], [], set()
                for record in self.records.values():
                    if normal @ record['normal_xz'] < math.cos(math.radians(30)):
                        continue
                    delta = (point-record['world_xyz_m'])[[0,2]]
                    budget = allowance+record['allowance_m']
                    if abs(delta@normal) > budget:
                        continue
                    along = abs(delta@tangent)
                    if along > budget+record['width_m']/2:
                        continue
                    possible.append(record)
                    if along > budget:
                        # A clipped/estimated plaque centre is not an exact
                        # physical centre. A unique new FULL measured plaque
                        # may refine an unconfirmed hint when its
                        # actual image contains the old projected point. Keep
                        # depth/face gates, token-size bound and ambiguity checks;
                        # do not widen reporting uncertainty or merge confirmed IDs.
                        if (not record['confirmed'] and not record['sent']
                                and row.get('validated_geometry') and not row.get('partial')
                                and row.get('center_type')=='DETECTED'
                                and row.get('kind')==record['kind']
                                and along<=budget+record['width_m']/2 and project is not None):
                            pixel=project(record['world_xyz_m']);x,y,w,h=row['bbox_px']
                            if pixel is not None and x<=pixel[0]<=x+w and y<=pixel[1]<=y+h:
                                strict.append(record)
                                refinements.add(record['id'])
                        continue
                    if project is not None:
                        pixel = project(record['world_xyz_m'])
                        along_world = np.array(record['world_xyz_m'])
                        along_world[[0,2]] += tangent*budget
                        edge = project(along_world)
                        if pixel is None or edge is None:
                            continue
                        axis = np.array(edge[:2])-pixel[:2]
                        allowance_px = np.linalg.norm(axis)
                        if allowance_px>1e-9:
                            residual = np.array(pixel[:2])-row['image_center_px']
                            if abs(residual@axis/allowance_px)>allowance_px+.75:
                                continue
                        # The projected height of a clipped target centre is uncertain.
                        # Require overlap with its observed box, not equal centre pixels.
                        x,y,w,h=row['bbox_px']
                        if not(x-allowance_px<=pixel[0]<=x+w+allowance_px and
                               y-allowance_px<=pixel[1]<=y+h+allowance_px):
                            continue
                    strict.append(record)
                if row['visual_status']=='REJECTED':
                    # A clipped ring can decode to an invalid colour sum while
                    # turning past a real, already-confirmed target. It vetoes
                    # THIS image, but cannot erase a fully established identity.
                    # A complete negative view still invalidates it immediately.
                    if len(strict)==1 and (not strict[0]['confirmed'] or not row.get('partial')):
                        strict[0].update(vote_count=0,vote_label=None,recognized=False,view_rejected=True)
                    return None,'rejected observation'
                if len(strict) == 1:
                    record = strict[0]
                elif strict or possible:
                    return None, 'ambiguous physical token association'
                else:
                    record = dict(id=self._next_id, kind=row['kind'], label=None,
                                  world_xyz_m=point.tolist(), normal_xz=normal.tolist(),
                                  allowance_m=allowance, width_m=.016 if row['kind']=='victim' else .04,
                                  sent=False, surface=None, last_seen_s=now,
                                  confirmed=False, guessed_label=row['label'])
                    self.records[self._next_id] = record
                    self._next_id += 1
                quality=int(bool(row['kind']!='victim' or
                    (row.get('center_type')=='DETECTED' and not row.get('partial') and row['label'])))
                if row.get('confirmation_vote') or row.get('validated_geometry'):quality=2
                if loc.get('metric_localized'):quality=3
                previous_quality=record.get('position_quality',-1)
                if (record['id'] in refinements and quality>=previous_quality) or quality>previous_quality or (quality==previous_quality and allowance<=record['allowance_m']):
                    record.update(world_xyz_m=point.tolist(), normal_xz=normal.tolist(),
                                  allowance_m=allowance,position_quality=quality)
                    # A transport anchor belongs to exactly one measurement.
                    # Delayed worker geometry may replace that measurement when
                    # its capture time cannot be anchored at the current pose.
                    record.pop('pose_anchor',None)
                    if self._pose_valid and now==self._pose_time and 'gps_world_xz_m' in loc:
                        record['pose_anchor']=dict(time=now,
                            relative=(point[[0,2]]-loc['gps_world_xz_m']+self._pose_displacement).tolist(),
                            surface_allowance=max(0,allowance-3*loc['position_sigma_m']))
                if loc.get('position_source')!='measured-map-ray' and not loc.get('retained_range'):
                    record['range_observed_s']=now
                record.update(last_seen_s=now, guessed_label=row['label'])
                # Inconclusive/missing images are not negative identity evidence. Keep
                # qualifying camera-only votes on the physical instance across revisits.
                vote=row.get('confirmation_vote')
                # Cropped rings can alias their scale and duplicate a band. They
                # may establish an initial provisional identity through the existing
                # checks, but cannot contradict an already confirmed physical token.
                # A complete new view can still correct a previous wrong read.
                if record['confirmed'] and record.get('label')!=vote and row.get('partial'):
                    vote=None
                if vote and now>record.get('vote_time',-math.inf) and loc.get('position_source')!='measured-map-ray':
                    if vote!=record.get('vote_label'):
                        record.update(vote_label=vote,vote_count=0,vote_since=now,recognized=False)
                        record.pop('motion_vote',None)
                    record.update(vote_count=min(4,record['vote_count']+1),vote_time=now)
                    spatial_ready=False
                    # Independence belongs to the observing CAMERA. Its lever
                    # arm moves during rotation even when the body/GPS is still.
                    gps=np.asarray(loc.get('camera_world_xz_m',loc.get('gps_world_xz_m')),float)
                    side=row.get('side')
                    if (row.get('validated_geometry')
                            and (row['kind']=='target' or (row.get('center_type')=='DETECTED' and not row.get('partial')))
                            and side in ('left','right')
                            and gps.shape==(2,) and np.isfinite(gps).all()):
                        pixel_allowance=None
                        if project is not None:
                            center_projection=project(point)
                            corners=[]
                            for dx,dz in ((-1,-1),(-1,1),(1,-1),(1,1)):
                                corner=point+np.array([dx,0.,dz])*allowance
                                projected=project(corner)
                                if projected is not None:corners.append(projected)
                            if center_projection is not None and len(corners)==4:
                                pixel_allowance=max(abs(p[0]-center_projection[0])for p in corners)
                                if not math.isfinite(pixel_allowance):pixel_allowance=None
                        anchor=record.get('motion_vote')
                        if anchor is None or anchor['side']!=side:
                            anchor=dict(side=side,gps=gps.tolist(),camera_sigma=float(loc.get('position_sigma_m',allowance/3)),pixel=center.tolist(),
                                width=float(box[2]),allowance=allowance,pixel_allowance=pixel_allowance,time=now,count=0)
                            record['motion_vote']=anchor
                        anchor['count']+=1
                        # Range-fit error is common to both views of this one
                        # physical token. Compare relative projected motion,
                        # not the sum of two absolute range-error footprints.
                        pose_px=None
                        if project is not None and loc.get('position_sigma_m')is not None:
                            def projected_samples(origin,spread):
                                ps=[project(origin+np.array([dx,0.,dz])*spread)for dx,dz in ((-1,-1),(-1,1),(1,-1),(1,1))]
                                center_px=project(origin)
                                if center_px is None or any(p is None for p in ps):return None
                                return float(center_px[0]),[float(p[0])for p in ps]
                            sigma=float(loc['position_sigma_m'])
                            pose_px=projected_samples(point,3*sigma)
                            if 'common_point'not in anchor and now==anchor['time']:
                                full_px=projected_samples(point,allowance)
                                if full_px is not None and pose_px is not None:
                                    anchor.update(common_point=point.tolist(),common_center=full_px[0],common_corners=full_px[1],pose_pixel=max(abs(x-pose_px[0])for x in pose_px[1]))
                        # Use the measured uncertainty projected into BOTH
                        # images, rather than an arbitrary half-plaque width.
                        # .75px centre sampling per image is already part of
                        # localization; retain its1.5px two-view lower bound.
                        pixel_span=max(box[2],anchor['width'])/2
                        if pixel_allowance is not None and anchor.get('pixel_allowance') is not None:
                            pixel_span=max(1.5,pixel_allowance+anchor['pixel_allowance'])
                        if project is not None and 'common_point'in anchor and pose_px is not None:
                            current=projected_samples(np.asarray(anchor['common_point']),anchor['allowance'])
                            if current is not None:
                                shift=current[0]-anchor['common_center']
                                common_error=max(abs((x-old)-shift)for x,old in zip(current[1],anchor['common_corners']))
                                pixel_span=1.5+common_error+anchor['pose_pixel']+max(abs(x-pose_px[0])for x in pose_px[1])
                        spatial_ready=(anchor['count']>=TargetVictimDetector.MIN_FRAMES
                            and now-anchor['time']+1e-9>=2*row.get('sensor_period_s',.016)
                            and abs(center[0]-anchor['pixel'][0])>=pixel_span
                            and np.linalg.norm(gps-anchor['gps'])>3*(float(loc.get('position_sigma_m',allowance/3))+anchor.get('camera_sigma',anchor['allowance']/3)))
                    # Use the existing identity rule on the SAME measured
                    # physical token. Pixel overlap is not stable while driving.
                    if spatial_ready or (record['vote_count']>=TargetVictimDetector.MIN_FRAMES
                            and now-record['vote_since']+1e-9>=TargetVictimDetector.MIN_SPAN_S):
                        if record.get('label')!=vote:
                            record['confirmed']=False
                        record.update(label=vote,kind=row['kind'],recognized=True,view_rejected=False)
                    # Recognizing the glyph permits a bounded approach, not a
                    # report or map stamp. Keep the existing authenticity gate
                    # on the CURRENT view, including after distant recognition.
                    if record.get('recognized') and record.get('label')==vote and row.get('confirmation_authorized',True):
                        record['confirmed']=True
                report_label=row.get('visual_report_label')
                names=TargetVictimDetector.VICTIM_NAMES if row['kind']=='victim'else TargetVictimDetector.TARGET_NAMES
                if (row.get('identity_stable') and report_label in names
                        and report_label==row.get('label')
                        and record.get('label') in (None,report_label)
                        and record.get('vote_label') in (None,report_label)):
                    record.update(label=row['visual_report_label'], kind=row['kind'])
                    if loc.get('position_source') != 'measured-map-ray':
                        record['confirmed'] = True
                record['surface'] = self.attachment(record)
                return record, 'associated'

            def attachment(self, record):
                """Finite measured wall/arc attachment; never use the competition raster."""
                m = self.mapper
                p = self.xy(record['world_xyz_m'])
                if p is None:
                    return None
                n = self.normal(record['normal_xz'])
                tolerance = record['allowance_m']*1000 + m.GRID_MM
                offset = np.array(m.last_grid_offset)
                candidates = []
                # Reuse measured model arrays, not token decisions. Position,
                # allowance, normal and raw-patch evidence are evaluated fresh.
                key=(frozenset(m._confirmed_straight_walls),frozenset(m._confirmed_curve_quadrants),
                     tuple(m.last_grid_offset),m.QUARTER_MM)
                if key!=getattr(self,'_attachment_model_key',None):
                    wall_keys=tuple(m._confirmed_straight_walls)
                    walls=np.asarray(wall_keys,float).reshape(-1,4)
                    a=walls[:,:2]+offset;direction=walls[:,2:]+offset-a
                    lengths=np.linalg.norm(direction,axis=1)
                    with np.errstate(divide='ignore',invalid='ignore'):t=direction/lengths[:,None]
                    curve_keys=tuple(m._confirmed_curve_quadrants)
                    curves=np.asarray(curve_keys,float).reshape(-1,4)
                    centres=(curves[:,:2]+curves[:,2:])*m.QUARTER_MM
                    self._attachment_models=(wall_keys,a,lengths,t,curve_keys,curves,centres)
                    self._attachment_model_key=key
                wall_keys,a,lengths,t,curve_keys,curves,centres=self._attachment_models
                along=np.sum((p-a)*t,axis=1)
                q=a+np.clip(along,0,lengths)[:,None]*t
                distances=np.linalg.norm(p-q,axis=1)
                normals=np.column_stack((-t[:,1],t[:,0]));dots=normals@n
                use=(along>=-tolerance)&(along<=lengths+tolerance)&(distances<=tolerance)&(abs(dots)>=math.cos(math.radians(30)))
                for i in np.flatnonzero(use):
                    candidates.append((float(distances[i]),dict(type='straight',key=list(wall_keys[i]),face=1 if dots[i]>0 else -1)))
                radial=p-centres;radii=np.linalg.norm(radial,axis=1)
                with np.errstate(divide='ignore',invalid='ignore'):dots=(radial/radii[:,None])@n
                use=(radii>=1e-6)&np.all(-curves[:,2:]*(p-centres)>=-tolerance,axis=1)&(abs(radii-m.QUARTER_MM)<=tolerance)&(abs(dots)>=math.cos(math.radians(30)))
                for i in np.flatnonzero(use):
                    candidates.append((float(abs(radii[i]-m.QUARTER_MM)),dict(type='curve',key=list(curve_keys[i]),face=1 if dots[i]>0 else -1)))
                if candidates:
                    candidates.sort(key=lambda c:(c[0],c[1]['type'],c[1]['key']))
                    best=candidates[0]
                    # Consecutive collinear segments describe the same surface at a join.
                    if len(candidates)>1 and candidates[1][0]-best[0] <= record['allowance_m']*1000:
                        a,b=best[1],candidates[1][1]
                        if a['type']!='straight' or b['type']!='straight':
                            return None
                        vertical=a['key'][0]==a['key'][2]
                        same_axis=vertical==(b['key'][0]==b['key'][2])
                        same_line=a['key'][0 if vertical else 1]==b['key'][0 if vertical else 1]
                        if not (same_axis and same_line):
                            return None
                    return best[1]
                # A partial raw patch may be known before an entire wall template exists.
                gx,gy=np.rint(p/m.GRID_MM).astype(int)+m.c
                radius=3
                if not(radius<=gx<m.n-radius and radius<=gy<m.n-radius):
                    return None
                window=m.logodds[gy-radius:gy+radius+1,gx-radius:gx+radius+1]>=m.OCC_T
                yy,xx=np.nonzero(window)
                points=np.column_stack(((xx+gx-radius-m.c)*m.GRID_MM,
                                        (yy+gy-radius-m.c)*m.GRID_MM))
                if len(points)<3:
                    return None
                mask=getattr(m,'_confirmed_obstacle_mask',None)
                if mask is not None and mask[max(0,gy-1):gy+2,max(0,gx-1):gx+2].any():
                    return dict(type='object',key=list(m._tile_of(*p)),face=n.tolist())
                t=np.array([-n[1],n[0]])
                close=abs((points-p)@n)<=m.GRID_MM+record['allowance_m']*1000
                points=points[close]
                if len(points)<3 or np.ptp(points@t)<2*m.GRID_MM:
                    return None
                # Raw patch attachment makes no unsupported straight/curve classification.
                return dict(type='patch',key=[float(p[0]),float(p[1])],face=n.tolist())

            def refresh(self):
                for record in self.records.values():
                    record['surface']=self.attachment(record)

            def ray_hit(self, origin, direction):
                """Unique first measured finite wall/arc intersection, in mapper mm."""
                m=self.mapper;origin=np.array(origin,float);d=np.array(direction,float);hits=[]
                offset=np.array(m.last_grid_offset)
                for key in m._confirmed_straight_walls:
                    a,b=np.array(key[:2],float)+offset,np.array(key[2:],float)+offset
                    t=b-a;system=np.column_stack((d,-t))
                    if abs(np.linalg.det(system))<1e-9:
                        continue
                    lam,u=np.linalg.solve(system,a-origin)
                    if lam>0 and 0<=u<=1:
                        normal=np.array([-t[1],t[0]])/np.linalg.norm(t)
                        if normal@d>0:normal=-normal
                        hits.append((float(lam),origin+lam*d,normal))
                for bx,by,sx,sy in m._confirmed_curve_quadrants:
                    centre=np.array([bx+sx,by+sy])*m.QUARTER_MM;v=origin-centre
                    roots=np.roots([d@d,2*v@d,v@v-m.QUARTER_MM**2])
                    for value in roots:
                        if abs(value.imag)>1e-8 or value.real<=0:continue
                        lam=float(value.real);p=origin+lam*d
                        if -sx*(p[0]-centre[0])>=-1e-6 and -sy*(p[1]-centre[1])>=-1e-6:
                            normal=(p-centre)/m.QUARTER_MM
                            if normal@d>0:normal=-normal
                            hits.append((lam,p,normal))
                hits.sort(key=lambda h:h[0])
                if not hits:return None
                lam,p,n=hits[0]
                distance=np.linalg.norm(p-origin)
                if distance>180 or distance<5:return None
                # Do not see through nearer occupied material. Unknown space is inconclusive.
                count=max(2,int(distance/(m.GRID_MM/2)))
                samples=origin+np.linspace(0,max(0,distance-2*m.GRID_MM),count)[:,None]*(p-origin)/distance
                cells=np.rint(samples/m.GRID_MM).astype(int)+m.c
                inside=np.all((cells>=0)&(cells<m.n),axis=1)
                if not inside.all() or np.any(m.logodds[cells[:,1],cells[:,0]]>=m.OCC_T):return None
                return lam,p,n

            def cell(self, record, point=None):
                """Installed MapAnswer placement rules expressed in Hero East/North mm."""
                m=self.mapper;meta=m._matrix_meta;surface=record['surface']
                if not meta or surface is None or surface['type'] not in ('straight','curve'):
                    return None
                p=self.xy(record['world_xyz_m']) if point is None else np.array(point,float)
                offset=np.array(m.last_grid_offset)
                if self._boot_count:
                    start=self._boot_sum/self._boot_count
                    offset=np.array([start[0]*1000-m.origin_x,
                                     m.GPS_Y_SIGN*start[1]*1000-m.origin_y])
                x,y=p-offset
                tx,ty=math.floor(x/m.TILE_MM+.5),math.floor(y/m.TILE_MM+.5)
                cx,cy=tx*m.TILE_MM,ty*m.TILE_MM
                if surface['type']=='curve':
                    c=round((cx-meta['x0_mm'])/meta['cell_mm'])+int(np.sign(x-cx))
                    r=round((meta['y1_mm']-cy)/meta['cell_mm'])+(-2 if y-cy>30 else 2 if y-cy< -30 else 0)
                elif surface['key'][0]==surface['key'][2]:
                    c=round((surface['key'][0]-meta['x0_mm'])/meta['cell_mm'])
                    r=round((meta['y1_mm']-cy)/meta['cell_mm'])+(-1 if y-cy>20 else 1 if y-cy< -20 else 0)
                else:
                    r=round((meta['y1_mm']-surface['key'][1])/meta['cell_mm'])
                    c=round((cx-meta['x0_mm'])/meta['cell_mm'])+(1 if x-cx>20 else -1 if x-cx< -20 else 0)
                return r,c

            def stamp(self, matrix):
                self.refresh();writes={}
                for record in self.records.values():
                    if not record['confirmed'] or not record['label'] or (record.get('view_rejected') and not record['sent']):continue
                    cell=self.cell(record)
                    if cell is None:continue
                    # Quantisation near a boundary needs a better measurement, not a guess.
                    point=self.xy(record['world_xyz_m']);allowance=record['allowance_m']*1000
                    if self._boot_count:allowance+=3*2.5/math.sqrt(self._boot_count)
                    if any(self.cell(record,point+np.array([dx,dy])*allowance)!=cell
                           for dx,dy in ((-1,-1),(-1,1),(1,-1),(1,1))):continue
                    r,c=cell
                    if not(0<=r<len(matrix) and 0<=c<len(matrix[r])) or matrix[r][c] not in ('0','1'):continue
                    writes.setdefault(cell,[]).append(record)
                for (r,c),records in writes.items():
                    matrix[r][c]=''.join(record['label'] for record in sorted(records,key=self._export_order))
                return matrix

            def _export_order(self, record):
                """Judge order: world Z then X; one horizontal face has equal wall Z.

        Use its measured wall attachment to remove normal-depth jitter from
        ordering only. Keep both faces distinct and retain raw report positions.
        """
                x,_,z=record['world_xyz_m'];surface=record['surface']
                if surface['type']=='straight' and surface['key'][1]==surface['key'][3]:
                    m=self.mapper
                    z=m.GPS_Y_SIGN*(surface['key'][1]+m.last_grid_offset[1]+m.origin_y)/1000
                    return z,math.copysign(1,record['normal_xz'][1]),x
                return z,0,x

            def public(self, record):
                point=record['world_xyz_m'];xy=self.xy(point)
                return dict(id=record['id'],label=record['label'] or record['guessed_label'],
                            location=dict(world_xz_m=[point[0],point[2]],navigation_xy_mm=None if xy is None else xy.tolist()),
                            surface=RescueVision._clone_payload(record['surface']),sent=record['sent'])

        class TargetVictimDetector:
            """Reusable perception engine. Call get_detection_return(data) once per snapshot.

    Returns target/victim types, eligible report characters and image centers.
    The class does not send supervisor packets or command movement.
    """

            VICTIM_NAMES = {"H": "Harmed", "S": "Stable", "U": "Unharmed"}
            TARGET_NAMES = {"F": "Flammable", "P": "Poison", "C": "Corrosive", "O": "Organic Peroxide"}
            VICTIM_SAVE_CODES = {"S": 30, "U": 32, "H": 31}
            TARGET_SAVE_CODES = {"F": 35, "P": 34, "C": 33, "O": 36}
            BASE_CAM_W = 128
            BASE_CAM_H = 128
            CAM_W = BASE_CAM_W
            CAM_H = BASE_CAM_H
            WHITE_V_MIN = 170
            WHITE_S_MAX = 60
            BASE_MIN_TOKEN_AREA = 60
            SQUARE_AR_MAX = 1.35
            SQUARE_FILL_MIN = 0.70
            BASE_MIN_SQUARE_SIDE = 14
            SQUARE_AR_PERSP = 1.60
            SQUARE_FILL_PERSP = 0.62
            LETTER_BRIGHT_MIN = 0.40
            LETTER_DARK_MIN = 0.22
            LETTER_DARK_MAX = 0.70
            LETTER_MIN_STD = 25.0
            LETTER_COLOR_MAX = 0.08
            NORM_SIZE = 100
            INNER_INSET = 0.04
            MIN_SYMBOL_AREA = 120
            HOLE_AREA_FRAC = 0.02
            LETTER_CLOSE_KSIZE = 3
            DEFECT_DEPTH_FRAC = 0.30
            PSI_MIN_DEEP = 3
            PSI_SECOND_DEPTH_FRAC = 0.38
            PSI_THIRD_DEPTH_FRAC = 0.24
            PSI_MIN_SOLIDITY = 0.35
            OMEGA_MAX_SOLIDITY = 0.78
            OMEGA_MIN_DEEP_RATIO = 0.18
            OMEGA_ALLOWED_STROKES = (2, 3, 4)
            OMEGA_STROKES4_MIN_DEEP_RATIO = 2.20
            PHI_ONE_HOLE_MIN_MAIN_AREA = 2600
            BLACK_V_MAX = 60
            GRAY_S_MAX = 50
            BASE_MIN_TARGET_AREA = 200
            BASE_MIN_TARGET_RADIUS = 8
            COLOR_VALUE = {"black": -2, "red": -1, "yellow": 0, "green": 1, "blue": 2}
            SUM_TO_TYPE = {0: "F", 1: "P", 2: "C", 3: "O"}
            TARGET_SAT_MIN = 60
            TARGET_VAL_MIN = 70
            TEAL_H_LO = 85
            TEAL_H_HI = 100
            BASE_READ_R_MAX = 56
            CENTER_TOL_FRAC = 0.35
            TARGET_CENTER_TOL_FRAC = 0.36
            TARGET_EDGE_CENTER_TOL_FRAC = 0.52
            TARGET_WALL_REPORT_MAX_MM = 180
            LIDAR_ANGLE_SIGN = 1.0
            LIDAR_WINDOW_DEG = 6.0
            PROTRUSION_THRESHOLD_M = 0.004
            PROTRUSION_HARD_FAKE_M = 0.012
            PROTRUSION_MIN_SIDE_SAMPLES = 6
            COG_NORM_R0 = 40
            COG_NORM_OUT_R = 56
            COG_MINOR_R_MIN_PX = 13
            COG_RAY_STEP_DEG = 10
            COG_PITCH_S_LO = 0.70
            COG_PITCH_S_HI = 1.35
            COG_PITCH_ALIGN_MAX = 0.06
            COG_BAND_CONF_MIN = 0.46
            COG_FINAL_CONF_MIN = 0.56
            COG_REPORT_MIN_COLORS = 1
            COG_HOUGH_DP = 1.0
            COG_HOUGH_MINDIST = 14
            COG_HOUGH_PARAM1 = 100
            COG_HOUGH_PARAM2 = 11
            COG_CENTER_REFINE_ITERS = 3
            COG_SAFE_DECISION_ENABLED = True
            COG_SAFE_MIN_READ = 0.56
            COG_SAFE_EDGE_MIN_READ = 0.58
            COG_SAFE_S_LO = 0.86
            COG_SAFE_S_HI = 1.20
            MIN_TOKEN_AREA = BASE_MIN_TOKEN_AREA
            MIN_SQUARE_SIDE = BASE_MIN_SQUARE_SIDE
            MIN_TARGET_AREA = BASE_MIN_TARGET_AREA
            MIN_TARGET_RADIUS = BASE_MIN_TARGET_RADIUS
            READ_R_MAX = BASE_READ_R_MAX
            TARGET_EDGE_MARGIN = 3
            COG_HOUGH_MIN_R = max(5, MIN_TARGET_RADIUS)
            COG_HOUGH_MAX_R = int(round(READ_R_MAX * 1.3))
            _STROKE_COS = np.asarray([math.cos(2.0 * math.pi * k / 360.0)
                                      for k in range(360)])
            _STROKE_SIN = np.asarray([math.sin(2.0 * math.pi * k / 360.0)
                                      for k in range(360)])
            _LETTER_TPL_B64 = {
                "H": "4AAAH///4AAAAAAf4AAAAAAP4AAAAAAA4AAP8AAAwAAP/AAAgAAP/gAAgAAD/AAAgAAD+AAAgAAD8AAAgAAD8AAAgAP/8AAAgD///+AAgH///+AAgP8H5/wAgfwH4H4Ag/AH4D8Ah/AH4B+Ah+AH4A/Ah+AH4A/gj+AH4A/wD8AH4A/wD8AH4A/wD8AH4A/wD8AH4APxD8AH4AfxD8AHwA/xD8AHgA/xD+AHgA/xB+APgA/hB+AfgA/BB/AfgB/BAfwfgD+BAP4fgH8BAD//w/4BAA////wBAAAf/8ABAAAf/8ABAAAfwAABAAAfgAABAAAfgAADAAH/gAADAAH/8AADAAD/8AADAAAAcAADAAAAAAAD/+AAAAAD/+AAAAAD",
                "S": "4AAAAP//4AAAAAAH4AAAAAAH4PAAAAAA4P4DwAAA4H4D/wAA4H4D/wEA4H4B/gP84H4B+AP84H4B+APw4H4B+APw4H4B+APwgH4B+APwgH4B+APwgH4B+AfwgHwB+AfggHwB8AfAgHwB8AfAgHwB4AfAgHwB4AfAgH4B4AfAgH4D4A/AgH4D4B/AgD8D4B+AAB/D4D+AAA/H4D8AAAf/4f8AAAB///wAAAA///AAAAAD/4AAAAAD4AAAAAADwAAAAAAHwAAAAAAPwAAAAAAPwAAAAAAPwAADAAAPwAADAAAPwAADAAAPwAADAAAPwAADAAAfwAADAAA/4AADAAA/+AADAAA/8AAD4AAAAAAD//AAAAAD///+AAAH///+AAAH",
                "U": "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAP/AAAAAB//4AAAAH//+AAAA/wD/AAAB/gB/wAAB/AAf4AAD+AAP8AAH8AAH+AAP8AAH+AAP8AAH+AAP4AAD+AAP4AAD/AAf4AAD/AAf4AAD/AAf4AAD/AAfwAAD/AAfwAAD/AAfwAAD/AAfwAAD/AAfwAAD/AAfwAAD/AAfwAAD/AAfwAAD/AAf4AAD/AAf4AAD+AAP4AAD+AAP4AAH+AAP4AAH8AAH4AAH8AAH8AAH8AAB8AAPwAAB8AAPwADh+AAPgADg+AAfgAHgeAAfAwHgPAA+B4HwPgB8B8H+PgB+D8H//gD//4H//gD//4H//gD//wD//gD//wB//gD//wAAAAAH/w",
            }
            LETTER_TPL_SIZE = 48
            LETTER_MATCH_ROT_STEP = 20
            LETTER_IOU_MIN = 0.38
            LETTER_IOU_MARGIN = 0.05
            _LETTER_TEMPLATES = None
            _LETTER_TEMPLATE_STACKS = None
            MIN_FRAMES = 3
            MIN_SPAN_S = .20
            MAX_GAP_S = .35
            VERIFIED_SOURCES = frozenset(('visible-core-and-partial-arcs',
                                          'visible-small-core-and-boundaries',
                                          'clipped-visible-boundaries'))
            RDF_FROM_FLU = np.array(((0., -1., 0.), (0., 0., -1.), (1., 0., 0.)))
            VICTIM_VISUAL_ACTIVE = True  # experimental upper-camera plaque gate
            TARGET_VICTIM_ANALYZE_PERIOD_S = 0.10  # one shared camera analysis at most 10 Hz
            SECONDARY_TARGET_ENABLED = True  # verified secondary targets may fill legacy misses
            CAPTURE_CALIBRATION_PATH = None  # optional robot.json for L overlays
            FINAL_RETURN_VALID_FOR_S = .35
            VICTIM_PLAQUE_DEBUG = False  # full measurements remain in explicit L captures
            VICTIM_OVERHANG_DEBUG = True  # also measure when the visual gate is disabled
            VICTIM_PLAQUE_CALIBRATION = {}
            AUTO_REPORT_ENABLED = False
            ALIGNMENT_CAPTURE_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'alignment_captures')
            LIDAR_FRONT_INDEX = 1024

            def __init__(self, robot_json=None, *, camera_size=(64, 64), camera_fov=1.,
                         capture_dir=None, mapper=None, _secondary=False, debug_enabled=False):
                self.debug_enabled = bool(debug_enabled)
                self._last_obs = None
                self._geom_cache = {}
                self._secondary = bool(_secondary)
                self.cam_w, self.cam_h = map(int, camera_size)
                self.cam_fov = float(camera_fov)
                self.mapper = mapper
                self._fusion_tracks = {}
                self._target_victim_cache = {}
                self._target_victim_cache_t = -math.inf
                self._victim_visual_tracks = {}
                if capture_dir is not None:
                    self.ALIGNMENT_CAPTURE_DIR = os.path.abspath(os.fspath(capture_dir))
                if isinstance(robot_json, dict):
                    self._configure_capture_profile(robot_json)
                    self._capture_calibration = self
                    self._capture_calibration_error = None
                elif robot_json is not None:
                    self.CAPTURE_CALIBRATION_PATH = os.path.abspath(os.fspath(robot_json))

            def _concentric_read(self, *args, **kwargs):
                method = self._secondary_concentric_read if self._secondary else self._primary_concentric_read
                return method(*args, **kwargs)

            def _order_quad(self, *args, **kwargs):
                method = self._secondary_order_quad
                return method(*args, **kwargs)

            def _projective_ring_geometry(self, *args, **kwargs):
                method = self._secondary_projective_ring_geometry if self._secondary else self._primary_projective_ring_geometry
                return method(*args, **kwargs)

            def analyze_target_victim_frame(self, *args, **kwargs):
                method = self._secondary_analyze_target_victim_frame if self._secondary else self._primary_analyze_target_victim_frame
                return method(*args, **kwargs)

            def find_target_candidates(self, *args, **kwargs):
                method = self._secondary_find_target_candidates if self._secondary else self._primary_find_target_candidates
                return method(*args, **kwargs)

            # Frame normalization and legacy recognition.

            def _configure_for_frame(self, frame_bgr):
                """Scale pixel thresholds for 64x64 / 128x128 style camera frames."""

                self.CAM_H, self.CAM_W = frame_bgr.shape[:2]
                res = self.CAM_W / float(self.BASE_CAM_W)
                self.MIN_TOKEN_AREA = max(8, int(round(self.BASE_MIN_TOKEN_AREA * res * res)))
                self.MIN_SQUARE_SIDE = max(12, int(round(self.BASE_MIN_SQUARE_SIDE * res)))
                self.MIN_TARGET_AREA = max(20, int(round(self.BASE_MIN_TARGET_AREA * res * res)))
                self.MIN_TARGET_RADIUS = max(3, int(round(self.BASE_MIN_TARGET_RADIUS * res)))
                self.READ_R_MAX = int(round(self.BASE_READ_R_MAX * res))
                self.TARGET_EDGE_MARGIN = max(1, int(round(3 * res)))
                self.COG_HOUGH_MIN_R = max(5, self.MIN_TARGET_RADIUS)
                self.COG_HOUGH_MAX_R = int(round(self.READ_R_MAX * 1.3))

            def _normalize_frame(self, frame, color_order="bgr"):
                """Return a BGR uint8 image from BGR/RGB/BGRA/RGBA input."""
                arr = np.asarray(frame)
                if arr.ndim != 3 or arr.shape[2] not in (3, 4):
                    raise ValueError("frame must have shape (H, W, 3) or (H, W, 4)")
                if arr.dtype != np.uint8:
                    arr = np.clip(arr, 0, 255).astype(np.uint8)
                if arr.shape[2] == 4:
                    arr = arr[:, :, :3]
                if color_order.lower() == "rgb":
                    arr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
                elif color_order.lower() != "bgr":
                    raise ValueError("color_order must be 'bgr' or 'rgb'")
                return arr.copy()

            def _camera_frame(self, data, side):
                """Read native BGRA bytes or a BGR/BGRA array without Webots imports."""
                image = data['cameras'][side]
                if isinstance(image, (bytes, bytearray, memoryview)):
                    info = (data.get('camera_intrinsics') or {}).get(side) or {}
                    width = int(info.get('width', data.get('cam_w', self.cam_w)))
                    height = int(info.get('height', data.get('cam_h', self.cam_h)))
                    if min(width,height) < 2 or len(image) != width*height*4:
                        raise ValueError('camera buffer does not match its BGRA dimensions')
                    image = np.frombuffer(image,np.uint8).reshape(height,width,4)
                return self._normalize_frame(image)

            def classify_color(self, hsv_pixel):
                h, s, v = int(hsv_pixel[0]), int(hsv_pixel[1]), int(hsv_pixel[2])
                if v < self.BLACK_V_MAX:
                    return "black"
                if s < self.GRAY_S_MAX:
                    return "unknown"
                if h < 10 or h >= 170:
                    return "red"
                if 20 <= h < 35:
                    return "yellow"
                if 40 <= h < 85:
                    return "green"
                if 100 <= h < 130:
                    return "blue"
                return "unknown"

            def _primary_order_quad(self, pts):
                pts = np.asarray(pts, dtype=np.float32)
                s = pts.sum(axis=1)
                d = np.diff(pts, axis=1).ravel()
                return np.array(
                    [pts[np.argmin(s)], pts[np.argmin(d)], pts[np.argmax(s)], pts[np.argmax(d)]],
                    dtype=np.float32,
                )

            def _warp_square(self, bgr, rect, out=NORM_SIZE):
                return self._warp_quad(bgr, cv2.boxPoints(rect), out)

            def _warp_quad(self, bgr, quad, out=NORM_SIZE):
                src = self._order_quad(quad)
                dst = np.array([[0, 0], [out - 1, 0], [out - 1, out - 1], [0, out - 1]], dtype=np.float32)
                m = cv2.getPerspectiveTransform(src, dst)
                return cv2.warpPerspective(bgr, m, (out, out))

            def _is_letter_token(self, crop_bgr):
                hsv = cv2.cvtColor(crop_bgr, cv2.COLOR_BGR2HSV)
                gray = cv2.cvtColor(crop_bgr, cv2.COLOR_BGR2GRAY)
                gray = cv2.resize(gray, (self.NORM_SIZE, self.NORM_SIZE), interpolation=cv2.INTER_AREA)
                hsv = cv2.resize(hsv, (self.NORM_SIZE, self.NORM_SIZE), interpolation=cv2.INTER_AREA)
                m = int(self.NORM_SIZE * self.INNER_INSET)
                inner = gray[m:self.NORM_SIZE - m, m:self.NORM_SIZE - m]
                inner_hsv = hsv[m:self.NORM_SIZE - m, m:self.NORM_SIZE - m]
                std = float(inner.std())
                if std < self.LETTER_MIN_STD:
                    return False, {"std": std, "reason": "blank/low contrast"}
                bright = float((inner >= 150).mean())
                colored = float(((inner_hsv[:, :, 1] >= 80) & (inner_hsv[:, :, 2] >= 60)).mean())
                _, b = cv2.threshold(inner, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
                dark = float((b > 0).mean())
                ok = bright >= self.LETTER_BRIGHT_MIN and self.LETTER_DARK_MIN <= dark <= self.LETTER_DARK_MAX and colored <= self.LETTER_COLOR_MAX
                return ok, {
                    "std": std,
                    "bright": bright,
                    "dark": dark,
                    "colored": colored,
                    "reason": "token" if ok else "not letter token",
                }

            def _count_holes(self, bin_fg):
                cnts, hier = cv2.findContours(bin_fg, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)
                if hier is None or len(cnts) == 0:
                    return 0, False, None
                hier = hier[0]
                externals = [i for i in range(len(cnts)) if hier[i][3] == -1]
                if not externals:
                    return 0, False, None
                main = max(externals, key=lambda i: cv2.contourArea(cnts[i]))
                main_area = cv2.contourArea(cnts[main])
                if main_area < self.MIN_SYMBOL_AREA:
                    return 0, False, None
                min_hole = main_area * self.HOLE_AREA_FRAC
                holes = 0
                for i in range(len(cnts)):
                    if hier[i][3] == main and cv2.contourArea(cnts[i]) >= min_hole:
                        holes += 1
                return holes, True, cnts[main]

            def _deep_defect_count(self, cnt):
                area = cv2.contourArea(cnt)
                if area <= 0 or len(cnt) < 4:
                    return 0, [], 0.0
                r_eq = math.sqrt(area / math.pi)
                try:
                    hull = cv2.convexHull(cnt, returnPoints=False)
                    defects = cv2.convexityDefects(cnt, hull)
                except cv2.error:
                    return 0, [], 0.0
                if defects is None:
                    return 0, [], 0.0
                depths = sorted(
                    (float(row[3]) / 256.0 for row in np.asarray(defects).reshape(-1, 4)),
                    reverse=True,
                )
                thr = self.DEFECT_DEPTH_FRAC * r_eq
                n_deep = sum(1 for d in depths if d >= thr)
                deep_ratio = (depths[0] / r_eq) if depths and r_eq > 0 else 0.0
                return n_deep, [round(x, 1) for x in depths[:5]], deep_ratio

            def _count_radial_strokes(self, bin_fg, fracs=(0.5, 0.65, 0.8)):
                m = cv2.moments(bin_fg, binaryImage=True)
                if m["m00"] <= 0:
                    return 0
                cx, cy = m["m10"] / m["m00"], m["m01"] / m["m00"]
                ys, xs = np.nonzero(bin_fg)
                if len(xs) == 0:
                    return 0
                rmax = float(np.sqrt(((xs - cx) ** 2 + (ys - cy) ** 2)).max())
                h, w = bin_fg.shape
                counts = []
                for frac in fracs:
                    r = frac * rmax
                    px = np.rint(cx + r * self._STROKE_COS).astype(np.intp)
                    py = np.rint(cy + r * self._STROKE_SIN).astype(np.intp)
                    valid = (px >= 0) & (px < w) & (py >= 0) & (py < h)
                    ring = np.zeros(360, dtype=bool)
                    ring[valid] = bin_fg[py[valid], px[valid]] > 0
                    counts.append(int(np.count_nonzero(ring & ~np.roll(ring, 1))))
                counts.sort()
                return counts[len(counts) // 2]

            def _is_psi_by_defect_balance(self, n_deep, depths):
                if n_deep < self.PSI_MIN_DEEP or len(depths) < self.PSI_MIN_DEEP or depths[0] <= 0:
                    return False
                second_ratio = depths[1] / depths[0]
                third_ratio = depths[2] / depths[0]
                return second_ratio >= self.PSI_SECOND_DEPTH_FRAC and third_ratio >= self.PSI_THIRD_DEPTH_FRAC

            def _zero_hole_omega_guard(self, cnt, n_deep, depths, deep_ratio, strokes):
                area = cv2.contourArea(cnt)
                if area <= 0:
                    return False, "area0"
                hull = cv2.convexHull(cnt)
                hull_area = cv2.contourArea(hull)
                solidity = (area / hull_area) if hull_area > 0 else 1.0
                ok = (
                    solidity <= self.OMEGA_MAX_SOLIDITY
                    and deep_ratio >= self.OMEGA_MIN_DEEP_RATIO
                    and strokes in self.OMEGA_ALLOWED_STROKES
                    and (strokes != 4 or deep_ratio >= self.OMEGA_STROKES4_MIN_DEEP_RATIO)
                    and n_deep >= 1
                    and len(depths) >= 1
                )
                return ok, "solidity=%.2f deep=%.2f strokes=%d n_deep=%d" % (solidity, deep_ratio, strokes, n_deep)

            def _build_letter_templates(self):
                out = {}
                n = self.LETTER_TPL_SIZE
                ctr = (n / 2.0, n / 2.0)
                for k, s in self._LETTER_TPL_B64.items():
                    bits = np.unpackbits(np.frombuffer(base64.b64decode(s), np.uint8))
                    img = (bits[: n * n].reshape(n, n) * 255).astype(np.uint8)
                    rots = []
                    for ang in range(0, 360, self.LETTER_MATCH_ROT_STEP):
                        m = cv2.getRotationMatrix2D(ctr, ang, 1.0)
                        rots.append(cv2.warpAffine(img, m, (n, n)) > 0)
                    out[k] = rots
                return out

            def _get_letter_templates(self):
                """Lazily build the rotated letter templates (needs cv2 at call time)."""
                if self._LETTER_TEMPLATES is None:
                    self._LETTER_TEMPLATES = self._build_letter_templates()
                return self._LETTER_TEMPLATES

            def _get_letter_template_stacks(self):
                """Return the same templates in arrays for one-pass IoU evaluation."""
                if self._LETTER_TEMPLATE_STACKS is None:
                    self._LETTER_TEMPLATE_STACKS = {
                        label: np.stack(rotations)
                        for label, rotations in self._get_letter_templates().items()
                    }
                return self._LETTER_TEMPLATE_STACKS

            def _match_letter(self, binimg):
                q = cv2.resize(binimg, (self.LETTER_TPL_SIZE, self.LETTER_TPL_SIZE), interpolation=cv2.INTER_NEAREST) > 0
                scores = {}
                for k, rots in self._get_letter_template_stacks().items():
                    unions = np.count_nonzero(rots | q, axis=(1, 2))
                    intersections = np.count_nonzero(rots & q, axis=(1, 2))
                    valid = unions > 0
                    scores[k] = (float(np.max(intersections[valid] / unions[valid]))
                                 if np.any(valid) else 0.0)
                ranked = sorted(scores.items(), key=lambda kv: kv[1], reverse=True)
                lab, iou = ranked[0]
                margin = iou - ranked[1][1]
                return lab, iou, margin, scores

            def _nearest_to_center(self, candidates, get_cx):
                best = None
                best_d = None
                center = self.CAM_W / 2.0
                for cand in candidates:
                    d = abs(get_cx(cand) - center)
                    if best_d is None or d < best_d:
                        best_d = d
                        best = cand
                return best, best_d

            def _target_wall_guard(self, candidate, wall_distance_mm=None):
                if wall_distance_mm is not None and wall_distance_mm > self.TARGET_WALL_REPORT_MAX_MM:
                    return False, "wall distance too far"
                return True, "ok"

            def _label_name(self, kind, label):
                if kind == "victim":
                    return self.VICTIM_NAMES.get(label, label)
                if kind == "target":
                    return self.TARGET_NAMES.get(label, label)
                return label

            def _save_code(self, kind, label):
                if kind == "victim":
                    return self.VICTIM_SAVE_CODES.get(label)
                if kind == "target":
                    return self.TARGET_SAVE_CODES.get(label)
                return None

            def _round_float(self, value, digits=3):
                if value is None:
                    return None
                try:
                    return round(float(value), digits)
                except Exception:
                    return value

            def _clean_center(self, center):
                if center is None:
                    return None
                return (self._round_float(center[0], 2), self._round_float(center[1], 2))

            def _make_report(self, kind, label, reportable, fake_victim=None, fake_target=None):
                name = self._label_name(kind, label)
                save_code = self._save_code(kind, label)
                victim_blocked = (
                    kind == "victim"
                    and fake_victim is not None
                    and fake_victim.get("enabled")
                    and fake_victim.get("block_report")
                )
                fake_rejected = (
                    kind == "victim"
                    and fake_victim is not None
                    and fake_victim.get("enabled")
                    and fake_victim.get("is_real") is False
                )

                should_report = bool(
                    kind and label and reportable and save_code is not None
                    and not victim_blocked and not fake_rejected
                )
                if victim_blocked:
                    reason = fake_victim.get("reason") or "victim report blocked"
                elif fake_rejected:
                    reason = "fake victim rejected"
                elif not kind or not label:
                    # nothing real to report; surface a confidently-read FAKE target
                    # (never reported to Erebus) instead of silently dropping it
                    reason = ("fake detected @ %s" % (fake_target,)) if fake_target is not None else "nothing detected"
                elif not reportable:
                    reason = "detected but not centered/reportable"
                elif save_code is None:
                    reason = "unknown save code"
                else:
                    reason = "ok"

                return {
                    "should_report": should_report,
                    "kind": kind,
                    "label": label,
                    "name": name,
                    "save_code": save_code,
                    "fake_target": fake_target,
                    "message": None if not kind or not label else ("%s %s %s" % (kind.upper(), label, name)),
                    "reason": reason,
                }

            def _extract_victim_debug(self, best):
                if not best or best.get("kind") != "victim":
                    return None
                soft = best.get("soft_guess") or {}
                template = best.get("template") or {}
                return {
                    "center": self._clean_center(best.get("center")),
                    "centered": bool(best.get("centered")),
                    "size": best.get("size"),
                    "template_label": template.get("label_guess"),
                    "iou": self._round_float(template.get("iou")),
                    "margin": self._round_float(template.get("margin")),
                    "template_scores": template.get("scores"),
                    "soft_guess": soft.get("label"),
                    "soft_confidence": self._round_float(soft.get("confidence")),
                    "soft_reason": soft.get("reason"),
                    "token": best.get("token"),
                    "path": best.get("path"),
                }

            def _extract_target_debug(self, best):
                if not best or best.get("kind") != "target":
                    return None
                obs = best.get("observation") or {}
                return {
                    "center": self._clean_center(best.get("center")),
                    "centered": bool(best.get("centered")),
                    "radius_px": self._round_float(best.get("radius")),
                    "bbox": best.get("bbox"),
                    "edge": bool(best.get("edge")),
                    "partial": bool(best.get("partial")),
                    "bands_inner_to_outer": list(best.get("bands") or []),
                    "band_confidences": list(obs.get("confs") or []),
                    "sum": best.get("sum"),
                    "result": best.get("label_guess"),
                    "geom": self._round_float(best.get("geom_score")),
                    "read": self._round_float(best.get("read_conf")),
                    "final": self._round_float(best.get("confidence")),
                    "pitch": self._round_float(obs.get("pitch_conf")),
                    "scale": self._round_float(best.get("pitch_scale")),
                    "symmetry": self._round_float(obs.get("sym_score")),
                    "center_shift": self._round_float(obs.get("center_shift")),
                    "mode": obs.get("mode"),
                    "reason": best.get("reason"),
                    "structure": best.get("structure"),
                }

            def _build_log_lines(self, result, report, victim_debug, target_debug, fake_victim):
                lines = []
                if victim_debug is not None:
                    lines.append(
                        "[VICTIM] %s %s | center=%s centered=%s iou=%s margin=%s soft=%s soft_conf=%s reason=%s"
                        % (
                            result.get("label"),
                            self._label_name("victim", result.get("label")),
                            victim_debug.get("center"),
                            victim_debug.get("centered"),
                            victim_debug.get("iou"),
                            victim_debug.get("margin"),
                            victim_debug.get("soft_guess"),
                            victim_debug.get("soft_confidence"),
                            victim_debug.get("soft_reason"),
                        )
                    )
                if target_debug is not None:
                    bands = "-".join({"red":"R", "green":"G", "blue":"B",
                                      "yellow":"Y", "black":"K"}.get(x, "?")
                                     for x in target_debug.get("bands_inner_to_outer", []))
                    lines.append(
                        "[TARGET] %s %s | status=ACCEPTED report_ready=%s | "
                        "colors(inner_to_outer)=%s sum=%s confidence=%s"
                        % (
                            result.get("label"),
                            self._label_name("target", result.get("label")),
                            bool(report.get("should_report")),
                            bands,
                            target_debug.get("sum"),
                            target_debug.get("final"),
                        )
                    )
                # Reflect every target-ish thing the detector SENSED on the wall but did
                # not take as a real detection (fake, low confidence, ...), with the same
                # detail as a real [TARGET] line, so no potential wall target is missed.
                # Hunches only: tagged [TARGET?], they never change should_report.
                best = result.get("best")
                rejected = [cand for cand in (result.get("target") or {}).get("candidates", [])
                            if cand is not best and cand.get("observation") is not None]
                if target_debug is None and rejected:
                    cand = max(rejected, key=lambda row: float(row.get("confidence") or 0.0))
                    td = self._extract_target_debug(cand)
                    if td is not None:
                        bands = "-".join({"red":"R", "green":"G", "blue":"B",
                                          "yellow":"Y", "black":"K"}.get(x, "?")
                                         for x in td.get("bands_inner_to_outer", []))
                        raw_reason = str(td.get("reason") or "unknown")
                        if raw_reason == "low confidence":
                            reason = "color reading confidence is too low"
                        elif raw_reason == "too small/thin to read rings":
                            reason = "target is too small or thin"
                        elif raw_reason == "fake":
                            reason = "color sum does not match a real target type"
                        elif "outside" in raw_reason:
                            reason = "ring spacing is inconsistent"
                        elif raw_reason.startswith("read "):
                            reason = "color reading is below the threshold"
                        else:
                            reason = raw_reason
                    lines.append(
                        "[TARGET?] best_candidate=%s | status=REJECTED | "
                        "colors(inner_to_outer)=%s confidence=%s | reason=%s"
                        % (
                            cand.get("label_guess"),
                            bands,
                            td.get("final"),
                            reason,
                        )
                    )
                if fake_victim is not None and fake_victim.get("enabled"):
                    lines.append(
                        "[FAKE-VICTIM] is_real=%s reason=%s"
                        % (
                            fake_victim.get("is_real"),
                            fake_victim.get("reason"),
                        )
                    )
                lines.append(
                    "[REPORT] should_report=%s kind=%s label=%s name=%s save_code=%s reason=%s"
                    % (
                        report.get("should_report"),
                        report.get("kind"),
                        report.get("label"),
                        report.get("name"),
                        report.get("save_code"),
                        report.get("reason"),
                    )
                )
                return lines

            def find_letter_candidates(self, bgr, hsv=None):
                if hsv is None:
                    hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
                white = cv2.inRange(hsv, (0, 0, self.WHITE_V_MIN), (180, self.WHITE_S_MAX, 255))
                white = cv2.morphologyEx(white, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8))
                cnts, _ = cv2.findContours(white, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
                out = []
                for c in cnts:
                    area = cv2.contourArea(c)
                    if area < self.MIN_TOKEN_AREA:
                        continue
                    rect = cv2.minAreaRect(c)
                    (w, h) = rect[1]
                    if w <= 0 or h <= 0:
                        continue
                    long_s, short_s = max(w, h), min(w, h)
                    undersized = short_s < self.MIN_SQUARE_SIDE
                    ar = long_s / short_s
                    fill = area / (w * h)
                    peri = cv2.arcLength(c, True)
                    approx = cv2.approxPolyDP(c, 0.04 * peri, True)
                    # A raster contour's two measured extents each have a one-
                    # pixel boundary allowance. At64px, a30x18px valid plaque
                    # measures1.667 instead of<=1.60. Test its lower aspect
                    # bound; do not relax symbol, temporal or fake-victim gates.
                    perspective_ok=(long_s-1.) <= self.SQUARE_AR_PERSP*(short_s+1.)
                    if len(approx) == 4 and cv2.isContourConvex(approx) and perspective_ok and fill >= self.SQUARE_FILL_PERSP:
                        crop = self._warp_quad(bgr, approx.reshape(4, 2).astype(np.float32))
                        path = "quad"
                    elif ar <= self.SQUARE_AR_MAX and fill >= self.SQUARE_FILL_MIN:
                        crop = self._warp_square(bgr, rect)
                        path = "min_rect"
                    else:
                        continue
                    ok, token_debug = self._is_letter_token(crop)
                    if not ok and path == "quad":
                        alt = self._warp_square(bgr, rect)
                        ok2, token_debug2 = self._is_letter_token(alt)
                        if ok2:
                            crop, ok, token_debug, path = alt, ok2, token_debug2, "quad_to_min_rect"
                    if ok and undersized:
                        # Small is not evidence of a false glyph. Retain all
                        # normal area/aspect/fill/token gates, then require two
                        # different rectifications to agree at strong margins.
                        if len(approx) != 4 or not cv2.isContourConvex(approx):
                            continue
                        quad_crop = self._warp_quad(bgr, approx.reshape(4, 2).astype(np.float32))
                        rect_crop = self._warp_square(bgr, rect)
                        if not self._is_letter_token(quad_crop)[0] or not self._is_letter_token(rect_crop)[0]:
                            continue
                        quad_label, quad_debug = self.classify_with_debug(quad_crop)
                        rect_label, rect_debug = self.classify_with_debug(rect_crop)
                        if (quad_label is None or rect_label != quad_label
                                or min(quad_debug['margin'], rect_debug['margin']) < 2*self.LETTER_IOU_MARGIN):
                            continue
                        crop = quad_crop
                        path = 'small_quad_rect_agreement'
                    if ok:
                        out.append({"rect": rect, "crop": crop, "path": path, "token_debug": token_debug})
                return out

            def classify_with_debug(self, crop_bgr, binimg=None):
                if binimg is None:
                    binimg = self._letter_binary(crop_bgr)
                lab, iou, margin, scores = self._match_letter(binimg)
                accepted = iou >= self.LETTER_IOU_MIN and margin >= self.LETTER_IOU_MARGIN
                return (lab if accepted else None), {
                    "label_guess": lab,
                    "iou": float(iou),
                    "margin": float(margin),
                    "scores": {k: float(v) for k, v in scores.items()},
                    "accepted": bool(accepted),
                }

            def _letter_binary(self, crop_bgr):
                gray = cv2.cvtColor(crop_bgr, cv2.COLOR_BGR2GRAY)
                gray = cv2.resize(gray, (self.NORM_SIZE, self.NORM_SIZE), interpolation=cv2.INTER_AREA)
                m = int(self.NORM_SIZE * self.INNER_INSET)
                gray = gray[m:self.NORM_SIZE - m, m:self.NORM_SIZE - m]
                gray = cv2.GaussianBlur(gray, (3, 3), 0)
                _, binimg = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
                k = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (self.LETTER_CLOSE_KSIZE, self.LETTER_CLOSE_KSIZE))
                return cv2.morphologyEx(binimg, cv2.MORPH_CLOSE, k)

            def classify_guess(self, crop_bgr, binimg=None):
                if binimg is None:
                    binimg = self._letter_binary(crop_bgr)
                holes, ok, main_cnt = self._count_holes(binimg)
                if not ok or main_cnt is None:
                    return None, 0.0, "no symbol"
                main_area = cv2.contourArea(main_cnt)
                if holes >= 2:
                    return "H", 0.95, "holes=%d" % holes
                if holes == 1:
                    conf = 0.90 if main_area >= self.PHI_ONE_HOLE_MIN_MAIN_AREA else max(0.25, 0.70 * main_area / self.PHI_ONE_HOLE_MIN_MAIN_AREA)
                    return "H", min(conf, 0.90), "holes=1 main_area=%.0f" % main_area
                n_deep, depths, deep_ratio = self._deep_defect_count(main_cnt)
                strokes = self._count_radial_strokes(binimg)
                is_psi = self._is_psi_by_defect_balance(n_deep, depths)
                if is_psi:
                    area = cv2.contourArea(main_cnt)
                    hull_area = cv2.contourArea(cv2.convexHull(main_cnt))
                    psi_solidity = (area / hull_area) if hull_area > 0 else 1.0
                    if psi_solidity < self.PSI_MIN_SOLIDITY:
                        is_psi = False
                omega_ok, omega_dbg = self._zero_hole_omega_guard(main_cnt, n_deep, depths, deep_ratio, strokes)
                second_ratio = (depths[1] / depths[0]) if len(depths) > 1 and depths[0] > 0 else 0.0
                third_ratio = (depths[2] / depths[0]) if len(depths) > 2 and depths[0] > 0 else 0.0
                if is_psi:
                    conf = min(0.92, 0.62 + 0.10 * n_deep + 0.15 * min(second_ratio, 1.0) + 0.10 * min(third_ratio, 1.0))
                    return "S", conf, "Psi-like n_deep=%d strokes=%d" % (n_deep, strokes)
                if omega_ok:
                    conf = min(0.90, 0.55 + 0.20 * min(deep_ratio, 1.5) + (0.10 if strokes in self.OMEGA_ALLOWED_STROKES else 0.0))
                    return "U", conf, "Omega-like %s" % omega_dbg
                return None, 0.0, "ambiguous"

            def _primary_projective_ring_geometry(self, bgr, hsv=None):
                """Share pure pixel geometry between the two decoders, not live gates."""
                if hsv is None:hsv=cv2.cvtColor(bgr,cv2.COLOR_BGR2HSV)
                key=(hsv.shape,hsv.dtype.str,hsv.tobytes(),self.BLACK_V_MAX,self.GRAY_S_MAX,
                     tuple(sorted(self.COLOR_VALUE.items())),tuple(sorted(self.SUM_TO_TYPE.items())))
                cache=getattr(self,'_primary_geometry_cache',None)
                if cache is None:cache=self._primary_geometry_cache={}
                if key in cache:
                    result=cache.pop(key)
                else:
                    result=self._compute_primary_ring_geometry(bgr,hsv=hsv)
                cache[key]=result
                if len(cache)>2:cache.pop(next(iter(cache)))
                return copy.deepcopy(result)

            def _compute_primary_ring_geometry(self, bgr, hsv=None):
                original = self._fit_primary_ring_geometry(bgr, hsv=hsv)
                if original is not None:
                    return original
                # A measured central disc can contain repeated same-colour bands.
                # Try the only other possible core extents with three colours in
                # five rings. Geometry/pixel checks decide; never pick by label.
                alternatives = [self._fit_primary_ring_geometry(bgr, hsv=hsv, core_rings=k)
                                for k in (2,3)]
                alternatives = [fit for fit in alternatives if fit is not None]
                return alternatives[0] if len(alternatives)==1 else None


            def _fit_primary_ring_geometry(self, bgr, hsv=None, core_rings=1):
                """Fit several ring boundaries together, including perspective centre drift.

        Work in units of the central disc radius. The radial metric divided by
        a linear denominator describes a plane viewed through a pinhole camera.
        No target label is used when fitting or selecting the five colours.
        """
                if hsv is None:
                    hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
                height, width = hsv.shape[:2]
                names = ('red', 'green', 'blue', 'yellow', 'black', 'unknown')
                hue, sat, val = cv2.split(hsv)
                labels = np.full((height, width), 5, np.uint8)
                for i, mask in enumerate(((hue < 10) | (hue >= 170),
                                          (hue >= 40) & (hue < 85),
                                          (hue >= 100) & (hue < 130),
                                          (hue >= 20) & (hue < 35))):
                    labels[mask & (sat >= self.GRAY_S_MAX)] = i
                labels[val < self.BLACK_V_MAX] = 4
                # Every accepted model later needs three distinct bands, each
                # with at least eight original pixels. Reject impossible fits
                # before contours/ellipses/least-squares; no confidence is relaxed.
                if np.count_nonzero(np.bincount(labels.ravel(),minlength=6)[:5]>=8)<3:
                    return None
                ellipses = []
                for color in range(5):
                    contours, hierarchy = cv2.findContours(
                        (labels == color).astype(np.uint8), cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
                    for i, contour in enumerate(contours):
                        pts = contour[:, 0]
                        visible = ((pts[:, 0] > 0) & (pts[:, 0] < width-1)
                                   & (pts[:, 1] > 0) & (pts[:, 1] < height-1))
                        curved = contour[visible]
                        if len(curved) < 12:
                            continue
                        (cx, cy), (a, b), angle = cv2.fitEllipse(curved)
                        if (not (0 < cx < width and 0 < cy < height) or a < 3
                                or b > 1.75 * max(width, height)):
                            continue
                        theta = math.radians(angle)
                        rotation = np.array([[math.cos(theta), -math.sin(theta)],
                                             [math.sin(theta), math.cos(theta)]])
                        uv = (curved[:, 0] - (cx, cy)) @ rotation
                        error = np.quantile(np.abs(np.sqrt((2*uv[:, 0]/a)**2
                                                           + (2*uv[:, 1]/b)**2)-1), .9)
                        # Raster contours have half-pixel boundary uncertainty.
                        # A valid oblique core can be only four pixels wide;
                        # its relative fit error must scale with that footprint.
                        if error > max(.12,1./max(1.,min(a,b))):
                            continue
                        depth, parent = 0, hierarchy[0, i, 3]
                        while parent >= 0:
                            depth += 1
                            parent = hierarchy[0, parent, 3]
                        hole = depth % 2 == 1
                        # Contours lie at pixel centres: estimate the colour boundary
                        # halfway to the neighbouring pixel, on either side of a hole.
                        a += -1 if hole else 1
                        b += -1 if hole else 1
                        # The innermost disc may be black. Background regions cannot
                        # pass the later concentric-level, five-band and sector-agreement
                        # checks, so excluding black here only disables valid targets.
                        core = (not hole and hierarchy[0, i, 2] < 0
                                and bool(np.all(visible)))
                        ellipses.append((np.array([cx, cy]), a, b, angle, core))
                cores = sorted((e for e in ellipses if e[4]), key=lambda e: e[1]*e[2])
                if not cores:
                    return None
                # At least three measured minor-axis pixels, plus the boundary
                # correction, resolve a core. Full five-band pixel/sector
                # agreement below still decides validity; aspect ratio does not.
                center, a, b, angle, _ = cores[0]
                a /= core_rings; b /= core_rings
                if a < 4 or len(ellipses) < 4:
                    return None
                theta = math.radians(angle)
                transform = np.array([[math.cos(theta), -math.sin(theta)],
                                      [math.sin(theta), math.cos(theta)]]) @ np.diag([2/a, 2/b])
                records = []
                angles = np.linspace(0, 2*math.pi, 40, endpoint=False)
                for c, aa, bb, angle, _ in ellipses:
                    ratio = math.sqrt(aa*bb/(a*b))
                    if np.linalg.norm((c-center) @ transform) > max(1, .5*ratio):
                        continue
                    theta = math.radians(angle)
                    rot = np.array([[math.cos(theta), -math.sin(theta)],
                                    [math.sin(theta), math.cos(theta)]])
                    points = (np.column_stack((aa/2*np.cos(angles), bb/2*np.sin(angles)))
                              @ rot.T + c-center) @ transform
                    records.append((points, ratio))
                selected = [(p, round(r)) for p, r in records if 1 <= round(r) <= 5]
                if len(set(k for _, k in selected)) < 2:
                    return None
                params = np.array([0., 0., 1., 0., 1., 0., 0.])

                def radii(parameters, points):
                    x, y = (points-parameters[:2]).T
                    squared = parameters[2]*x*x + 2*parameters[3]*x*y + parameters[4]*y*y
                    denominator = 1 + parameters[5]*x + parameters[6]*y
                    return np.sqrt(np.maximum(1e-9, squared)) / np.maximum(.2, denominator)

                for _ in range(2):
                    xy = np.concatenate([p for p, _ in selected])
                    target = np.concatenate([np.full(len(p), k) for p, k in selected])
                    for _ in range(12):
                        predicted = radii(params, xy)
                        residual = predicted-target
                        jacobian = np.column_stack([
                            (radii(params+np.eye(7)[j]*1e-5, xy)-predicted)/1e-5
                            for j in range(7)])
                        delta = np.linalg.lstsq(jacobian.T @ jacobian + np.eye(7)*.01,
                                                -jacobian.T @ residual, rcond=None)[0]
                        for step in (1., .5, .25, .1):
                            proposed = params + step*delta
                            if np.mean((radii(proposed, xy)-target)**2) < np.mean(residual**2):
                                params = proposed
                                break
                        else:
                            break
                        if np.linalg.norm(step*delta) < 1e-5:
                            break
                    selected = []
                    for points, _ in records:
                        rr = radii(params, points)
                        k = round(float(np.median(rr)))
                        if 1 <= k <= 5 and np.quantile(np.abs(rr-k), .9) < .22:
                            selected.append((points, k))
                    levels = set(k for _, k in selected)
                    if (len(levels) < 3 or max(levels) < 3
                            or core_rings > 1 and (core_rings not in levels or 5 not in levels)):
                        return None
                metric = np.array([[params[2], params[3]], [params[3], params[4]]])
                if np.min(np.linalg.eigvalsh(metric)) < .4 or np.linalg.norm(params[5:]) > .15:
                    return None
                yy, xx = np.indices((height, width))
                xy = np.einsum('ij,jk->ik', np.column_stack((xx.ravel(), yy.ravel()))-center, transform)
                radius = radii(params, xy).reshape(height, width)
                direction = np.arctan2(xy[:, 1]-params[1], xy[:, 0]-params[0]).reshape(height, width)
                bands, confidences = [], []
                for k in range(5):
                    band = (radius > k+.25) & (radius < k+.75) & (labels != 5)
                    if np.count_nonzero(band) < 8:
                        return None
                    counts = np.bincount(labels[band], minlength=5)
                    winner = int(np.argmax(counts))
                    confidence = float(counts[winner]/counts.sum())
                    # Require agreement in several visible directions, not just a
                    # majority dominated by one large surviving fragment.
                    sectors = ((direction+math.pi) * 8/(2*math.pi)).astype(int) % 8
                    votes = []
                    for sector in range(8):
                        pixels = labels[band & (sectors == sector)]
                        if len(pixels) >= 2:
                            votes.append(int(np.argmax(np.bincount(pixels, minlength=5))))
                    if confidence < .80 or len(votes) < 4 or votes.count(winner)/len(votes) < .85:
                        return None
                    bands.append(names[winner])
                    confidences.append(confidence)
                if (len(set(bands)) < 3 or core_rings > 1 and
                        (bands[:core_rings] != [bands[0]]*core_rings
                         or bands[core_rings] == bands[0])):
                    return None
                inside = radius <= 5
                ys, xs = np.nonzero(inside)
                bbox = (int(xs.min()), int(ys.min()), int(xs.max()-xs.min()+1), int(ys.max()-ys.min()+1))
                cx, cy = center + params[:2] @ np.linalg.inv(transform)
                total = sum(self.COLOR_VALUE[c] for c in bands)
                result = self.SUM_TO_TYPE.get(total, 'FAKE')
                edge = bool(inside[0].any() or inside[-1].any() or inside[:, 0].any() or inside[:, -1].any())
                observation = dict(bands=tuple(bands), confs=tuple(confidences), sum=total,
                                   result=result, cx=float(cx), cy=float(cy), bbox=bbox,
                                   r=float(max(bbox[2:])/2), geom_score=1., read_conf=min(confidences),
                                   final=min(confidences), s=1., edge=edge, mode='projective-rings',
                                   accepted=result != 'FAKE', reason='accepted' if result != 'FAKE' else 'fake',
                                   core_rings=core_rings)
                return observation



            def _primary_find_target_candidates(self, bgr, hsv=None):
                if hsv is None:
                    hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
                h, w = hsv.shape[:2]
                out = []
                self._geom_cache = {}
                projective = self._projective_ring_geometry(bgr, hsv=hsv)
                if projective is not None:
                    cx, cy, r = projective['cx'], projective['cy'], projective['r']
                    self._geom_cache[(int(round(cx)), int(round(cy)))] = {
                        'mode': 'projective-rings', 'observation': projective}
                    return [(cx, cy, r, projective['edge'], projective['bbox'], projective['edge'])]
                # The Hough fallback is relatively expensive and its result is unused
                # whenever the projective ring model succeeds.
                gray = cv2.medianBlur(cv2.cvtColor(bgr, cv2.COLOR_BGR2GRAY), 3)
                circles = cv2.HoughCircles(
                    gray,
                    cv2.HOUGH_GRADIENT,
                    self.COG_HOUGH_DP,
                    self.COG_HOUGH_MINDIST,
                    param1=self.COG_HOUGH_PARAM1,
                    param2=self.COG_HOUGH_PARAM2,
                    minRadius=self.COG_HOUGH_MIN_R,
                    maxRadius=self.COG_HOUGH_MAX_R,
                )
                # Full coloured outlines remain ellipses when viewed obliquely.
                # Exclude the cyan arena walls; retain red/yellow/green/blue paint.
                paint = (((hsv[:, :, 0] <= 130) | (hsv[:, :, 0] >= 170))
                         & ~((hsv[:, :, 0] >= 80) & (hsv[:, :, 0] <= 105))
                         & (hsv[:, :, 1] >= 100) & (hsv[:, :, 2] >= 70)).astype(np.uint8)
                contours, _ = cv2.findContours(paint, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
                ellipse_regions = []
                for contour in contours:
                    if len(contour) < 12 or cv2.contourArea(contour) < self.MIN_TARGET_AREA:
                        continue
                    x, y, bw, bh = cv2.boundingRect(contour)
                    edge = x <= 0 or y <= 0 or x+bw >= w or y+bh >= h
                    boundary = contour[:, 0, :]
                    visible = ((boundary[:, 0] > 0) & (boundary[:, 0] < w-1)
                               & (boundary[:, 1] > 0) & (boundary[:, 1] < h-1))
                    curved = contour[visible]
                    if len(curved) < 12 or len(curved) < .65*len(contour):
                        continue
                    (cx, cy), (aw, ah), angle = cv2.fitEllipse(curved)
                    if min(aw, ah) < 2*self.COG_MINOR_R_MIN_PX or min(aw, ah)/max(aw, ah) < .45:
                        continue
                    if not (0 <= cx < w and 0 <= cy < h) or max(aw, ah) > 1.5*max(w,h):
                        continue
                    th = math.radians(angle)
                    points = curved[:, 0, :].astype(float) - (cx, cy)
                    u = points[:, 0]*math.cos(th) + points[:, 1]*math.sin(th)
                    v = -points[:, 0]*math.sin(th) + points[:, 1]*math.cos(th)
                    residual = np.abs(np.sqrt((2*u/aw)**2 + (2*v/ah)**2)-1)
                    if float(np.quantile(residual, .9)) > .09:
                        continue
                    geom = {'cx': cx, 'cy': cy, 'r': max(aw, ah)/2, 'q': min(aw, ah)/max(aw, ah),
                            'angle': angle, 'aw': aw, 'ah': ah, 'score': 1.0,
                            'partial': edge, 'mode': 'ellipse'}
                    self._geom_cache[(int(round(cx)), int(round(cy)))] = geom
                    out.append((cx, cy, geom['r'], edge, (x,y,bw,bh), edge))
                    # Keep the old proposal path when the coloured contour is an inner
                    # ring (e.g. a black outer ring) and cannot be decoded reliably.
                    if self.read(bgr, cx, cy, geom['r'], edge, (x,y,bw,bh)) in self.TARGET_NAMES:
                        ellipse_regions.append((x,y,bw,bh))
                if circles is None:
                    return out
                for c in circles[0]:
                    cx, cy = float(c[0]), float(c[1])
                    if any(x <= cx < x+bw and y <= cy < y+bh for x,y,bw,bh in ellipse_regions):
                        continue
                    r_out = self._outer_radius(hsv, cx, cy, float(c[2]))
                    if r_out < self.MIN_TARGET_RADIUS:
                        continue
                    x = int(max(0, round(cx - r_out)))
                    y = int(max(0, round(cy - r_out)))
                    bw = int(min(w - x, round(2 * r_out)))
                    bh = int(min(h - y, round(2 * r_out)))
                    edge = (
                        cx - r_out <= self.TARGET_EDGE_MARGIN
                        or cy - r_out <= self.TARGET_EDGE_MARGIN
                        or cx + r_out >= w - self.TARGET_EDGE_MARGIN
                        or cy + r_out >= h - self.TARGET_EDGE_MARGIN
                    )
                    geom = {
                        "cx": cx,
                        "cy": cy,
                        "r": r_out,
                        "q": 1.0,
                        "angle": 0.0,
                        "aw": 2.0 * r_out,
                        "ah": 2.0 * r_out,
                        "score": 1.0,
                        "partial": edge,
                        "mode": "hough",
                    }
                    self._geom_cache[(int(round(cx)), int(round(cy)))] = geom
                    out.append((cx, cy, r_out, edge, (x, y, bw, bh), edge))
                return out

            def _outer_radius(self, hsv, cx, cy, r_hint):
                h, w = hsv.shape[:2]
                maxr = int(min(max(r_hint * 2.6, 18), math.hypot(h, w)))
                rs = []
                for a in range(0, 360, 15):
                    rad = math.radians(a)
                    dx, dy = math.cos(rad), math.sin(rad)
                    run = 0
                    last = 0
                    for t in range(2, maxr):
                        px = int(round(cx + dx * t))
                        py = int(round(cy + dy * t))
                        if not (0 <= px < w and 0 <= py < h):
                            break
                        if self._is_bg(hsv[py, px]):
                            run += 1
                            if run >= 2:
                                rs.append(t - 1)
                                break
                        else:
                            run = 0
                            last = t
                    else:
                        if last > 0:
                            rs.append(last)
                return float(np.median(rs)) if rs else float(r_hint)

            def _target_structure(self, hsv, cx, cy, r):
                h, w = hsv.shape[:2]
                rr = max(1, int(r))
                step = max(1, rr // 6)
                offsets = np.arange(-rr, rr + 1, step, dtype=np.int32)
                dx, dy = np.meshgrid(offsets, offsets)
                inside = dx * dx + dy * dy <= rr * rr
                px = (cx + dx[inside]).astype(np.intp)
                py = (cy + dy[inside]).astype(np.intp)
                in_frame = (px >= 0) & (px < w) & (py >= 0) & (py < h)
                pixels = hsv[py[in_frame], px[in_frame]]
                valid = len(pixels)
                if valid:
                    hh, ss, vv = pixels.T
                    usable = (ss >= self.TARGET_SAT_MIN) & (vv >= self.TARGET_VAL_MIN)
                    hue = hh[usable]
                    seen = set()
                    if np.any((hue < 10) | (hue >= 170)):
                        seen.add("red")
                    if np.any((hue >= 20) & (hue < 35)):
                        seen.add("yellow")
                    if np.any((hue >= 40) & (hue < 85)):
                        seen.add("green")
                    if np.any((hue >= 100) & (hue < 130)):
                        seen.add("blue")
                    black_frac = float(np.count_nonzero(vv < self.BLACK_V_MAX)) / valid
                else:
                    seen = set()
                    black_frac = 0.0
                return len(seen), sorted(seen), black_frac

            def _is_bg(self, px):
                h, s, v = int(px[0]), int(px[1]), int(px[2])
                if self.TEAL_H_LO <= h < self.TEAL_H_HI and s >= 50:
                    return True
                if s < self.GRAY_S_MAX and v >= 90:
                    return True
                return False

            def _uncertain(self, why, cx, cy):
                self._last_obs = {"accepted": False, "reason": why, "cx": float(cx), "cy": float(cy)}
                return None

            def read(self, bgr, cx, cy, r, edge=False, bbox=None):
                try:
                    geom = self._geom_for(cx, cy)
                    key = (cx, cy, r, edge, None if bbox is None else tuple(bbox))
                    cached = geom.get('_read_cache') if geom is not None else None
                    if cached is not None and cached[0] == key and np.array_equal(cached[1], bgr):
                        self._last_obs = dict(cached[3])
                        return cached[2]
                    result = self._concentric_read(bgr, cx, cy, r, edge, bbox)
                    if geom is not None:
                        # Geometry caches are replaced for each frame. Keep an image
                        # snapshot as well so standalone read() calls cannot reuse a
                        # result after their input buffer has been modified.
                        geom['_read_cache'] = (key, bgr.copy(), result, dict(self._last_obs or {}))
                    return result
                except Exception as exc:
                    self._last_obs = {"accepted": False, "reason": "exception: %s" % exc, "cx": float(cx), "cy": float(cy)}
                    return None

            def _geom_for(self, cx, cy):
                key = (int(round(cx)), int(round(cy)))
                if key in self._geom_cache:
                    return self._geom_cache[key]
                for (kx, ky), geom in self._geom_cache.items():
                    if abs(kx - cx) <= 2 and abs(ky - cy) <= 2:
                        return geom
                return None

            def _norm_transform(self, geom):
                r0 = float(self.COG_NORM_R0)
                rout = self.COG_NORM_OUT_R
                a = max(geom["aw"] / 2.0, 1e-6)
                b = max(geom["ah"] / 2.0, 1e-6)
                th = math.radians(geom["angle"])
                ca, sa = math.cos(th), math.sin(th)
                mat = np.array([[r0 / a, 0.0], [0.0, r0 / b]], dtype=np.float64) @ np.array(
                    [[ca, sa], [-sa, ca]], dtype=np.float64
                )
                center = np.array([geom["cx"], geom["cy"]], dtype=np.float64)
                off = np.array([rout, rout], dtype=np.float64) - mat @ center
                return np.hstack([mat, off.reshape(2, 1)]).astype(np.float32), rout

            def _sample_ray_labels(self, hsv, ccx, ccy, distances, *, stop_at_edge=True):
                """Original nearest-even pixel samples and colour rules, in batches."""
                angles=[math.radians(a)for a in range(0,360,self.COG_RAY_STEP_DEG)]
                dx=np.array([math.cos(a)for a in angles])[:,None]
                dy=np.array([math.sin(a)for a in angles])[:,None]
                distances=np.asarray(distances,float)[None,:]
                px=np.rint(ccx+dx*distances);py=np.rint(ccy+dy*distances)
                h,w=hsv.shape[:2]
                valid=(px>=0)&(px<w)&(py>=0)&(py<h)
                if stop_at_edge:valid=np.logical_and.accumulate(valid,axis=1)
                pixels=hsv[np.clip(py,0,h-1).astype(np.intp),np.clip(px,0,w-1).astype(np.intp)]
                hue,saturation,value=np.moveaxis(pixels,-1,0)
                labels=np.zeros(valid.shape,np.uint8)
                colour=valid&(value>=self.BLACK_V_MAX)&(saturation>=self.GRAY_S_MAX)
                labels[colour&((hue<10)|(hue>=170))]=2
                labels[colour&(hue>=20)&(hue<35)]=3
                labels[colour&(hue>=40)&(hue<85)]=4
                labels[colour&(hue>=100)&(hue<130)]=5
                labels[valid&(value<self.BLACK_V_MAX)]=1
                return labels

            def _ray_transitions(self, hsv, ccx, ccy, rmax):
                labels=self._sample_ray_labels(hsv,ccx,ccy,np.arange(2,int(rmax)))
                if not labels.shape[1]:return labels,np.zeros(labels.shape,bool),np.zeros(labels.shape,int)
                known=labels!=0
                indices=np.maximum.accumulate(np.where(known,np.arange(labels.shape[1]),-1),axis=1)
                previous=np.pad(indices[:,:-1],((0,0),(1,0)),constant_values=-1)
                before=labels[np.arange(len(labels))[:,None],np.maximum(previous,0)]
                return labels,known&(previous>=0)&(labels!=before),previous

            def _first_transition_radii(self, hsv, ccx, ccy, rmax):
                labels,changes,_=self._ray_transitions(hsv,ccx,ccy,rmax)
                if not labels.shape[1]:return {}
                rows=np.flatnonzero(np.any(changes,axis=1))
                first=np.argmax(changes[rows],axis=1)
                return {int(row*self.COG_RAY_STEP_DEG):int(col+2)for row,col in zip(rows,first)}

            def _refine_center_norm(self, hsv, ccx, ccy, rmax):
                for _ in range(self.COG_CENTER_REFINE_ITERS):
                    radii = self._first_transition_radii(hsv, ccx, ccy, rmax)
                    sx = sy = 0.0
                    n = 0
                    for ang, ra in radii.items():
                        opp = (ang + 180) % 360
                        if opp in radii:
                            rad = math.radians(ang)
                            sx += (ra - radii[opp]) * 0.5 * math.cos(rad)
                            sy += (ra - radii[opp]) * 0.5 * math.sin(rad)
                            n += 1
                    if n == 0:
                        break
                    ccx += sx / n
                    ccy += sy / n
                    if abs(sx / n) < 0.5 and abs(sy / n) < 0.5:
                        break
                return ccx, ccy

            def _transition_symmetry_score(self, hsv, ccx, ccy, rmax):
                radii = self._first_transition_radii(hsv, ccx, ccy, rmax)
                diffs = []
                for ang in range(0, 180, self.COG_RAY_STEP_DEG):
                    opp = (ang + 180) % 360
                    if ang in radii and opp in radii:
                        diffs.append(abs(radii[ang] - radii[opp]))
                if not diffs:
                    return 0.0, 0, None
                med_diff = float(np.median(diffs))
                score = max(0.0, 1.0 - med_diff / max(float(self.COG_NORM_R0), 1.0))
                return score, len(diffs), med_diff

            def _radial_transitions(self, hsv, ccx, ccy, rmax):
                _,changes,previous=self._ray_transitions(hsv,ccx,ccy,rmax)
                rows,cols=np.nonzero(changes)
                return ((cols+previous[rows,cols]+4)/2.).tolist()

            def _pitch_scale(self, fracs):
                fracs = [f for f in fracs if 0.05 < f < 1.40]
                if not fracs:
                    return 1.0, self.COG_PITCH_ALIGN_MAX
                scales = []
                s = self.COG_PITCH_S_LO
                while s <= self.COG_PITCH_S_HI + 1e-9:
                    scales.append(s)
                    s += 0.01
                if not scales:
                    return 1.0, 1e9
                # Preserve the original accumulated scale values and first-minimum
                # tie breaking, while evaluating all transitions in NumPy.
                ratios = np.asarray(fracs)[None, :] / (.2 * np.asarray(scales)[:, None])
                errors = np.median(np.abs(ratios - np.rint(ratios)), axis=1)
                best = int(np.argmin(errors))
                return scales[best], float(errors[best])

            def _read_rings_norm(self, hsv, ccx, ccy, pitch):
                distances=[(k+fr)*pitch for k in range(5)for fr in(.30,.50,.70)]
                sampled=self._sample_ray_labels(hsv,ccx,ccy,distances,stop_at_edge=False)
                names=('unknown','black','red','yellow','green','blue');bands=[];confs=[]
                for k in range(5):
                    labels=sampled[:,k*3:k*3+3].ravel()
                    labels=labels[labels!=0].tolist()
                    if not labels:return None,None
                    top,cnt=Counter(labels).most_common(1)[0]
                    bands.append(names[top]);confs.append(cnt/float(len(labels)))
                return bands,confs

            def _primary_concentric_read(self, bgr, cx, cy, r, edge, bbox):
                self._last_obs = None
                geom = self._geom_for(cx, cy)
                if geom is None:
                    return self._uncertain("no round geometry", cx, cy)
                if geom['mode'] == 'projective-rings':
                    self._last_obs = dict(geom['observation'])
                    return self._last_obs['result']
                minor_r = min(geom["aw"], geom["ah"]) / 2.0
                if minor_r < self.COG_MINOR_R_MIN_PX:
                    return self._uncertain("too small/thin to read rings", geom["cx"], geom["cy"])

                m, rout = self._norm_transform(geom)
                norm = cv2.warpAffine(
                    bgr,
                    m,
                    (2 * rout, 2 * rout),
                    flags=cv2.INTER_CUBIC,
                    borderMode=cv2.BORDER_CONSTANT,
                    borderValue=(128, 128, 128),
                )
                nhsv = cv2.cvtColor(norm, cv2.COLOR_BGR2HSV)
                start_ccx = float(rout)
                start_ccy = float(rout)
                ccx, ccy = self._refine_center_norm(nhsv, start_ccx, start_ccy, rout)
                center_shift = math.hypot(ccx - start_ccx, ccy - start_ccy)
                sym_score, sym_pairs, sym_med_diff = self._transition_symmetry_score(nhsv, ccx, ccy, rout)
                trans = self._radial_transitions(nhsv, ccx, ccy, rout)
                s, align_err = self._pitch_scale([t / float(self.COG_NORM_R0) for t in trans])
                # The chromatic outline may terminate BEFORE a black outer band.
                # Preserve measured transition pitch instead of declaring it five bands.
                pitch = 0.2 * s * self.COG_NORM_R0
                bands, confs = self._read_rings_norm(nhsv, ccx, ccy, pitch)
                if geom['mode']=='ellipse':
                    # A genuinely monochrome measured disc has no internal
                    # transitions from which to infer pitch. Its complete paint
                    # outline supplies scale; do not apply that premise to a
                    # multi-colour outline which may omit a black outer band.
                    uniform,uniform_confs=self._read_rings_norm(nhsv,ccx,ccy,.2*self.COG_NORM_R0)
                    if uniform and len(set(uniform))==1:
                        s=1.;pitch=.2*self.COG_NORM_R0
                        bands,confs=uniform,uniform_confs
                if bands is None:
                    return self._uncertain("empty ring read", geom["cx"], geom["cy"])

                read_conf = min(confs)
                pitch_conf = max(0.0, 1.0 - align_err / self.COG_PITCH_ALIGN_MAX)
                final = 0.50 * geom["score"] + 0.50 * read_conf
                total = sum(self.COLOR_VALUE[b] for b in bands)
                result = self.SUM_TO_TYPE.get(total, "FAKE")
                distinct = len(set(bands))

                self._last_obs = {
                    "bands": tuple(bands),
                    "confs": tuple(float(c) for c in confs),
                    "sum": int(total),
                    "result": result,
                    "cx": float(geom["cx"]),
                    "cy": float(geom["cy"]),
                    "r": float(geom["r"]),
                    "bbox": bbox,
                    "geom_score": float(geom["score"]),
                    "read_conf": float(read_conf),
                    "final": float(final),
                    "s": float(s),
                    "pitch_conf": float(pitch_conf),
                    "align_err": float(align_err),
                    "center_shift": float(center_shift),
                    "sym_score": float(sym_score),
                    "sym_pairs": int(sym_pairs),
                    "sym_med_diff": None if sym_med_diff is None else float(sym_med_diff),
                    "edge": bool(edge),
                    "mode": geom["mode"],
                    "accepted": False,
                    "reason": "decoded",
                }

                # A Hough circle inside one broad colour is not evidence of five
                # rings: it also occurs inside a cropped multi-colour target or
                # coloured arena surface. A one-colour target is observable only
                # through its measured chromatic outer outline (ellipse mode).
                if distinct == 1 and geom['mode'] != 'ellipse':
                    self._last_obs['reason'] = 'monochrome read without measured outer outline'
                    return None
                if geom['mode']=='hough' and distinct==2 and result!='FAKE':
                    check=self._native_ring_structure(bgr,geom,self._last_obs)
                    self._last_obs['new_structure_check']=check
                    if check['status']!='PASS':
                        self._last_obs['reason']='structure: '+check['reason']
                        return None
                if result != "FAKE" and distinct < self.COG_REPORT_MIN_COLORS:
                    self._last_obs["reason"] = "not enough distinct colors"
                    return None
                if read_conf < self.COG_BAND_CONF_MIN or final < self.COG_FINAL_CONF_MIN:
                    self._last_obs["reason"] = "low confidence"
                    return None
                if self.COG_SAFE_DECISION_ENABLED and result != "FAKE":
                    safe_min_read = self.COG_SAFE_EDGE_MIN_READ if edge else self.COG_SAFE_MIN_READ
                    safe_reasons = []
                    if read_conf < safe_min_read:
                        safe_reasons.append("read %.2f<%.2f" % (read_conf, safe_min_read))
                    if s < self.COG_SAFE_S_LO or s > self.COG_SAFE_S_HI:
                        safe_reasons.append("s %.2f outside %.2f-%.2f" % (s, self.COG_SAFE_S_LO, self.COG_SAFE_S_HI))
                    if safe_reasons:
                        self._last_obs["reason"] = ", ".join(safe_reasons)
                        return None

                if result == "FAKE":
                    # An invalid sum from an unproved Hough/ellipse scale is not
                    # physical negative evidence. Require the same original-pixel
                    # five-band and outer-boundary proof before rejecting identity.
                    check = self._native_ring_structure(bgr, geom, self._last_obs)
                    self._last_obs["new_structure_check"] = check
                    if check["status"] != "PASS":
                        self._last_obs["reason"] = "unproved negative: " + check["reason"]
                        return None
                self._last_obs["accepted"] = result != "FAKE"
                self._last_obs["reason"] = "accepted" if result != "FAKE" else "fake"
                return result

            def analyze_frame(self, frame, color_order="bgr", wall_distance_mm=None):
                """Analyze one camera frame and return victim/target perception output."""
                bgr = self._normalize_frame(frame, color_order=color_order)
                self._configure_for_frame(bgr)
                hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
                debug_lines = []

                victim_candidates = []
                for cand in self.find_letter_candidates(bgr, hsv=hsv):
                    try:
                        rect = cand["rect"]
                        (cx, cy), (rw, rh), _ = rect
                        box = cv2.boxPoints(rect)
                        x0 = max(0, int(np.floor(np.min(box[:, 0]))))
                        y0 = max(0, int(np.floor(np.min(box[:, 1]))))
                        x1 = min(self.CAM_W, int(np.ceil(np.max(box[:, 0]))))
                        y1 = min(self.CAM_H, int(np.ceil(np.max(box[:, 1]))))
                        binimg = self._letter_binary(cand["crop"])
                        label, cls_dbg = self.classify_with_debug(cand["crop"], binimg=binimg)
                        soft_label, soft_conf, soft_reason = self.classify_guess(cand["crop"], binimg=binimg)
                        # Pixel-stepped quad corners can win the right template
                        # but narrowly lose its margin test. Accept an alternate
                        # rectification only when BOTH warps rank the same glyph,
                        # the first passes the IoU floor and the second passes
                        # every normal threshold with twice the usual margin.
                        if label is None and cand['path']=='quad' and cls_dbg['iou']>=self.LETTER_IOU_MIN:
                            alt_crop=self._warp_square(bgr,rect)
                            alt_ok,alt_token=self._is_letter_token(alt_crop)
                            if alt_ok:
                                alt_label,alt_debug=self.classify_with_debug(alt_crop)
                                if alt_label==cls_dbg['label_guess'] and alt_debug['margin']>=2*self.LETTER_IOU_MARGIN:
                                    label,cls_dbg=alt_label,alt_debug
                                    soft_label,soft_conf,soft_reason=self.classify_guess(alt_crop)
                                    cand=dict(cand,crop=alt_crop,token_debug=alt_token,path='quad_rect_agreement')
                        # Pixel-stepped plaque corners can distort a quad homography.
                        # Retry only an unaccepted, independently S-like candidate;
                        # the alternative must pass the existing template thresholds.
                        if (label is None and cand['path'] == 'quad'
                                and cls_dbg['label_guess'] == 'S' and soft_label == 'S'
                                and soft_conf >= .85):
                            alt_crop = self._warp_square(bgr, rect)
                            alt_ok, alt_token = self._is_letter_token(alt_crop)
                            if alt_ok:
                                alt_bin = self._letter_binary(alt_crop)
                                alt_label, alt_debug = self.classify_with_debug(alt_crop, binimg=alt_bin)
                                alt_soft, alt_conf, alt_reason = self.classify_guess(alt_crop, binimg=alt_bin)
                                if alt_label == 'S' and alt_soft == 'S' and alt_conf >= .85:
                                    label, cls_dbg = alt_label, alt_debug
                                    soft_label, soft_conf, soft_reason = alt_soft, alt_conf, alt_reason
                                    cand = dict(cand, crop=alt_crop, token_debug=alt_token,
                                                path='quad_to_min_rect_S')
                        if (label is None and cand['path'] == 'quad'
                                and soft_label == 'H'
                                and soft_reason == 'holes=2' and soft_conf >= .85):
                            # Require two holes in both views; one-hole H guesses must
                            # not gain acceptance through this geometric fallback.
                            alt_crop = self._warp_square(bgr, rect)
                            alt_ok, alt_token = self._is_letter_token(alt_crop)
                            if alt_ok:
                                alt_bin = self._letter_binary(alt_crop)
                                alt_label, alt_debug = self.classify_with_debug(alt_crop, binimg=alt_bin)
                                alt_soft, alt_conf, alt_reason = self.classify_guess(alt_crop, binimg=alt_bin)
                                if (alt_label == 'H' and alt_soft == 'H'
                                        and alt_reason == 'holes=2' and alt_conf >= .85):
                                    label, cls_dbg = alt_label, alt_debug
                                    soft_label, soft_conf, soft_reason = alt_soft, alt_conf, alt_reason
                                    cand = dict(cand, crop=alt_crop, token_debug=alt_token,
                                                path='quad_to_min_rect_H')
                        # A fixed inset can cut the ends off a tightly framed symbol.
                        # Keep the entire crop by adding a small neutral white margin.
                        # Both margin sizes must independently pass the unchanged
                        # classifier and agree with the original shape hypothesis.
                        if (label is None and cand['path'] == 'quad'
                                and cls_dbg['label_guess'] in ('S', 'H')
                                and soft_label == cls_dbg['label_guess'] and soft_conf >= .85
                                and (soft_label == 'S' or soft_reason.startswith('holes=1 '))):
                            alternatives = []
                            for padding in (3, 4):
                                padded = cv2.copyMakeBorder(cand['crop'], padding, padding, padding, padding,
                                                           cv2.BORDER_CONSTANT, value=(220, 220, 220))
                                alt_bin = self._letter_binary(padded)
                                alt_label, alt_debug = self.classify_with_debug(padded, binimg=alt_bin)
                                alt_soft, alt_conf, alt_reason = self.classify_guess(padded, binimg=alt_bin)
                                shape_agrees=alt_soft==soft_label and alt_conf>=.85
                                # A one-pixel resampling change can make the
                                # secondary defect counter inconclusive. Keep
                                # two positive template votes and require the
                                # original plus another padded shape to agree;
                                # a different shape label still vetoes the read.
                                shape_unresolved=(soft_label=='S' and alt_soft is None
                                    and alt_debug['iou']>=self.LETTER_IOU_MIN+self.LETTER_IOU_MARGIN
                                    and alt_debug['margin']>=2*self.LETTER_IOU_MARGIN)
                                if (alt_label != soft_label or not(shape_agrees or shape_unresolved)
                                        or alt_debug['margin'] < (.06 if soft_label == 'S' else .07)
                                        or (soft_label == 'H' and alt_debug['iou'] < .52)
                                        or (soft_label == 'H' and not alt_reason.startswith('holes=1 '))):
                                    break
                                alternatives.append((padded,alt_debug,alt_conf if shape_agrees else soft_conf,
                                                     alt_reason if shape_agrees else soft_reason,shape_agrees))
                            if len(alternatives) == 2 and any(a[4]for a in alternatives):
                                padded, cls_dbg, soft_conf, soft_reason, _ = alternatives[-1]
                                label = soft_label
                                cand = dict(cand, crop=padded, path='quad_preserve_symbol_' + label)
                        centered = abs(cx - self.CAM_W / 2.0) <= self.CENTER_TOL_FRAC * self.CAM_W
                        item = {
                            "kind": "victim",
                            "label": label,
                            "label_guess": cls_dbg["label_guess"],
                            "confidence": float(cls_dbg["iou"]),
                            "accepted": label is not None,
                            "reportable": bool(label is not None and centered),
                            "centered": bool(centered),
                            "center": (float(cx), float(cy)),
                            "bbox": (x0, y0, max(0, x1 - x0), max(0, y1 - y0)),
                            "size": (float(rw), float(rh)),
                            "template": cls_dbg,
                            "soft_guess": {"label": soft_label, "confidence": float(soft_conf), "reason": soft_reason},
                            "token": cand["token_debug"],
                            "path": cand["path"],
                        }
                        victim_candidates.append(item)
                        debug_lines.append(
                            "[victim] cx=%.0f cy=%.0f centered=%s label=%s iou=%.2f margin=%.2f"
                            % (cx, cy, centered, label, cls_dbg["iou"], cls_dbg["margin"])
                        )
                    except Exception as exc:
                        import traceback
                        _tb = traceback.extract_tb(exc.__traceback__)
                        _where = " <- ".join("%s:%d" % (f.name, f.lineno) for f in _tb[-3:])
                        debug_lines.append("[victim] skipped candidate: %s | at %s" % (exc, _where))
                        continue

                target_candidates = []
                for raw in self.find_target_candidates(bgr, hsv=hsv):
                    try:
                        guard_ok, guard_reason = self._target_wall_guard(raw, wall_distance_mm)
                        cx, cy, r = raw[:3]
                        edge = raw[3] if len(raw) > 3 else False
                        bbox = raw[4] if len(raw) > 4 else None
                        partial = raw[5] if len(raw) > 5 else False
                        if not guard_ok:
                            target_candidates.append(
                                {
                                    "kind": "target",
                                    "label": None,
                                    "accepted": False,
                                    "reportable": False,
                                    "centered": False,
                                    "center": (float(cx), float(cy)),
                                    "bbox": bbox,
                                    "radius": float(r),
                                    "edge": bool(edge),
                                    "partial": bool(partial),
                                    "reason": guard_reason,
                                }
                            )
                            continue
                        label = self.read(bgr, cx, cy, r, edge, bbox)
                        obs = dict(self._last_obs or {})
                        ncolors, colorset, black_frac = self._target_structure(hsv, cx, cy, r)
                        centered = abs(cx - self.CAM_W / 2.0) <= (self.TARGET_EDGE_CENTER_TOL_FRAC if edge and partial else self.TARGET_CENTER_TOL_FRAC) * self.CAM_W
                        accepted = label is not None and label != "FAKE"
                        item = {
                            "kind": "target",
                            "label": label if accepted else None,
                            "label_guess": obs.get("result"),
                            "confidence": float(obs.get("final", 0.0)),
                            "accepted": bool(accepted),
                            "reportable": bool(accepted and centered),
                            "centered": bool(centered),
                            "center": (float(cx), float(cy)),
                            "bbox": bbox,
                            "radius": float(r),
                            "edge": bool(edge),
                            "partial": bool(partial),
                            "bands": list(obs.get("bands", ())),
                            "sum": obs.get("sum"),
                            "read_conf": float(obs.get("read_conf", 0.0)),
                            "geom_score": float(obs.get("geom_score", 0.0)),
                            "pitch_scale": float(obs.get("s", 0.0)),
                            "reason": obs.get("reason", "no read"),
                            "observation": obs,
                            "structure": {"ncolors": int(ncolors), "colors": colorset, "black_frac": float(black_frac)},
                        }
                        target_candidates.append(item)
                        debug_lines.append(
                            "[target] cx=%.0f cy=%.0f r=%.1f centered=%s label=%s bands=%s conf=%.2f reason=%s"
                            % (cx, cy, r, centered, item["label"], item["bands"], item["confidence"], item["reason"])
                        )
                    except Exception as exc:
                        import traceback
                        _tb = traceback.extract_tb(exc.__traceback__)
                        _where = " <- ".join("%s:%d" % (f.name, f.lineno) for f in _tb[-3:])
                        debug_lines.append("[target] skipped candidate: %s | at %s" % (exc, _where))
                        continue

                accepted_victims = [c for c in victim_candidates if c["accepted"]]
                accepted_targets = [c for c in target_candidates if c["accepted"]]
                victim_best, victim_d = self._nearest_to_center(accepted_victims, lambda c: c["center"][0])
                target_best, target_d = self._nearest_to_center(accepted_targets, lambda c: c["center"][0])

                best = None
                if victim_best is not None and (target_best is None or victim_d <= target_d):
                    best = victim_best
                elif target_best is not None:
                    best = target_best

                if best is None:
                    result = {
                        "detected": False,
                        "kind": None,
                        "label": None,
                        "confidence": 0.0,
                        "reportable": False,
                        "center": None,
                        "bbox": None,
                    }
                else:
                    result = {
                        "detected": True,
                        "kind": best["kind"],
                        "label": best["label"],
                        "confidence": float(best["confidence"]),
                        "reportable": bool(best["reportable"]),
                        "centered": bool(best["centered"]),
                        "center": best["center"],
                        "bbox": best.get("bbox"),
                    }

                result.update(
                    {
                        "frame_size": (int(self.CAM_W), int(self.CAM_H)),
                        "best": best,
                        "victim": {
                            "count": len(victim_candidates),
                            "accepted_count": len(accepted_victims),
                            "candidates": victim_candidates,
                        },
                        "target": {
                            "count": len(target_candidates),
                            "accepted_count": len(accepted_targets),
                            "candidates": target_candidates,
                        },
                        "debug_lines": debug_lines,
                    }
                )
                return result

            def check_fake_victim(
                self,
                token_cx_px,
                lidar_ranges,
                lidar_fov_rad,
                camera_fov_rad,
                camera_width_px=None,
                token_width_px=None,
                lidar_window_deg=LIDAR_WINDOW_DEG,
                protrusion_threshold_m=PROTRUSION_THRESHOLD_M,
                lidar_angle_sign=LIDAR_ANGLE_SIGN,
            ):
                """Fake-victim check using side LiDAR around and beside the token.

        A fake plate can be flat across the tiny centre window, so the decision
        compares the token centre against wall/reference shoulders beside it.
        """
                if camera_width_px is None:
                    camera_width_px = self.CAM_W
                ranges = list(lidar_ranges or [])
                n = len(ranges)
                if n <= 1:
                    return {
                        "is_real": None,
                        "protrusion_m": None,
                        "samples": 0,
                        "bearing_rad": None,
                        "reason": "empty lidar range image",
                    }

                focal = (camera_width_px / 2.0) / math.tan(camera_fov_rad / 2.0)
                dx = float(token_cx_px) - camera_width_px / 2.0
                bearing = math.atan2(dx, focal)
                half_cols = max(2.0, (math.radians(lidar_window_deg) / max(camera_fov_rad, 1e-6)) * n)
                token_idx = float(token_cx_px) * (n - 1) / max(float(camera_width_px - 1), 1.0)
                if token_width_px is None:
                    token_width_px = max(10.0, 0.18 * float(camera_width_px))
                token_width_idx = max(4.0, float(token_width_px) * n / max(float(camera_width_px), 1.0))
                center_half = max(half_cols, 0.35 * token_width_idx)
                token_half = max(center_half, 0.60 * token_width_idx)
                gap = max(1.0, 0.20 * token_width_idx)
                shoulder_w = max(3.0, center_half, 0.80 * token_width_idx)

                clean = []
                for dist in ranges:
                    try:
                        d = float(dist)
                    except Exception:
                        d = float("inf")
                    if d == float("inf") or math.isnan(d) or d <= 0:
                        clean.append(float("inf"))
                    else:
                        clean.append(d)

                def window_values(a, b):
                    lo = max(0, int(math.floor(a)))
                    hi = min(n - 1, int(math.ceil(b)))
                    vals = []
                    for i in range(lo, hi + 1):
                        d = clean[i]
                        if math.isfinite(d) and d > 0.0:
                            vals.append(d)
                    return vals

                centre_vals = window_values(token_idx - center_half, token_idx + center_half)
                left_vals = window_values(
                    token_idx - token_half - gap - shoulder_w,
                    token_idx - token_half - gap,
                )
                right_vals = window_values(
                    token_idx + token_half + gap,
                    token_idx + token_half + gap + shoulder_w,
                )

                if len(centre_vals) < 3:
                    return {
                        "is_real": None,
                        "protrusion_m": None,
                        "samples": len(centre_vals),
                        "bearing_rad": float(bearing),
                        "reason": "not enough centre lidar samples",
                    }
                if len(left_vals) < 2 or len(right_vals) < 2:
                    return {
                        "is_real": None,
                        "protrusion_m": None,
                        "samples": len(centre_vals),
                        "side_samples": [len(left_vals), len(right_vals)],
                        "bearing_rad": float(bearing),
                        "center_m": float(np.median(centre_vals)),
                        "reason": "not enough side wall samples",
                    }

                center_m = float(np.median(centre_vals))
                left_m = float(np.median(left_vals))
                right_m = float(np.median(right_vals))
                wall = max(left_m, right_m)
                near = center_m
                protrusion = wall - near
                side_balance = abs(left_m - right_m)
                side_reference_strong = (
                    len(left_vals) >= self.PROTRUSION_MIN_SIDE_SAMPLES
                    and len(right_vals) >= self.PROTRUSION_MIN_SIDE_SAMPLES
                    and side_balance <= max(0.025, 4.0 * protrusion_threshold_m)
                )

                body_lo = max(0, int(math.floor(token_idx - token_half)))
                body_hi = min(n - 1, int(math.ceil(token_idx + token_half)))
                close_run = 0
                max_close_run = 0
                close_count = 0
                body_count = 0
                for i in range(body_lo, body_hi + 1):
                    d = clean[i]
                    if not (math.isfinite(d) and d > 0.0):
                        close_run = 0
                        continue
                    body_count += 1
                    if wall - d >= protrusion_threshold_m:
                        close_count += 1
                        close_run += 1
                        max_close_run = max(max_close_run, close_run)
                    else:
                        close_run = 0
                close_frac = close_count / float(max(body_count, 1))
                wide_close_surface = (
                    max_close_run >= max(5, int(round(0.45 * token_width_idx)))
                    or close_frac >= 0.55
                )

                if protrusion >= self.PROTRUSION_HARD_FAKE_M:
                    is_real = False
                    reason = "hard protrusion/fake"
                elif protrusion >= protrusion_threshold_m and side_reference_strong and wide_close_surface:
                    is_real = False
                    reason = "wide protruding/fake"
                elif protrusion >= protrusion_threshold_m and not side_reference_strong:
                    is_real = True
                    reason = "weak side reference; small protrusion accepted"
                elif protrusion >= protrusion_threshold_m:
                    is_real = None
                    reason = "small protrusion without wide fake surface"
                elif protrusion <= -protrusion_threshold_m:
                    is_real = None
                    reason = "centre farther than side reference"
                else:
                    is_real = True
                    reason = "flush/real"

                return {
                    "is_real": is_real if is_real is None else bool(is_real),
                    "protrusion_m": float(protrusion),
                    "samples": len(centre_vals),
                    "side_samples": [len(left_vals), len(right_vals)],
                    "bearing_rad": float(bearing),
                    "center_m": center_m,
                    "wall_median_m": wall,
                    "left_median_m": left_m,
                    "right_median_m": right_m,
                    "nearest_m": near,
                    "side_reference_strong": bool(side_reference_strong),
                    "wide_close_surface": bool(wide_close_surface),
                    "close_run": int(max_close_run),
                    "close_frac": float(close_frac),
                    "threshold_m": float(protrusion_threshold_m),
                    "reason": reason,
                }

            def _primary_analyze_target_victim_frame(
                self,
                frame_bgr,
                side_lidar_mm=None,
                enable_fake_victim_check=False,
                lidar_ranges=None,
                lidar_fov_rad=None,
                camera_fov_rad=None,
                camera_width_px=None,
                lidar_angle_sign=LIDAR_ANGLE_SIGN,
            ):
                """Main entry point: analyze a BGR frame for victims/targets.

        Returns a dict with detected, kind, label, name, confidence, reportable,
        center, report, victim_debug, target_debug, fake_victim, log_lines and
        raw_result.
        """
                # Only pixel decoding is reusable. Pose/range, fake checks and
                # temporal confirmation below/outside this method stay live.
                # Two LRU entries cover the two cameras without growing memory.
                pixels=np.asarray(frame_bgr)
                key=(pixels.shape,pixels.dtype.str,pixels.tobytes(),side_lidar_mm)
                cache=getattr(self,'_pixel_analysis_cache',None)
                if cache is None:cache=self._pixel_analysis_cache={}
                saved=cache.pop(key,None)
                if saved is None:
                    raw=self.analyze_frame(frame_bgr,color_order='bgr',wall_distance_mm=side_lidar_mm)
                    saved=(RescueVision._clone_payload(raw),RescueVision._clone_payload(getattr(self,'_partial_geometry_debug',None)))
                else:
                    self._configure_for_frame(frame_bgr)
                    raw=RescueVision._clone_payload(saved[0])
                    self._partial_geometry_debug=RescueVision._clone_payload(saved[1])
                cache[key]=saved
                if len(cache)>2:cache.pop(next(iter(cache)))
                kind = raw.get("kind")
                label = raw.get("label")
                best = raw.get("best")
                name = self._label_name(kind, label)

                fake_victim = {
                    "enabled": bool(kind == "victim" and best is not None),
                    "is_real": None,
                    "protrusion_m": None,
                    "samples": 0,
                    "reason": "fake check disabled or not a victim",
                }
                if kind == "victim" and best is not None:
                    bbox = best.get("bbox")
                    fake_victim["bbox"] = bbox
                    if bbox:
                        _, _, bw, bh = [float(v) for v in bbox]
                        fh, fw = np.asarray(frame_bgr).shape[:2]
                        fake_victim["bbox_area_frac"] = float(
                            max(0.0, bw) * max(0.0, bh) / float(max(fw * fh, 1))
                        )
                    fake_victim["reason"] = "geometry check pending"
                if enable_fake_victim_check and kind == "victim" and best is not None:
                    missing = []
                    if lidar_ranges is None:
                        missing.append("lidar_ranges")
                    if camera_fov_rad is None:
                        missing.append("camera_fov_rad")
                    if missing:
                        fake_victim["reason"] = "missing input(s): " + ", ".join(missing)
                    else:
                        if camera_width_px is None:
                            camera_width_px = np.asarray(frame_bgr).shape[1]
                        lidar_fake = self.check_fake_victim(
                            token_cx_px=best["center"][0],
                            lidar_ranges=lidar_ranges,
                            lidar_fov_rad=lidar_fov_rad,
                            camera_fov_rad=camera_fov_rad,
                            camera_width_px=camera_width_px,
                            token_width_px=max(best.get("size") or (0.0, 0.0)),
                            lidar_angle_sign=lidar_angle_sign,
                        )
                        fake_victim["lidar_check"] = lidar_fake
                        if fake_victim.get("is_real") is not False and lidar_fake.get("is_real") is False:
                            fake_victim.update(lidar_fake)
                elif enable_fake_victim_check:
                    fake_victim["reason"] = "enabled, but current result is not a victim"

                # A target whose rings decoded confidently but whose colour-sum matches
                # no real hazmat type is read as "FAKE" (reason=="fake"). It is never a
                # reportable detection, but surface it so it doesn't vanish as "nothing
                # detected". Only used when there's no real victim/target to report.
                fake_target = None
                for c in (raw.get("target") or {}).get("candidates", []):
                    if c.get("reason") == "fake":
                        fake_target = self._clean_center(c.get("center"))
                        break

                report = self._make_report(
                    kind, label, raw.get("reportable", False),
                    fake_victim=fake_victim, fake_target=fake_target,
                )
                victim_debug = self._extract_victim_debug(best)
                target_debug = self._extract_target_debug(best)
                log_lines = self._build_log_lines(raw, report, victim_debug, target_debug, fake_victim)

                return {
                    "detected": bool(raw.get("detected")),
                    "kind": kind,
                    "label": label,
                    "name": name,
                    "confidence": self._round_float(raw.get("confidence")),
                    "reportable": bool(report.get("should_report")),
                    "center": self._clean_center(raw.get("center")),
                    "report": report,
                    "victim_debug": victim_debug,
                    "target_debug": target_debug,
                    "fake_victim": fake_victim,
                    "log_lines": log_lines,
                    "raw_result": raw,
                }

            # Additional geometry for unresolved targets.

            def _secondary_projective_ring_geometry(self, bgr, hsv=None):
                self._partial_geometry_debug = None
                original = self._primary_projective_ring_geometry(bgr, hsv=hsv)
                if original is not None:
                    return original
                return self._partial_ring_geometry(bgr, hsv=hsv)

            def _ring_fit_solver(self):
                solver = getattr(self, '_partial_fit_solver', None)
                if solver is None:
                    from scipy.optimize import least_squares
                    solver = self._partial_fit_solver = least_squares
                return solver

            def _secondary_find_target_candidates(self, bgr, hsv=None):
                candidates = self._primary_find_target_candidates(bgr, hsv=hsv)
                refined = []
                for candidate in candidates:
                    cx, cy, radius, edge, bbox = candidate[:5]
                    geometry = self._geom_for(cx, cy)
                    if edge and geometry and geometry['mode'] == 'hough':
                        # This read is cached for the normal analysis below. Only a
                        # failed native-pixel check can enter the additional fit.
                        self.read(bgr, cx, cy, radius, edge, bbox)
                        observation = self._last_obs or {}
                        check = observation.get('new_structure_check') or {}
                        if check.get('status') == 'REJECTED':
                            recovered = self._clipped_boundary_geometry(bgr, observation, check)
                            if recovered is not None:
                                cx, cy = recovered['cx'], recovered['cy']
                                self._geom_cache[(int(round(cx)), int(round(cy)))] = {
                                    'mode': 'projective-rings', 'observation': recovered}
                                candidate = (cx, cy, recovered['r'], recovered['edge'],
                                             recovered['bbox'], recovered['edge'])
                    refined.append(candidate)
                return refined

            def _clipped_boundary_geometry(self, bgr, observation, native_check):
                """Refine a clipped proposal using four independently visible boundaries.

        A merged or cropped centre need not provide a closed ellipse. The
        decoded colour sequence proposes boundary identities, never a target
        ID. Every identity must be unambiguous, fit held-out edges and retain
        its colour in several visible directions before it can be recovered.
        """
                names = ('red', 'green', 'blue', 'yellow', 'black')
                colours = observation.get('bands') or ()
                if (len(colours) != 5 or len(set(colours)) < 3
                        or any(c not in names for c in colours)
                        or 'sampling_center_px' not in native_check):
                    return None
                bands = [names.index(c) for c in colours]
                transitions = {}
                for level in range(1, 6):
                    if level < 5 and bands[level-1] == bands[level]:
                        continue
                    pair = tuple(sorted((bands[level-1], bands[level] if level < 5 else 5)))
                    if pair in transitions:
                        # Repeated edge colours can imply different ring numbers.
                        # This fallback does not resolve that ambiguity by colour sum.
                        return None
                    transitions[pair] = level
                if len(transitions) < 4:
                    return None
                hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
                height, width = hsv.shape[:2]
                hue, sat, val = cv2.split(hsv)
                labels = np.full((height, width), 5, np.uint8)
                for colour, mask in enumerate(((hue < 10) | (hue >= 170),
                                               (hue >= 40) & (hue < 85),
                                               (hue >= 100) & (hue < 130),
                                               (hue >= 20) & (hue < 35))):
                    labels[mask & (sat >= self.GRAY_S_MAX)] = colour
                labels[val < self.BLACK_V_MAX] = 4
                center = np.asarray(native_check['sampling_center_px'], dtype=float)
                pitch = float(native_check['sampling_pitch_px'])
                if not np.all(np.isfinite(center)) or not math.isfinite(pitch) or pitch < 2:
                    return None
                boundaries, pairs = [], []
                for dy, dx in ((0, 1), (1, 0)):
                    first, second = labels[:height-dy, :width-dx], labels[dy:, dx:]
                    y, x = np.nonzero(first != second)
                    boundaries.append(np.column_stack((x+dx/2, y+dy/2)))
                    pairs.append(np.sort(np.column_stack((first[y, x], second[y, x])), axis=1))
                boundaries, pairs = np.concatenate(boundaries), np.concatenate(pairs)
                points = (boundaries-center)/pitch
                initial_radius = np.linalg.norm(points, axis=1)
                levels = np.zeros(len(points), dtype=int)
                for pair, level in transitions.items():
                    levels[np.all(pairs == pair, axis=1) & (np.abs(initial_radius-level) < 1.)] = level
                keep = levels > 0
                points, target = points[keep], levels[keep]
                distinct, counts = np.unique(target, return_counts=True)
                if len(distinct) < 4 or 5 not in distinct or min(counts) < 8:
                    return None
                # Fix the split before fitting; validation outliers cannot be dropped
                # after seeing the model's residuals.
                validation = np.arange(len(points)) % 4 == 0
                support = [{'level': int(k), 'train': int(np.count_nonzero((target == k) & ~validation)),
                            'validation': int(np.count_nonzero((target == k) & validation))}
                           for k in distinct]
                if any(row['train'] < 5 or row['validation'] < 2 for row in support):
                    return None

                def radii(parameters, xy):
                    x, y = (xy-parameters[:2]).T
                    squared = parameters[2]*x*x+2*parameters[3]*x*y+parameters[4]*y*y
                    return np.sqrt(np.maximum(1e-9, squared))/np.maximum(
                        .2, 1+parameters[5]*x+parameters[6]*y)

                fit = self._ring_fit_solver()(
                    lambda p: radii(p, points[~validation])-target[~validation],
                    np.array([0., 0., 1., 0., 1., 0., 0.]),
                    bounds=([-1., -1., .45, -.6, .45, -.15, -.15],
                            [1., 1., 2., .6, 2., .15, .15]),
                    loss='soft_l1', f_scale=.1, max_nfev=45)
                if not fit.success:
                    return None
                metric = np.array([[fit.x[2], fit.x[3]], [fit.x[3], fit.x[4]]])
                if np.min(np.linalg.eigvalsh(metric)) < .4 or np.linalg.norm(fit.x[5:]) > .15:
                    return None
                residual = np.abs(radii(fit.x, points)-target)
                fit_error = float(np.quantile(residual[~validation], .9))
                validation_error = float(np.quantile(residual[validation], .9))
                if fit_error > .18 or validation_error > .18:
                    return None
                for row in support:
                    level = target == row['level']
                    row['fit_error'] = float(np.quantile(residual[level & ~validation], .9))
                    row['validation_error'] = float(np.quantile(residual[level & validation], .9))
                    if row['fit_error'] > .18 or row['validation_error'] > .18:
                        return None
                yy, xx = np.indices((height, width))
                xy = (np.column_stack((xx.ravel(), yy.ravel()))-center)/pitch
                radius = radii(fit.x, xy).reshape(height, width)
                direction = np.arctan2(xy[:, 1]-fit.x[1], xy[:, 0]-fit.x[0]).reshape(height, width)
                sectors = ((direction+math.pi)*8/(2*math.pi)).astype(int) % 8
                checks = []
                for k, expected in enumerate(bands):
                    mask = (radius > k+.25) & (radius < k+.75)
                    values = labels[mask]
                    confidence = float(np.count_nonzero(values == expected)/max(1, len(values)))
                    votes = []
                    for sector in range(8):
                        part = labels[mask & (sectors == sector) & (labels != 5)]
                        if len(part) >= 2:
                            votes.append(int(np.argmax(np.bincount(part, minlength=5))))
                    agreement = votes.count(expected)/max(1, len(votes))
                    if len(values) < 8 or confidence < .80 or len(votes) < 4 or agreement < .85:
                        return None
                    checks.append({'band': k+1, 'color': names[expected], 'pixels': len(values),
                                   'confidence': confidence, 'sectors': len(votes),
                                   'sector_agreement': agreement})
                outer = points[target == 5]-fit.x[:2]
                outer_sectors = ((np.arctan2(outer[:, 1], outer[:, 0])+math.pi)*8/(2*math.pi)).astype(int) % 8
                if len(np.unique(outer_sectors)) < 3:
                    return None
                fitted_center = center+pitch*fit.x[:2]
                if not (0 < fitted_center[0] < width and 0 < fitted_center[1] < height):
                    return None
                inside = radius <= 5
                y, x = np.nonzero(inside)
                bbox = (int(x.min()), int(y.min()), int(np.ptp(x)+1), int(np.ptp(y)+1))
                total = sum(self.COLOR_VALUE[c] for c in colours)
                label = self.SUM_TO_TYPE.get(total, 'FAKE')
                diagnostic = {'status': 'PASS', 'reason': 'clipped rings validated from visible boundaries',
                              'fit_error': fit_error, 'validation_error': validation_error,
                              'boundary_samples': support, 'outer_sectors': len(np.unique(outer_sectors)),
                              'bands': checks, 'parameters': fit.x.tolist(),
                              'initial_native_reason': native_check.get('reason')}
                return {'bands': tuple(colours), 'confs': tuple(row['confidence'] for row in checks),
                        'sum': total, 'result': label, 'cx': float(fitted_center[0]),
                        'cy': float(fitted_center[1]), 'bbox': bbox, 'r': float(max(bbox[2:])/2),
                        'edge': bool(any(np.any(edge) for edge in
                                         (inside[0], inside[-1], inside[:, 0], inside[:, -1]))),
                        'geom_score': max(0., 1-validation_error),
                        'read_conf': min(row['confidence'] for row in checks),
                        'final': min(row['confidence'] for row in checks), 's': 1.,
                        'mode': 'projective-rings', 'geometry_source': 'clipped-visible-boundaries',
                        'new_structure_check': diagnostic, 'accepted': label != 'FAKE',
                        'reason': 'accepted' if label != 'FAKE' else 'fake'}

            def _small_core_seed(self, labels, transform, core_a, core_b, radii):
                """Initialize a full target from closed boundaries, not guessed colours.

        A small disc can measure the centre accurately while its pixel-stepped
        outline gives a poor initial perspective scale. Several complete ring
        outlines constrain that scale before any band label is selected.
        """
                height, width = labels.shape
                records = []
                angles = np.linspace(0, 2*math.pi, 32, endpoint=False)
                for color in range(5):
                    contours, hierarchy = cv2.findContours(
                        (labels == color).astype(np.uint8), cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
                    for i, contour in enumerate(contours):
                        points = contour[:, 0]
                        if (len(points) < 12 or np.any(points[:, 0] <= 0)
                                or np.any(points[:, 0] >= width-1) or np.any(points[:, 1] <= 0)
                                or np.any(points[:, 1] >= height-1)):
                            continue
                        (cx, cy), (a, b), angle = cv2.fitEllipse(contour)
                        if min(a, b) < 5 or min(a, b)/max(a, b) < .45:
                            continue
                        theta = math.radians(angle)
                        ca, sa = math.cos(theta), math.sin(theta)
                        delta = points-(cx, cy)
                        u = delta[:, 0]*ca+delta[:, 1]*sa
                        v = -delta[:, 0]*sa+delta[:, 1]*ca
                        error = np.quantile(np.abs(np.sqrt((2*u/a)**2+(2*v/b)**2)-1), .9)
                        if error > .12:
                            continue
                        depth, parent = 0, hierarchy[0, i, 3]
                        while parent >= 0:
                            depth += 1
                            parent = hierarchy[0, parent, 3]
                        a += -1 if depth % 2 else 1
                        b += -1 if depth % 2 else 1
                        ratio = math.sqrt(a*b/(core_a*core_b))
                        level = int(round(ratio))
                        if not (.8 < ratio < 5.6 and 1 <= level <= 5):
                            continue
                        center = transform(np.array([[cx, cy]]))[0]
                        if np.linalg.norm(center) > max(1., .5*ratio):
                            continue
                        u, v = a/2*np.cos(angles), b/2*np.sin(angles)
                        boundary = np.column_stack((cx+ca*u-sa*v, cy+sa*u+ca*v))
                        records.append((transform(boundary), level))
                levels = {level for _, level in records}
                if len(levels) < 4 or 1 not in levels or 5 not in levels:
                    return None
                points = np.concatenate([points for points, _ in records])
                target = np.concatenate([np.full(len(points), level) for points, level in records])
                fit = self._ring_fit_solver()(
                    lambda p: radii(p, points)-target, np.array([0., 0., 1., 0., 1., 0., 0.]),
                    bounds=([-.3, -.3, .45, -.6, .45, -.15, -.15],
                            [.3, .3, 2., .6, 2., .15, .15]),
                    loss='soft_l1', f_scale=.1, max_nfev=45)
                if not fit.success or np.quantile(np.abs(radii(fit.x, points)-target), .9) > .18:
                    return None
                return fit.x

            def _partial_ring_geometry(self, bgr, hsv=None):
                """Fit visible boundaries, including two merged central colour bands.

        Whole ellipses are unreliable when the camera crops their outlines.
        Pixel-boundary midpoints retain the visible arcs without completing
        invisible arcs or treating the image border as a painted boundary.
        The label/sum is computed only after geometric and colour validation.
        """
                self._partial_geometry_debug = None
                if hsv is None:
                    hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
                height, width = hsv.shape[:2]
                names = ('red', 'green', 'blue', 'yellow', 'black')
                hue, sat, val = cv2.split(hsv)
                labels = np.full((height, width), 5, np.uint8)
                for color, mask in enumerate(((hue < 10) | (hue >= 170),
                                               (hue >= 40) & (hue < 85),
                                               (hue >= 100) & (hue < 130),
                                               (hue >= 20) & (hue < 35))):
                    labels[mask & (sat >= self.GRAY_S_MAX)] = color
                labels[val < self.BLACK_V_MAX] = 4
                cores = []
                for color in range(5):
                    contours, hierarchy = cv2.findContours(
                        (labels == color).astype(np.uint8), cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
                    for i, contour in enumerate(contours):
                        x, y, w, h = cv2.boundingRect(contour)
                        if (hierarchy[0, i, 3] >= 0 or hierarchy[0, i, 2] >= 0
                                or min(x, y) <= 0 or x+w >= width or y+h >= height
                                or len(contour) < 12 or cv2.contourArea(contour) < 12):
                            continue
                        (cx, cy), (a, b), angle = cv2.fitEllipse(contour)
                        if min(a, b) < 5 or min(a, b)/max(a, b) < .45:
                            continue
                        theta = math.radians(angle)
                        ca, sa = math.cos(theta), math.sin(theta)
                        delta = contour[:, 0] - (cx, cy)
                        u = delta[:, 0]*ca + delta[:, 1]*sa
                        v = -delta[:, 0]*sa + delta[:, 1]*ca
                        error = np.quantile(np.abs(np.sqrt((2*u/a)**2 + (2*v/b)**2)-1), .9)
                        if error <= .16:
                            # A closed, fully visible core supplies a measured scale,
                            # including the half-pixel boundary on each side.
                            cores.append((a*b, cx, cy, a+1, b+1, angle, color))
                if not cores:
                    return None

                boundaries, pairs = [], []
                for dy, dx in ((0, 1), (1, 0)):
                    first = labels[:height-dy, :width-dx]
                    second = labels[dy:, dx:]
                    # Background is not a ring colour, but a real colour/background
                    # transition can prove that a fully visible outer ring terminates.
                    y, x = np.nonzero(first != second)
                    boundaries.append(np.column_stack((x+dx/2, y+dy/2)))
                    pairs.append(np.sort(np.column_stack((first[y, x], second[y, x])), axis=1))
                boundaries = np.concatenate(boundaries)
                pairs = np.concatenate(pairs)
                if len(boundaries) < 40:
                    return None
                yy, xx = np.indices((height, width))
                image_points = np.column_stack((xx.ravel(), yy.ravel()))

                def radii(parameters, points):
                    x, y = (points-parameters[:2]).T
                    squared = parameters[2]*x*x + 2*parameters[3]*x*y + parameters[4]*y*y
                    return np.sqrt(np.maximum(1e-9, squared)) / np.maximum(
                        .2, 1+parameters[5]*x+parameters[6]*y)

                def read_bands(radius):
                    bands = []
                    for k in range(5):
                        pixels = labels[(radius > k+.25) & (radius < k+.75) & (labels != 5)]
                        if len(pixels) < 8:
                            return None
                        bands.append(int(np.argmax(np.bincount(pixels, minlength=5))))
                    return bands

                # Preserve every single-band attempt before trying a merged centre.
                # A double-band core is accepted only with four visible boundaries;
                # the invisible boundary between equal colours is never invented.
                ordered_cores = sorted(cores)[:4]
                trials = [(core, count) for count in (1, 2) for core in ordered_cores]
                merged_solutions = []
                for (_, cx, cy, measured_a, measured_b, angle, color), core_rings in trials:
                    a, b = measured_a/core_rings, measured_b/core_rings
                    theta = math.radians(angle)
                    ca, sa = math.cos(theta), math.sin(theta)

                    def transform(points):
                        delta = points-(cx, cy)
                        return np.column_stack(((delta[:, 0]*ca+delta[:, 1]*sa)*2/a,
                                                (-delta[:, 0]*sa+delta[:, 1]*ca)*2/b))

                    xy, edge_points = transform(image_points), transform(boundaries)
                    params = np.array([0., 0., 1., 0., 1., 0., 0.])
                    radius = radii(params, xy).reshape(height, width)
                    clipped = any(np.any(edge < 5) for edge in
                                  (radius[0], radius[-1], radius[:, 0], radius[:, -1]))
                    if core_rings == 1 and not clipped and a >= 8:
                        continue
                    if core_rings == 1 and not clipped:
                        params = self._small_core_seed(labels, transform, a, b, radii)
                        if params is None:
                            continue
                        radius = radii(params, xy).reshape(height, width)
                    bands = read_bands(radius)
                    if bands is None or bands[0] != color or len(set(bands)) < 3:
                        continue
                    if core_rings == 2 and (bands[:2] != [color, color] or bands[2] == color):
                        continue
                    description = "partial target" if clipped else "small-core target"
                    diagnostic = {"status": "WAITING", "reason": description + ", geometry uncertain",
                                  "core_center": [float(cx), float(cy)],
                                  "core_short_diameter_px": float(min(measured_a, measured_b)),
                                  "clipped": bool(clipped),
                                  "initial_bands": [names[i] for i in bands]}
                    if core_rings == 1:
                        self._partial_geometry_debug = diagnostic
                    else:
                        diagnostic.update(core_rings=core_rings, band_short_diameter_px=float(min(a, b)))
                    solver = self._ring_fit_solver()
                    selected = None
                    for iteration in range(4):
                        radial = radii(params, edge_points)
                        errors = []
                        for k in range(1, 6):
                            if k == 5:
                                matches = (pairs[:, 0] == bands[4]) | (pairs[:, 1] == bands[4])
                                if clipped:
                                    # Preserve the clipped-target path while the
                                    # separate clipped-background issue is investigated.
                                    matches &= (pairs[:, 0] != 5) & (pairs[:, 1] != 5)
                            elif bands[k-1] == bands[k]:
                                matches = np.zeros(len(pairs), dtype=bool)
                            else:
                                matches = np.all(pairs == sorted((bands[k-1], bands[k])), axis=1)
                            errors.append(np.where(matches, np.abs(radial-k), np.inf))
                        errors = np.asarray(errors).T
                        levels = np.argmin(errors, axis=1)+1
                        keep = np.min(errors, axis=1) < (.55 if iteration == 0 else .30)
                        points, target = edge_points[keep], levels[keep]
                        distinct, counts = np.unique(target, return_counts=True)
                        # Require a visible centre, outer termination and independent
                        # intermediate boundaries. Same-colour neighbours may merge.
                        # Three edges may seed the fit; a merged-core solution must
                        # resolve all four observable edges after that first update.
                        required_boundaries = 4 if core_rings == 2 and iteration > 0 else 3
                        if (len(distinct) < required_boundaries or core_rings not in distinct
                                or 5 not in distinct or min(counts) < 8):
                            selected = None
                            break
                        validation = np.arange(len(points)) % 4 == 0
                        fit = solver(
                            lambda p: radii(p, points[~validation])-target[~validation], params,
                            bounds=([-.3, -.3, .45, -.6, .45, -.15, -.15],
                                    [.3, .3, 2., .6, 2., .15, .15]),
                            loss='soft_l1', f_scale=.1, max_nfev=45)
                        if not fit.success:
                            selected = None
                            break
                        params = fit.x
                        radius = radii(params, xy).reshape(height, width)
                        bands = read_bands(radius)
                        if bands is None:
                            selected = None
                            break
                        selected = (points, target, validation)
                    if selected is None or bands[0] != color or len(set(bands)) < 3:
                        continue
                    if core_rings == 2 and (bands[:2] != [color, color] or bands[2] == color):
                        continue
                    metric = np.array([[params[2], params[3]], [params[3], params[4]]])
                    if np.min(np.linalg.eigvalsh(metric)) < .4 or np.linalg.norm(params[5:]) > .15:
                        continue
                    points, target, validation = selected
                    residual = np.abs(radii(params, points)-target)
                    validation_error = float(np.quantile(residual[validation], .9))
                    fit_error = float(np.quantile(residual[~validation], .9))
                    diagnostic.update(fit_error=fit_error, validation_error=validation_error,
                                      parameters=params.tolist(), boundary_samples=[
                                          {"level": int(k), "train": int(np.count_nonzero((target == k) & ~validation)),
                                           "validation": int(np.count_nonzero((target == k) & validation))}
                                          for k in np.unique(target)])
                    if (validation_error > .18 or fit_error > .18 or any(
                            row["train"] < 5 or row["validation"] < 2
                            for row in diagnostic["boundary_samples"])):
                        continue

                    direction = np.arctan2(xy[:, 1]-params[1], xy[:, 0]-params[0]).reshape(height, width)
                    sectors = ((direction+math.pi)*8/(2*math.pi)).astype(int) % 8
                    confidences, checks = [], []
                    for k, expected in enumerate(bands):
                        mask = (radius > k+.25) & (radius < k+.75) & (labels != 5)
                        pixels = labels[mask]
                        confidence = float(np.count_nonzero(pixels == expected)/max(1, len(pixels)))
                        votes = []
                        for sector in range(8):
                            part = labels[mask & (sectors == sector)]
                            if len(part) >= 2:
                                votes.append(int(np.argmax(np.bincount(part, minlength=5))))
                        agreement = votes.count(expected)/max(1, len(votes))
                        confidences.append(confidence)
                        checks.append({"band": k+1, "color": names[expected], "pixels": len(pixels),
                                       "confidence": confidence, "sectors": len(votes),
                                       "sector_agreement": agreement})
                    diagnostic["bands"] = checks
                    min_sectors = 4 if clipped else 6
                    if any(row["confidence"] < .80 or row["sectors"] < min_sectors or
                           row["sector_agreement"] < .85 for row in checks):
                        continue
                    diagnostic.update(status="PASS", reason=("partial rings validated from visible center and arcs"
                                      if clipped else "small-core rings validated from original pixels and boundaries"))
                    inside = radius <= 5
                    y, x = np.nonzero(inside)
                    bbox = (int(x.min()), int(y.min()), int(np.ptp(x)+1), int(np.ptp(y)+1))
                    center_x = cx+ca*a/2*params[0]-sa*b/2*params[1]
                    center_y = cy+sa*a/2*params[0]+ca*b/2*params[1]
                    colors = tuple(names[i] for i in bands)
                    total = sum(self.COLOR_VALUE[name] for name in colors)
                    result = self.SUM_TO_TYPE.get(total, 'FAKE')
                    observation = {"bands": colors, "confs": tuple(confidences), "sum": total,
                            "result": result, "cx": float(center_x), "cy": float(center_y),
                            "bbox": bbox, "r": float(max(bbox[2:])/2),
                            "edge": bool(any(np.any(edge) for edge in
                                             (inside[0], inside[-1], inside[:, 0], inside[:, -1]))),
                            "geom_score": max(0., 1-validation_error), "read_conf": min(confidences),
                            "final": min(confidences), "s": 1., "mode": "projective-rings",
                            "geometry_source": ("visible-core-and-partial-arcs" if clipped
                                                else "visible-small-core-and-boundaries"),
                            "new_structure_check": diagnostic, "accepted": result != 'FAKE',
                            "reason": "accepted" if result != 'FAKE' else "fake"}
                    if core_rings == 1:
                        return observation
                    merged_solutions.append(observation)
                if not merged_solutions:
                    return None
                best = min(merged_solutions, key=lambda obs: obs['new_structure_check']['validation_error'])
                if any(other['bands'] != best['bands'] or
                       math.hypot(other['cx']-best['cx'], other['cy']-best['cy']) >
                       max(2., .1*min(other['r'], best['r'])) for other in merged_solutions):
                    # Competing merged-core models cannot choose an ID or a centre.
                    return None
                self._partial_geometry_debug = best['new_structure_check']
                return best

            def _secondary_analyze_target_victim_frame(self, *args, **kwargs):
                result = self._primary_analyze_target_victim_frame(*args, **kwargs)
                partial = getattr(self, '_partial_geometry_debug', None)
                if partial:
                    result['partial_geometry_debug'] = partial
                if partial and partial['status'] == 'WAITING' and result.get('kind') is None:
                    raw = result.get('raw_result') or {}
                    candidates = (raw.get('target') or {}).get('candidates') or []
                    validated_fake = any((c.get('observation') or {}).get('new_structure_check', {})
                                         .get('status') == 'PASS' for c in candidates)
                    if not validated_fake:
                        for candidate in candidates:
                            observation = candidate.get('observation')
                            if observation is not None and candidate.get('edge'):
                                candidate['unverified_label_guess'] = candidate.get('label_guess')
                                candidate.update(label_guess=None, reason=partial['reason'])
                                observation.update(reason=partial['reason'], new_structure_check=partial)
                        result['report'].update(reason=partial['reason'], fake_target=None)
                        result['log_lines'] = self._build_log_lines(
                            raw, result['report'], result.get('victim_debug'),
                            result.get('target_debug'), result.get('fake_victim'))
                return result

            def _secondary_order_quad(self, pts):
                points = np.asarray(pts, dtype=np.float32).reshape(-1, 2)
                if points.shape != (4, 2) or not np.all(np.isfinite(points)):
                    raise ValueError("four finite plaque corners required")
                old = self._primary_order_quad(points)
                if (len(np.unique(old, axis=0)) == 4 and cv2.isContourConvex(old)
                        and abs(cv2.contourArea(old)) > 1):
                    return old
                center = points.mean(axis=0)
                angles = np.arctan2(points[:, 1]-center[1], points[:, 0]-center[0])
                ordered = points[np.argsort(angles)]
                first = min(range(4), key=lambda i: (float(ordered[i].sum()),
                                                    float(ordered[i, 1]), float(ordered[i, 0])))
                ordered = np.roll(ordered, -first, axis=0).copy()
                if (len(np.unique(ordered, axis=0)) != 4 or not cv2.isContourConvex(ordered)
                        or abs(cv2.contourArea(ordered)) <= 1):
                    raise ValueError("degenerate plaque corners")
                return ordered

            def _native_ring_structure(self, bgr, geom, obs):
                """Check original camera pixels, avoiding colours invented by resampling.

        Repeated neighbouring colours are allowed. Missing angular coverage is
        inconclusive; it is not evidence that a target is fake.
        """
                out = {"status": "WAITING", "reason": "insufficient original-pixel support",
                       "bands": [], "outer_support": 0, "outer_change": 0.0}
                matrix, rout = self._norm_transform(geom)
                norm = cv2.warpAffine(bgr, matrix, (2*rout, 2*rout),
                                      flags=cv2.INTER_CUBIC, borderMode=cv2.BORDER_CONSTANT,
                                      borderValue=(128, 128, 128))
                normalized_hsv = cv2.cvtColor(norm, cv2.COLOR_BGR2HSV)
                cx, cy = self._refine_center_norm(normalized_hsv, float(rout), float(rout), rout)
                inverse = cv2.invertAffineTransform(matrix)
                hsv = cv2.cvtColor(bgr, cv2.COLOR_BGR2HSV)
                height, width = hsv.shape[:2]
                pitch = .2 * float(obs.get("s", 1.0)) * self.COG_NORM_R0
                if geom['mode'] == 'hough':
                    out['sampling_center_px'] = (inverse @ np.array([cx, cy, 1.])).tolist()
                    out['sampling_pitch_px'] = pitch*float(geom['r'])/self.COG_NORM_R0
                angles = np.linspace(0, 2*math.pi, 24, endpoint=False)

                def sample(radius, angle):
                    x = cx + radius*math.cos(angle)
                    y = cy + radius*math.sin(angle)
                    u = int(round(inverse[0, 0]*x + inverse[0, 1]*y + inverse[0, 2]))
                    v = int(round(inverse[1, 0]*x + inverse[1, 1]*y + inverse[1, 2]))
                    if 0 <= u < width and 0 <= v < height:
                        return (u, v), self.classify_color(hsv[v, u])
                    return None, None

                for k, expected in enumerate(obs.get("bands", ())):
                    pixels, sectors = {}, []
                    for angle in angles:
                        votes = []
                        for fraction in (.30, .50, .70):
                            point, label = sample((k+fraction)*pitch, angle)
                            if point is not None:
                                pixels[point] = label
                            if label not in (None, "unknown"):
                                votes.append(label)
                        if votes:
                            sectors.append(Counter(votes).most_common(1)[0][0])
                    known = [value for value in pixels.values() if value != "unknown"]
                    fraction = known.count(expected)/max(1, len(known))
                    agreement = sectors.count(expected)/max(1, len(sectors))
                    out["bands"].append({"index": k, "expected": expected,
                                         "unique_pixels": len(known), "directions": len(sectors),
                                         "agreement": fraction, "direction_agreement": agreement})
                    if len(known) < 3 or len(sectors) < 8:
                        return out
                    if fraction < .75 or agreement < .75:
                        out.update(status="REJECTED", reason="original pixels disagree with decoded ring %d" % (k+1))
                        return out
                if len(out["bands"]) != 5:
                    return out
                # Check that the supposed outer ring actually ends. A coloured arena
                # background must not supply an unlimited outer band around a disc.
                expected = obs["bands"][-1]
                changes = []
                for angle in angles:
                    _, inner = sample(4.5*pitch, angle)
                    point, outer = sample(5.5*pitch, angle)
                    if inner == expected and point is not None:
                        changes.append(outer != expected)
                out["outer_support"] = len(changes)
                out["outer_change"] = sum(changes)/max(1, len(changes))
                if len(changes) < 8:
                    out["reason"] = "outer boundary needs more visible directions"
                elif out["outer_change"] < .6:
                    out.update(status="REJECTED", reason="outer ring blends into background")
                else:
                    out.update(status="PASS", reason="five rings and outer boundary supported by original pixels")
                return out

            def _secondary_concentric_read(self, bgr, cx, cy, r, edge, bbox):
                label = self._primary_concentric_read(bgr, cx, cy, r, edge, bbox)
                obs = self._last_obs or {}
                if label is None:
                    return None
                geom = self._geom_for(cx, cy)
                if geom["mode"] == "projective-rings":
                    check = obs.get("new_structure_check") or {
                        "status": "PASS", "reason": "projective boundary and sector checks passed"}
                else:
                    check = self._native_ring_structure(bgr, geom, obs)
                self._last_obs["new_structure_check"] = check
                if check["status"] != "PASS":
                    self._last_obs.update(accepted=False, reason="structure: " + check["reason"])
                    return None
                return label

            # Legacy-first target arbitration.

            @staticmethod
            def overlap(first, second):
                if not first or not second:
                    return 0.
                x, y, w, h = first
                xx, yy, ww, hh = second
                intersection = max(0., min(x+w, xx+ww)-max(x, xx))*max(0., min(y+h, yy+hh)-max(y, yy))
                return intersection/max(1., w*h+ww*hh-intersection)

            def _verified_target(self, comparison):
                if not comparison or comparison.get('error') or not comparison.get('would_report'):
                    return False
                new = comparison.get('result') or {}
                best = (new.get('raw_result') or {}).get('best') or {}
                observation = best.get('observation') or {}
                check = observation.get('new_structure_check') or {}
                bands = best.get('bands') or []
                rows, edges = check.get('bands') or [], check.get('boundary_samples') or []
                try:
                    source = observation.get('geometry_source')
                    minimum_sectors = 6 if source == 'visible-small-core-and-boundaries' else 4
                    return bool(
                        new.get('kind') == 'target' and new.get('label') in self.TARGET_NAMES
                        and best.get('accepted') and best.get('centered') and best.get('bbox')
                        and source in self.VERIFIED_SOURCES and check.get('status') == 'PASS'
                        and len(bands) == len(rows) == 5
                        and all(math.isfinite(float(check[key])) and 0 <= float(check[key]) <= .18
                                for key in ('fit_error', 'validation_error'))
                        and len({row['level'] for row in edges}) >= 4
                        and 5 in {row['level'] for row in edges}
                        and all(row['train'] >= 5 and row['validation'] >= 2 for row in edges)
                        and all(row['color'] == color and row['confidence'] >= .80
                                and row['sectors'] >= minimum_sectors and row['sector_agreement'] >= .85
                                for row, color in zip(rows, bands))
                        and self.SUM_TO_TYPE.get(sum(self.COLOR_VALUE[c] for c in bands))
                            == new['label'])
                except (KeyError, TypeError, ValueError):
                    return False

            def _fuse_target(self, primary, comparison, side, now):
                result = dict(primary)
                new = (comparison or {}).get('result') or {}
                verified = self._verified_target(comparison)
                metadata = {'source': 'legacy', 'secondary_verified': verified,
                            'secondary_label': new.get('label') if new.get('kind') == 'target' else None,
                            'stable_frames': 0, 'stable_span_s': 0.}
                if primary.get('kind') in ('target', 'victim'):
                    self._fusion_tracks.pop(side, None)
                    if primary['kind'] == 'target' and new.get('kind') == 'target':
                        metadata['source'] = ('legacy_confirmed' if primary.get('label') == new.get('label')
                                              else 'legacy_conflict')
                    result['target_policy'] = metadata
                    return result
                if not verified:
                    self._fusion_tracks.pop(side, None)
                    result['target_policy'] = metadata
                    return result
                bbox = new['raw_result']['best']['bbox']
                previous = self._fusion_tracks.get(side)
                same = (previous and 0 <= now-previous['time'] <= self.MAX_GAP_S
                        and previous['label'] == new['label'] and self.overlap(previous['bbox'], bbox) >= .60)
                state = dict(previous) if same else {'start': now, 'count': 0, 'label': new['label'], 'time': -math.inf}
                if now > state['time']:
                    state['count'] += 1
                state.update(time=now, bbox=bbox)
                self._fusion_tracks[side] = state
                span = now-state['start']
                metadata.update(source='secondary_pending', stable_frames=state['count'],
                                stable_span_s=round(span, 3), center=new.get('center'), bbox=bbox)
                if state['count'] >= self.MIN_FRAMES and span+1e-9 >= self.MIN_SPAN_S:
                    result = copy.deepcopy(new)
                    result['report'] = self._make_report('target', new['label'], True)
                    result['reportable'] = bool(result['report']['should_report'])
                    metadata['source'] = 'secondary_fallback'
                result['target_policy'] = metadata
                if comparison is not None:
                    result['new_only_debug'] = comparison
                return result

            # Optional camera-LiDAR calibration for detailed captures.

            def _configure_capture_profile(self, robot_json, source='configured robot.json'):
                import hashlib
                if not isinstance(robot_json,dict) or not all(isinstance(entry,dict) for entry in robot_json.values()):
                    raise ValueError('robot.json must contain a device dictionary')
                self.source = source
                self.profile_hash = hashlib.sha256(json.dumps(robot_json, sort_keys=True).encode()).hexdigest()
                devices = {}
                for entry in robot_json.values():
                    if entry.get('customName') in ('camL', 'camR', 'lidar'):
                        name = entry['customName']
                        if name in devices:
                            raise ValueError('duplicate capture calibration device: '+name)
                        devices[name] = entry
                if set(devices) != {'camL', 'camR', 'lidar'}:
                    raise ValueError('robot.json must define camL, camR and lidar')
                self.poses = {}
                self.sizes = {}
                for name, entry in devices.items():
                    values = np.asarray([entry[k] for k in ('x','y','z','rx','rz','ry','a')], float)
                    if not np.all(np.isfinite(values)):
                        raise ValueError('nonfinite capture calibration pose: '+name)
                    translation = np.clip(values[:3], [-370,-100,-370], [370,370,370])/10000
                    translation[1] += .0185
                    rotation = self.axis_rotation(values[3:])
                    if name == 'lidar':
                        rotation = rotation @ self.axis_rotation((0,0,1,3.14159))
                    self.poses[name] = (rotation, translation)
                    if name in ('camL','camR'):
                        size = tuple(map(int, entry.get('custom', (40,32))))
                        if len(size) != 2 or min(size) < 2:
                            raise ValueError('invalid camera size in robot.json')
                        self.sizes[name] = size

            @staticmethod
            def axis_rotation(axis_angle):
                x, y, z, angle = map(float, axis_angle)
                axis = np.array([x,y,z]); length = np.linalg.norm(axis)
                if length < 1e-12:
                    if abs(angle) > 1e-12:
                        raise ValueError('nonzero rotation needs an axis')
                    return np.eye(3)
                x, y, z = axis/length
                skew = np.array(((0,-z,y),(z,0,-x),(-y,x,0)))
                return np.eye(3)+math.sin(angle)*skew+(1-math.cos(angle))*(skew@skew)


            def profile_snapshot(self):
                return {'source': self.source, 'sha256': self.profile_hash,
                        'status': 'configured JSON; live overlay validation required',
                        'poses': {name: {'rotation': r.tolist(), 'translation_m': t.tolist()}
                                  for name, (r,t) in self.poses.items()}}

            def camera_calibration(self, side):
                name = 'camL' if side == 'left' else 'camR'
                rc, tc = self.poses[name]; rl, tl = self.poses['lidar']
                # Existing capture renderer uses Right-Up-Forward rather than RDF.
                optical = np.diag([1.,-1.,1.]) @ self.RDF_FROM_FLU @ rc.T
                return {'rotation': optical@rl, 'translation_m': optical@(tl-tc),
                        'input_frame': 'lidar-native', 'source': self.source,
                        'profile_sha256': self.profile_hash}


            def _side_lidar_ranges_m(self, data, side):
                """Return one camera-width LiDAR sector, aligned with a side camera."""
                lidar = np.asarray(data.get("lidar", ()), dtype=np.float32)
                total = lidar.size
                hres = int(data.get("lidar_hres", total))
                if hres <= 0 or total < hres:
                    return []

                nlay = total // hres
                layer = min(getattr(self.mapper, 'FRONT_INDEX', self.LIDAR_FRONT_INDEX) // hres, nlay - 1)
                start = layer * hres
                scan = lidar[start:start + hres]
                if scan.size < 8:
                    return []

                lfov = float(data.get("lidar_fov", 2.0 * math.pi))
                cam_fov = float(data.get("cam_fov", self.cam_fov))
                w = int(data.get("cam_w", self.cam_w))
                if w <= 1 or lfov <= 0.0 or cam_fov <= 0.0:
                    return []

                front = max(0, min(getattr(self.mapper, 'FRONT_INDEX', self.LIDAR_FRONT_INDEX) - start, scan.size - 1))
                rad_per_idx = lfov / scan.size
                half_idx = (cam_fov / 2.0) / rad_per_idx
                centres = {"right": front + hres / 4.0, "left": front + 3.0 * hres / 4.0}
                centre = centres.get(side)
                if centre is None:
                    return []

                cols = np.arange(w, dtype=np.float32)
                idxf = centre + (cols - w / 2.0) / (w / 2.0) * half_idx
                li = np.mod(np.rint(idxf).astype(np.int64), scan.size)
                ranges_mm = scan[li]
                out = []
                for value in ranges_mm:
                    d = float(value)
                    out.append(d / 1000.0 if math.isfinite(d) and d > 0.0 else float("inf"))
                return out

            # Optional surface diagnostics.

            def _apply_victim_wall_geometry_check(self, result, data, side):
                """Gate live victim reports with the new segmented surface model."""
                if result.get("kind") != "victim":
                    return result
                try:
                    surface = self._victim_surface_shadow(data, side, result)
                except Exception as exc:
                    surface = {"decision": "unknown", "reason": "surface error %s: %s" %
                               (type(exc).__name__, exc), "alignment": "unverified",
                               "models": [], "points": [], "breaks_px": []}
                decision = surface.get("decision", "unknown")
                fake = dict(result.get("fake_victim") or {})
                fake.update(enabled=True,
                            is_real=True if decision == "real" else False if decision == "fake" else None,
                            block_report=decision != "real",
                            decision_mode="segmented-surface-" + decision,
                            reason=surface.get("reason"), active_surface=surface)
                raw = result.get("raw_result") or {}
                # raw.reportable contains visual centering eligibility, before the
                # old fake-victim gate. Never inherit the old gate's reportable=False.
                report = self._make_report(
                    result.get("kind"), result.get("label"),
                    raw.get("reportable", result.get("reportable", False)),
                    fake_victim=fake, fake_target=(result.get("report") or {}).get("fake_target"))
                result["fake_victim"] = fake
                result["report"] = report
                result["reportable"] = bool(report["should_report"])
                lines = [line for line in result.get("log_lines", [])
                         if not line.startswith(("[FAKE-VICTIM]", "[REPORT]", "[SURFACE-ACTIVE]"))]
                lines.append("[SURFACE-ACTIVE] decision=%s block=%s alignment=%s reason=%s" %
                             (decision, fake["block_report"], surface.get("alignment"), surface.get("reason")))
                lines.append("[REPORT] should_report=%s kind=%s label=%s name=%s save_code=%s reason=%s" %
                             tuple(report.get(key) for key in
                                   ("should_report", "kind", "label", "name", "save_code", "reason")))
                result["log_lines"] = lines
                return result

            def _victim_surface_shadow(self, data, side, result):
                """Pure surface calculation for captures and the optional surface gate.

        Uses unique rays, never resampled pixel counts. Extrinsics and layer
        height are NOT calibrated: same origin/side direction as the old path
        is a stated hypothesis, not a verified camera-to-LiDAR transform.
        No track, report or production detector state is written here.
        """
                out = {"decision": "unknown", "reason": "missing geometry",
                       "alignment": "pinhole/unverified-extrinsics-and-height",
                       "points": [], "models": [], "breaks_px": []}
                bbox = result.get("bbox") or ((result.get("raw_result") or {}).get("best") or {}).get("bbox")
                if bbox is None or side not in ("left", "right"):
                    return out
                w = int(data.get("cam_w", self.cam_w))
                fov = float(data.get("cam_fov", self.cam_fov))
                lfov = float(data.get("lidar_fov", 0.0))
                hres = int(data.get("lidar_hres", 0))
                scan_all = np.asarray(data.get("lidar", ()), dtype=float)
                if w < 2 or not 0 < fov < math.pi or hres < 8 or scan_all.size < hres:
                    return out
                # Quarter-turn side indices are only valid for a full-circle scan.
                if abs(lfov - 2 * math.pi) > 0.01:
                    out["reason"] = "unsupported non-360-degree lidar"
                    return out
                layer = min(getattr(self.mapper, 'FRONT_INDEX', self.LIDAR_FRONT_INDEX) // hres, scan_all.size // hres - 1)
                start = layer * hres
                front = max(0, min(getattr(self.mapper, 'FRONT_INDEX', self.LIDAR_FRONT_INDEX) - start, hres - 1))
                centre = front + hres * (0.25 if side == "right" else 0.75)
                step = lfov / hres
                fx = w / (2 * math.tan(fov / 2))
                bx, _, bw, _ = map(float, bbox)
                if bw < 4 or bx + bw <= 0 or bx >= w:
                    out["reason"] = "invalid or tiny bbox"
                    return out
                scan = scan_all[start:start + hres] / 1000.0
                # Project native LiDAR rays into a hypothetical co-located pinhole
                # camera. Do not interpolate over missing returns or depth boundaries.
                rays = []
                for index, distance in enumerate(scan):
                    angle = ((index - centre) * step + math.pi) % (2 * math.pi) - math.pi
                    if abs(angle) >= fov / 2 or not math.isfinite(distance) or not 0.015 <= distance <= 0.8:
                        continue
                    px = w / 2 + fx * math.tan(angle)
                    rays.append({"index": start + index, "px": float(px),
                                 "x": float(distance * math.sin(angle)),
                                 "z": float(distance * math.cos(angle)),
                                 "distance": float(distance), "angle": angle})
                rays.sort(key=lambda p: p["px"])
                segment = 0
                for i, point in enumerate(rays):
                    if i:
                        prev = rays[i - 1]
                        da = point["angle"] - prev["angle"]
                        # Range-dependent discontinuity limit; missing rays also split.
                        jump = max(0.008, 4 * min(point["distance"], prev["distance"]) * da)
                        if da > 1.6 * step or abs(point["distance"] - prev["distance"]) > jump:
                            segment += 1
                            out["breaks_px"].append(round(point["px"], 1))
                    point["segment"] = segment
                gap = max(1.0, 0.10 * bw)
                center = [p for p in rays if bx + 0.1 * bw <= p["px"] <= bx + 0.9 * bw]
                left = [p for p in rays if bx - 2 * bw <= p["px"] < bx - gap]
                right = [p for p in rays if bx + bw + gap < p["px"] <= bx + 3 * bw]
                # Keep nearby opposite evidence even if segmentation later trims it.
                # A visible depth step must not be mistaken for an absent shoulder.
                left_probe, right_probe = left[-6:], right[:6]
                # Keep only the nearest continuous surface on each side. Never fit
                # across an additional corner/gap hidden in a broad shoulder window.
                left = [p for p in left if p["segment"] == left[-1]["segment"]] if left else []
                right = [p for p in right if p["segment"] == right[0]["segment"]] if right else []
                chosen = {p["index"]: "center" for p in center}
                chosen.update({p["index"]: "left" for p in left})
                chosen.update({p["index"]: "right" for p in right})
                out.update({"layer": layer, "support": (len(left), len(center), len(right)),
                            "bbox": tuple(bbox), "segments": segment + 1 if rays else 0})
                for p in rays:
                    out["points"].append({"index": p["index"], "px": round(p["px"], 2),
                                          "range_mm": round(p["distance"] * 1000, 2),
                                          "segment": p["segment"],
                                          "role": chosen.get(p["index"], "unused")})
                edges = [max(0.0, min(w - 1.0, x)) for x in (bx, bx + bw / 2, bx + bw)]
                out["ray_shift"] = [round((math.atan((x - w / 2) / fx)
                                          - (x - w / 2) / (w / 2) * fov / 2) / step, 2) for x in edges]
                if len(center) < 6 or max(len(left), len(right)) < 6:
                    out["reason"] = "insufficient unique rays on continuous shoulders"
                    return out

                # With a missing/short shoulder, extrapolate from the supported side.
                # The short shoulder is optional validation, never a sample-count veto.
                single_side = min(len(left), len(right)) < 6
                support_side = "left" if len(left) >= len(right) else "right"
                optional = (right_probe if support_side == "left" else left_probe) if single_side else []
                out["support_mode"] = support_side + "-only" if single_side else "two-sided"

                def fit_profile(a, b):
                    # Every third point is held out on each available fitting side.
                    train = [p for group in (a, b) for i, p in enumerate(group) if i % 3 != 0]
                    validation = [p for group in (a, b) for i, p in enumerate(group) if i % 3 == 0]
                    origin = float(np.mean([p["x"] for p in train]))
                    scale = max(0.005, max(abs(p["x"] - origin) for p in train))
                    candidates = []
                    for degree in (1, 2):
                        u = np.array([(p["x"] - origin) / scale for p in train])
                        if len(np.unique(u)) < degree + 3:
                            continue
                        coeff = np.polyfit(u, [p["z"] for p in train], degree)

                        def residual(points):
                            u = np.array([(p["x"] - origin) / scale for p in points])
                            slope = np.polyval(np.polyder(coeff), u) / scale
                            return 1000 * (np.polyval(coeff, u) - [p["z"] for p in points]) / np.sqrt(1 + slope ** 2)

                        fit = float(np.percentile(np.abs(residual(train)), 80))
                        valid = float(np.percentile(np.abs(residual(validation)), 80))
                        r = residual(center)
                        candidates.append({"model": "linear" if degree == 1 else "quadratic",
                                           "fit_mm": fit, "validation_mm": valid,
                                           "median_mm": float(np.median(r)),
                                           "abs_p80_mm": float(np.percentile(np.abs(r), 80)),
                                           "p80_mm": float(np.percentile(r, 80)),
                                           "iqr_mm": float(np.percentile(r, 90) - np.percentile(r, 10)),
                                           "close": float(np.mean(r >= 5)),
                                           "center_residuals_mm": r.tolist(),
                                           "opposite_error_mm": float(np.max(np.abs(residual(optional)))) if optional else 0.0,
                                           "score": valid + fit + (0.5 if degree == 2 else 0)})
                    return min(candidates, key=lambda c: c["score"]) if candidates else None

                fit_left = left if not single_side or support_side == "left" else []
                fit_right = right if not single_side or support_side == "right" else []
                base_n = max(len(left), len(right)) if single_side else min(len(left), len(right))
                near_n = max(6, int(base_n * 0.5))
                near = fit_profile(fit_left[-near_n:], fit_right[:near_n])
                wide = fit_profile(fit_left, fit_right)
                out["models"] = [dict(m, scale=s) for s, m in (("near", near), ("wide", wide)) if m]
                if not near or not wide or any(m["fit_mm"] > 3 or m["validation_mm"] > 4 for m in (near, wide)):
                    out["reason"] = "wall model fails held-out validation (corner/mixed surface)"
                    return out
                wide_extra = (max(0, len(fit_left) - near_n), max(0, len(fit_right) - near_n))
                out["wide_extra"] = wide_extra
                out["wide_support"] = ("both" if min(wide_extra) >= 3 else
                                       "left" if wide_extra[0] >= 3 else
                                       "right" if wide_extra[1] >= 3 else "none")
                if max(wide_extra) < 3:
                    out["reason"] = "not enough independent wider wall support"
                    return out
                if abs(near["median_mm"] - wide["median_mm"]) > 3:
                    out["reason"] = "near/wide wall models disagree"
                    return out
                if single_side and (
                    max(m["opposite_error_mm"] for m in (near, wide)) > 4
                    or np.max(np.abs(np.asarray(near["center_residuals_mm"])
                                     - np.asarray(wide["center_residuals_mm"]))) > 3
                ):
                    out["reason"] = "one-sided extrapolation disagrees across scales or opposite samples"
                    return out
                center_segments = {p["segment"] for p in center}
                reference_segments = {p["segment"] for group in (fit_left, fit_right) for p in group}
                continuous = len(center_segments) == 1 and center_segments == reference_segments
                if all(m["median_mm"] >= 5 and m["p80_mm"] >= 6 and m["close"] >= 0.55 for m in (near, wide)):
                    out.update(decision="fake", reason="protrusion against validated near/wide support")
                elif continuous and all(abs(m["median_mm"]) <= 2 and m["abs_p80_mm"] <= 3 and m["iqr_mm"] <= 5 for m in (near, wide)):
                    out.update(decision="real", reason="continuous near/wide surface (wall identity still unverified)")
                else:
                    out["reason"] = "discontinuous or inconclusive surface; another view needed"
                return out

            # Calibrated overlays and plaque measurements.

            def _plaque_native_calibration(self, side):
                override = self.VICTIM_PLAQUE_CALIBRATION.get(side)
                if override is not None:
                    return override
                calibration_profile = self._get_capture_calibration()
                return calibration_profile.camera_calibration(side) if calibration_profile is not None else None

            def _project_plaque_lidar(self, data, side, width, height, calibration, *, include_outside_vertical=False):
                """R2023b fixed LiDAR spherical ranges -> optical XYZ and pixels.

        Matches WbLidar::updatePointCloud: theta=FOV/2-i*FOV/(N-1),
        phi=verticalFOV/2-layer*verticalFOV/(layers-1), native FLU axes.
        No point-cloud device or supervisor access is required.
        """
                if calibration is None:
                    info = (data.get('camera_intrinsics') or {}).get(side) or {}
                    return {'indices':np.empty(0,int), 'layers':np.empty(0,int),
                            'xyz':np.empty((0,3)), 'u':np.empty(0), 'v':np.empty(0),
                            'source':'unavailable', 'translation_m':None,
                            'metadata_matches_profile':False,
                            'live_camera':{'width':width, 'height':height,
                                           'fov':float(info.get('fov',data.get('cam_fov',self.cam_fov)))}}
                rotation = np.asarray(calibration["rotation"], dtype=float)
                translation = np.asarray(calibration["translation_m"], dtype=float)
                if (rotation.shape != (3, 3) or translation.shape != (3,)
                        or not np.all(np.isfinite(rotation)) or not np.all(np.isfinite(translation))
                        or not np.allclose(rotation.T @ rotation, np.eye(3), atol=1e-5)
                        # RUF is left-handed: the FLU -> RUF basis has determinant -1.
                        or not np.isclose(np.linalg.det(rotation), -1, atol=1e-5)):
                    raise ValueError("invalid native calibration transform")
                hres = int(data["lidar_hres"])
                scan = np.asarray(data["lidar_raw_m"], dtype=float).reshape(-1)
                if hres < 2 or not scan.size or scan.size % hres:
                    raise ValueError("invalid lidar scan dimensions")
                info = (data.get('camera_intrinsics') or {}).get(side) or {}
                fov = float(info.get('fov',data.get("cam_fov", self.cam_fov)))
                if (not 0 < fov < math.pi or width < 2 or height < 2
                        or abs(float(data["lidar_fov"]) - 2 * math.pi) > 0.01):
                    raise ValueError("unsupported camera or lidar projection")
                metadata_matches = bool(
                    "expected_fov" not in calibration
                    or (abs(fov - calibration["expected_fov"]) <= 0.001
                        and width == calibration["expected_width"]
                        and height == calibration["expected_height"])
                )
                layers = scan.size // hres
                theta = np.linspace(float(data["lidar_fov"]) / 2, -float(data["lidar_fov"]) / 2, hres)
                phi = np.linspace(float(data.get("lidar_vertical_fov", data.get("lidar_vfov"))) / 2,
                                  -float(data.get("lidar_vertical_fov", data.get("lidar_vfov"))) / 2, layers) if layers > 1 else np.zeros(1)
                # Installed profile has tiltAngle=0 and type=fixed. Do not substitute
                # the old FRONT_INDEX approximation for the native sensor directions.
                directions = np.stack(np.broadcast_arrays(
                    np.cos(phi[:, None]) * np.cos(theta),
                    np.cos(phi[:, None]) * np.sin(theta), np.sin(phi[:, None])), axis=-1).reshape(-1, 3)
                ids = np.flatnonzero(np.isfinite(scan) & (scan >= 0.015) & (scan <= 0.8))
                xyz = np.einsum('ij,kj->ik', directions[ids] * scan[ids, None], rotation) + translation
                visible = xyz[:, 2] > 0.005
                xyz, ids = xyz[visible], ids[visible]
                fx = width / (2 * math.tan(fov / 2))
                # Pixel centres lie at integer coordinates; optical centre is (N-1)/2.
                u = (width - 1) / 2 + fx * xyz[:, 0] / xyz[:, 2]
                v = (height - 1) / 2 - fx * xyz[:, 1] / xyz[:, 2]
                visible = (u >= 0) & (u <= width - 1)
                if not include_outside_vertical:
                    visible &= (v >= 0) & (v <= height - 1)
                return {"xyz": xyz[visible], "indices": ids[visible],
                        "layers": ids[visible] // hres, "u": u[visible], "v": v[visible],
                        "source": calibration.get("source", "configured-native-transform"),
                        "translation_m": translation.tolist(),
                        "metadata_matches_profile": metadata_matches,
                        "live_camera": {"width": width, "height": height, "fov": fov}}

            def _victim_plaque_depth_debug(self, frame, data, side, result):
                """Measure black-to-white relief, without assigning a real/fake label.

        Projection uses the active JSON sensor poses and native Webots rays.
        Visual overlay validation is still required in simulation.
        Per-layer lines cancel plaque tilt without inventing vertical support.
        No wall pixels, active reports or temporal tracks are used here.
        """
                out = {"status": "insufficient", "decision": "unknown",
                       "alignment": "unverified-co-located", "black_n": 0,
                       "white_n": 0, "rays_in_bbox": 0, "layers": [], "points": [],
                       "median_mm": None, "p80_mm": None,
                       "reason": "missing plaque or lidar metadata"}
                if cv2 is None:
                    out["reason"] = "opencv unavailable"
                    return out
                bbox = result.get("bbox") or ((result.get("raw_result") or {}).get("best") or {}).get("bbox")
                hres = int(data.get("lidar_hres", 0))
                vfov = data.get("lidar_vertical_fov", data.get("lidar_vfov"))
                raw = data.get("lidar_raw_m")
                if bbox is None or raw is None or hres < 8 or vfov is None:
                    return out
                scan = np.asarray(raw, dtype=float).reshape(-1)
                if scan.size == 0 or scan.size % hres or side not in ("left", "right"):
                    return out
                layers = scan.size // hres
                fov = float(data.get("cam_fov", self.cam_fov))
                lfov = float(data.get("lidar_fov", 0))
                if abs(lfov - 2 * math.pi) > 0.01 or not 0 < fov < math.pi:
                    out["reason"] = "unsupported projection metadata"
                    return out
                height, width = frame.shape[:2]
                bx, by, bw, bh = map(float, bbox)
                x0, x1 = max(0, int(math.floor(bx))), min(width, int(math.ceil(bx + bw)))
                y0, y1 = max(0, int(math.floor(by))), min(height, int(math.ceil(by + bh)))
                if x1 - x0 < 5 or y1 - y0 < 5:
                    out["reason"] = "plaque too small"
                    return out
                hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
                roi = np.zeros((height, width), np.uint8)
                roi[y0:y1, x0:x1] = 1
                white = ((hsv[:, :, 1] <= 60) & (hsv[:, :, 2] >= 170)).astype(np.uint8) * roi
                black = (hsv[:, :, 2] <= 70).astype(np.uint8) * roi
                # Exclude mixed pixels and the outside edge of the detected plaque.
                kernel = np.ones((3, 3), np.uint8)
                white = cv2.erode(white, kernel, borderType=cv2.BORDER_CONSTANT, borderValue=0)
                black = cv2.erode(black, kernel, borderType=cv2.BORDER_CONSTANT, borderValue=0)
                out["white_pixels"], out["black_pixels"] = int(white.sum()), int(black.sum())
                calibration = self._plaque_native_calibration(side)
                if calibration is None or calibration.get('input_frame') != 'lidar-native':
                    out['reason'] = 'native sensor calibration is unavailable'
                    return out
                projection = self._project_plaque_lidar(data, side, width, height, calibration)
                out.update(alignment='robot-profile/needs-overlay-check',
                           calibration_source=projection['source'],
                           translation_m=projection['translation_m'],
                           metadata_matches_profile=projection['metadata_matches_profile'],
                           live_camera=projection['live_camera'])
                out['projected_points'] = [
                    {'index':int(i), 'layer':int(layer), 'u':float(u), 'v':float(v)}
                    for i,layer,u,v in zip(projection['indices'],projection['layers'],projection['u'],projection['v'])]
                residuals = []
                for layer in range(layers):
                    chosen = projection['layers'] == layer
                    xyz = projection['xyz'][chosen]
                    ids = projection['indices'][chosen]-layer*hres
                    u = np.rint(projection['u'][chosen]).astype(int)
                    v = np.rint(projection['v'][chosen]).astype(int)
                    inside = (u >= x0) & (u < x1) & (v >= y0) & (v < y1)
                    out["rays_in_bbox"] += int(inside.sum())
                    xyz, ids, u, v = xyz[inside], ids[inside], u[inside], v[inside]
                    wm, bm = white[v, u] != 0, black[v, u] != 0
                    out["white_n"] += int(wm.sum())
                    out["black_n"] += int(bm.sum())
                    row = {"layer": layer, "white_n": int(wm.sum()), "black_n": int(bm.sum()),
                           "median_mm": None, "white_error_mm": None, "reason": "insufficient samples"}
                    out["layers"].append(row)
                    for i in np.flatnonzero(wm | bm):
                        out["points"].append({"index": int(layer * hres + ids[i]),
                                              "u": int(u[i]), "v": int(v[i]),
                                              "role": "white" if wm[i] else "black"})
                    if wm.sum() < 6 or bm.sum() < 3:
                        continue
                    whites, blacks = xyz[wm], xyz[bm]
                    # Do not extrapolate a short white patch across the whole symbol.
                    if blacks[:, 0].min() < whites[:, 0].min() or blacks[:, 0].max() > whites[:, 0].max():
                        row["reason"] = "white samples do not span black samples"
                        continue
                    hold = np.arange(len(whites)) % 3 == 0
                    train, validation = whites[~hold], whites[hold]
                    origin = train[:, 0].mean()
                    scale = np.ptp(train[:, 0])
                    if scale < 0.002:
                        row["reason"] = "white reference too narrow"
                        continue
                    coeff = np.polyfit((train[:, 0] - origin) / scale, train[:, 2], 1)
                    divisor = math.sqrt(1 + (coeff[0] / scale) ** 2)
                    def residual(points):
                        return 1000 * (np.polyval(coeff, (points[:, 0] - origin) / scale) - points[:, 2]) / divisor
                    error = float(np.percentile(np.abs(residual(validation)), 80))
                    row["white_error_mm"] = error
                    if error > 2.0:  # reference-quality gate only; NOT a fake threshold
                        row["reason"] = "white reference inconsistent"
                        continue
                    relief = residual(blacks)
                    row.update(median_mm=float(np.median(relief)), reason="measurement only")
                    residuals.extend(relief.tolist())
                if residuals:
                    out.update(status="measurement-only", median_mm=float(np.median(residuals)),
                               p80_mm=float(np.percentile(residuals, 80)),
                               reason="alignment and real/fake threshold require validation")
                elif out["rays_in_bbox"] == 0:
                    out["reason"] = "no projected lidar returns inside plaque; check scan coverage and sensor poses"
                else:
                    out["reason"] = "no layer has usable black and white reference samples"
                return out

            # Optional calibration loading for detailed captures.

            def _get_capture_calibration(self):
                """Load the active JSON once; never fall back to the retired side-camera profile."""
                if hasattr(self,'_capture_calibration'):
                    return self._capture_calibration
                self._capture_calibration = None
                folder = os.path.dirname(os.path.abspath(__file__))
                explicit = self.CAPTURE_CALIBRATION_PATH
                candidates = [explicit] if explicit else [
                    os.path.join(folder, 'robot.json'), os.path.join(folder, 'json_files', 'robot.json')]
                path = next((p for p in candidates if p and os.path.isfile(p)), None)
                try:
                    if path is None:
                        raise ValueError('robot.json was not found beside the detector module')
                    with open(path, encoding='utf-8') as profile_file:
                        self._configure_capture_profile(json.load(profile_file), os.path.abspath(path))
                    self._capture_calibration = self
                    self._capture_calibration_error = None
                except (OSError, ValueError, TypeError, KeyError) as exc:
                    self._capture_calibration_error = str(exc)
                return self._capture_calibration


            # Explicit detailed captures.

            def _alignment_debug_value(self, value, depth=0):
                """Convert detector state into readable JSON without duplicating images.

        Camera frames and masks are already stored losslessly in the NPZ file.
        The text report records their shape and basic statistics so the full
        decision state remains readable and reasonably small.
        """
                if depth > 12:
                    return "<maximum nesting depth reached>"
                if isinstance(value, np.ndarray):
                    summary = {
                        "type": "ndarray",
                        "shape": list(value.shape),
                        "dtype": str(value.dtype),
                        "size": int(value.size),
                    }
                    if value.size <= 16:
                        summary["values"] = value.tolist()
                    elif value.size and np.issubdtype(value.dtype, np.number):
                        finite = value[np.isfinite(value)]
                        if finite.size:
                            summary.update(min=float(np.min(finite)), max=float(np.max(finite)))
                    return summary
                if isinstance(value, np.generic):
                    return self._alignment_debug_value(value.item(), depth + 1)
                if isinstance(value, dict):
                    return {str(key): self._alignment_debug_value(item, depth + 1)
                            for key, item in value.items()}
                if isinstance(value, (list, tuple, deque)):
                    return [self._alignment_debug_value(item, depth + 1) for item in value]
                if isinstance(value, bytes):
                    return {"type": "bytes", "size": len(value)}
                if isinstance(value, float) and not math.isfinite(value):
                    return str(value)
                if value is None or isinstance(value, (bool, int, float, str)):
                    return value
                return repr(value)

            def _alignment_detection_debug_text(self, data, results, metadata):
                """Build a complete, plain-English detection report for an L capture."""
                lines = [
                    "CAMERA-LIDAR ALIGNMENT CAPTURE - DETAILED DETECTION DEBUG",
                    "=" * 64,
                    "Simulation time: %.3f s" % float(data.get("time", 0.0)),
                    "Detection time: %.3f s (cached detector state)" %
                    float(getattr(self, '_target_victim_cache_t', data.get('time', 0.0))),
                    "Robot position (mm): %s" % json.dumps(
                        self._alignment_debug_value(data.get('position') or {}), sort_keys=True),
                    "Robot orientation (degrees): %s" % json.dumps(
                        self._alignment_debug_value(data.get('orientation') or {}), sort_keys=True),
                    "Capture directory: %s" % self.ALIGNMENT_CAPTURE_DIR,
                    "Supervisor report output: %s (READY describes detector eligibility only)" %
                    ("ON" if getattr(self, "AUTO_REPORT_ENABLED", False) else "OFF"),
                    "Camera: %sx%s px, FOV=%.6f rad" %
                    (self.cam_w, self.cam_h, float(data.get("cam_fov", self.cam_fov))),
                    "LiDAR: horizontal_resolution=%s, FOV=%.6f rad, vertical_FOV=%.6f rad" %
                    (data.get("lidar_hres"), float(data.get("lidar_fov", 0.0)),
                     float(data.get("lidar_vertical_fov", 0.0))),
                    "This file records the existing decision state; saving it does not rerun detection.",
                ]
                for side in ("left", "right"):
                    lines.extend(("", "[%s CAMERA]" % side.upper(), "-" * 64))
                    entry = results.get(side) or {}
                    if entry.get("error"):
                        lines.append("Detection error: %s" % entry["error"])
                        continue
                    result = entry.get("result")
                    if result is None:
                        lines.append("No camera frame or detector result.")
                        continue
                    result["side"] = side
                    report = result.get("report") or {}
                    lines.append(
                        "Summary: detected=%s kind=%s label=%s name=%s confidence=%s "
                        "reportable=%s center=%s" %
                        (result.get("detected"), result.get("kind"), result.get("label"),
                         result.get("name"), result.get("confidence"),
                         result.get("reportable"), result.get("center")))
                    lines.append("Final report: should_report=%s save_code=%s reason=%s" %
                                 (report.get("should_report"), report.get("save_code"),
                                  report.get("reason")))

                    lines.append("\nDetector decision log:")
                    decision_lines = list(result.get("log_lines") or [])
                    lines.extend("  " + row for row in decision_lines)
                    if not decision_lines:
                        lines.append("  No detector decision log lines.")

                    summary = self._active_debug_return(result, data)
                    lines.append('\nActive-system return:')
                    lines.append('  ' + self._compact_detection_return({'observations': {side: summary}}))

                    lines.append("\nSECONDARY TARGET ANALYSIS - final policy controls eligibility:")
                    comparison_lines = self._new_only_debug_lines(result, side)
                    lines.extend("  " + row for row in comparison_lines)
                    if not comparison_lines:
                        lines.append("  No shadow comparison available.")

                    overhang = result.get("overhang_debug") or {}
                    lines.append("\nVisual overhang measurement:")
                    lines.append(
                        "  status=%s decision=%s bbox=%s overhang_px=%s black_below=%s "
                        "density=%s max_width=%s" %
                        (overhang.get("status"), overhang.get("decision"), overhang.get("bbox"),
                         overhang.get("overhang_px"), overhang.get("black_below"),
                         overhang.get("below_density"), overhang.get("below_max_width")))

                    lines.append("\n3D surface and plaque measurements:")
                    surface = ((result.get("fake_victim") or {}).get("active_surface") or {})
                    if surface:
                        lines.append("  surface decision=%s alignment=%s reason=%s" %
                                     (surface.get("decision"), surface.get("alignment"),
                                      surface.get("reason")))
                        lines.append("  layer=%s support=%s wide_support=%s wide_extra=%s breaks_px=%s" %
                                     (surface.get("layer"), surface.get("support"),
                                      surface.get("wide_support"), surface.get("wide_extra"),
                                      surface.get("breaks_px")))
                        for model in surface.get("models", []):
                            lines.append("  wall model: %s" % json.dumps(
                                self._alignment_debug_value(model), sort_keys=True))
                        for role in ("left", "center", "right"):
                            points = [point for point in surface.get("points", [])
                                      if point.get("role") == role]
                            lines.append("  %s rays: %s" %
                                         (role, json.dumps(self._alignment_debug_value(points),
                                                           sort_keys=True)))
                    else:
                        lines.append("  No active surface model (current result is not a victim).")
                    plaque_lines = self._victim_plaque_debug_lines(result)
                    lines.extend("  " + row for row in plaque_lines)
                    if not plaque_lines:
                        lines.append("  No plaque-depth measurement.")

                    internal_lines = list((result.get("raw_result") or {}).get("debug_lines") or [])
                    lines.append("\nInternal candidate processing log:")
                    lines.extend("  " + row for row in internal_lines)
                    if not internal_lines:
                        lines.append("  No skipped-candidate or processing errors.")

                    snapshot = {
                        "result": result,
                        "active_debug_return": summary,
                        "projection_metadata": metadata.get(side),
                    }
                    lines.append("\nStructured detector snapshot:")
                    lines.append(json.dumps(self._alignment_debug_value(snapshot), indent=2,
                                            sort_keys=True, ensure_ascii=False))
                lines.append("")
                return "\n".join(lines)

            def _capture_diagnostics(self, data, results):
                """Add expensive comparisons to a snapshot without changing live decisions."""
                if 'lidar' not in data and data.get('lidar_raw_m') is not None:
                    data = dict(data, lidar=np.asarray(data['lidar_raw_m'])*1000.)
                snapshot = copy.deepcopy(results)
                for side, entry in snapshot.items():
                    result = entry.get('result')
                    buffer = (data.get('cameras') or {}).get(side)
                    if result is None or buffer is None:
                        continue
                    frame = self._camera_frame(data,side)
                    if result.get('kind') == 'victim':
                        try:
                            surface = self._victim_surface_shadow(data, side, result)
                            result['fake_victim']['active_surface'] = surface
                            result['plaque_depth_debug'] = self._victim_plaque_depth_debug(frame, data, side, result)
                        except Exception as exc:
                            result['diagnostic_error'] = '%s: %s' % (type(exc).__name__, exc)
                    elif self.SECONDARY_TARGET_ENABLED and not result.get('new_only_debug'):
                        result['new_only_debug'] = self._new_only_debug_analysis(frame, data, side, result)
                return snapshot

            def _save_lidar_alignment(self, data):
                """Camera frames remain in RAM, including with DEBUG enabled."""
                return None

            def _victim_plaque_debug_lines(self, result):
                check = result.get("plaque_depth_debug")
                if not check:
                    return []
                lines = ["[PLAQUE-DEBUG] status=%s black_n=%s white_n=%s relief_mm=%s p80_mm=%s alignment=%s report_unchanged=True reason=%s" %
                         (check["status"], check["black_n"], check["white_n"],
                          None if check["median_mm"] is None else round(check["median_mm"], 3),
                          None if check["p80_mm"] is None else round(check["p80_mm"], 3),
                          check["alignment"], check["reason"])]
                if check.get("calibration_source"):
                    lines.append("[PLAQUE-ALIGN] source=%s camera_offset_mm=%s visible_rays=%s overlay_key=L" %
                                 (check["calibration_source"],
                                  [round(1000*v, 2) for v in check["translation_m"]],
                                  len(check.get("projected_points", []))))
                if check.get("metadata_matches_profile") is False:
                    lines.append("[PLAQUE-ALIGN] profile_metadata_mismatch live_camera=%s using_live_intrinsics=True" %
                                 check.get("live_camera"))
                if "rays_in_bbox" in check:
                    lines.append("[PLAQUE-COVERAGE] rays_in_bbox=%s black=%s white=%s" %
                                 (check["rays_in_bbox"], check["black_n"], check["white_n"]))
                for row in check["layers"]:
                    lines.append("[PLAQUE-LAYER] layer=%s black=%s white=%s relief_mm=%s white_error_mm=%s reason=%s" %
                                 (row["layer"], row["black_n"], row["white_n"],
                                  None if row["median_mm"] is None else round(row["median_mm"], 3),
                                  None if row["white_error_mm"] is None else round(row["white_error_mm"], 3), row["reason"]))
                return lines

            # Visual victim validation.

            def _victim_overhang_debug(self, frame, victim_bbox=None):
                """Find a white plaque and measure dark pixels extending below it.

        The raised fake symbol can cross the plaque's lower image boundary
        when viewed from above. The active visual gate uses these measurements.
        """
                out = {"status": "no-plaque", "bbox": None, "black_below": 0,
                       "overhang_px": 0, "below_density": 0.0,
                       "below_max_width": 0, "decision": "unknown"}
                hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
                white = cv2.inRange(hsv, (0, 0, 170), (180, 60, 255))
                count, _, stats, _ = cv2.connectedComponentsWithStats(white, 8)
                height, width = white.shape
                candidates = []
                for idx in range(1, count):
                    x, y, w, h, area = map(int, stats[idx])
                    # A close plaque can occupy more than65% of the frame.
                    # Its measured borders, overlap and overhang remain required.
                    if (area >= 50 and w >= 10 and h >= 10
                            and x >= 2 and y >= 2 and x + w <= width - 2
                            and y + h <= height - 2 and .55 <= w / h <= 1.8):
                        if victim_bbox is not None:
                            vx, vy, vw, vh = victim_bbox
                            intersection = max(0, min(x+w, vx+vw)-max(x, vx)) * max(0, min(y+h, vy+vh)-max(y, vy))
                            if intersection / max(1, w*h + vw*vh - intersection) < .4:
                                continue
                        candidates.append((area, x, y, w, h))
                if not candidates:
                    return out
                _, x, y, w, h = max(candidates)
                out["bbox"] = (x, y, w, h)
                gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
                # Separate the black glyph from a darker wall only when the
                # measured local dark pixels contain two supported clusters.
                # A raised black continuation keeps the same ink intensity;
                # an unresolved histogram retains the original conservative gate.
                cutoff=55
                region=gray[y:min(height,y+h+8),max(0,x-2):min(width,x+w+2)]
                samples=region[region<=55]
                if len(samples)>=20:
                    split,_=cv2.threshold(samples.reshape(-1,1),0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU)
                    low=samples[samples<=split];high=samples[samples>split]
                    if len(low)>=10 and len(high)>=10 and float(high.mean()-low.mean())>=8.:
                        cutoff=int(split)
                out['ink_threshold']=cutoff
                dark = (gray <= cutoff).astype(np.uint8)
                dark_count, dark_labels, dark_stats, _ = cv2.connectedComponentsWithStats(dark, 8)
                best = None
                for idx in range(1, dark_count):
                    bx, by, bw, bh, size = map(int, dark_stats[idx])
                    if size < 10 or bx >= x + w or bx + bw <= x or by >= y + h or by + bh <= y:
                        continue
                    inside = int(np.count_nonzero(dark_labels[y:y+h, x:x+w] == idx))
                    if inside < 10:
                        continue
                    if best is None or inside > best[0]:
                        best = (inside, idx)
                if best is None:
                    out["status"] = "no-linked-dark-symbol"
                    return out
                _, idx = best
                lower = dark_labels[y+h:min(height, y+h+8), max(0, x-2):min(width, x+w+2)] == idx
                ys = np.flatnonzero(np.any(lower, axis=1))
                out["black_below"] = int(np.count_nonzero(lower))
                out["overhang_px"] = int(ys.max() + 1) if ys.size else 0
                out["below_density"] = float(out["black_below"] / max(1, lower.size))
                out["below_max_width"] = int(np.max(np.count_nonzero(lower, axis=1))) if lower.size else 0
                out["status"] = "measured"
                # A raised fake produces a narrow continuation of the symbol. If most
                # of this strip is dark, the symbol is connected to dark background;
                # that broad region is not evidence of a local protrusion.
                broad_background = (out["below_density"] >= .55
                                    or out["below_max_width"] >= .80 * max(1, lower.shape[1]))
                if broad_background:
                    out["decision"] = "no-localized-overhang"
                elif out["overhang_px"] >= 3 and out["black_below"] >= 10:
                    out["decision"] = "fake-candidate"
                elif out["overhang_px"] > 0:
                    out["decision"] = "weak-overhang/unknown"
                else:
                    out["decision"] = "no-visible-overhang"
                return out

            def _apply_victim_visual_check(self, result, data, side):
                """Experimental acceptance: stable recognized symbol with no overhang.

        Absence of overhang is not proof of a flush symbol. Keep this limitation
        explicit in the live decision; weak or unavailable evidence blocks.
        """
                tracks = getattr(self, '_victim_visual_tracks', {})
                self._victim_visual_tracks = tracks
                if result.get('kind') != 'victim':
                    tracks.pop(side, None)
                    return result
                check = result.get('overhang_debug') or {}
                bbox = check.get('bbox')
                now = float(data.get('time', 0))
                previous = tracks.get(side)
                same = False
                if previous and bbox and previous['bbox']:
                    x, y, w, h = bbox
                    px, py, pw, ph = previous['bbox']
                    overlap = max(0, min(x+w, px+pw)-max(x, px)) * max(0, min(y+h, py+ph)-max(y, py))
                    same = (0 < now-previous['time'] <= .35 and previous['label'] == result.get('label')
                            and overlap / max(1, w*h+pw*ph-overlap) >= .6)
                state = previous if same else {'start': now, 'count': 0, 'fake_until': -1}
                state.update(time=now, bbox=bbox, label=result.get('label'))
                tracks[side] = state
                decision = check.get('decision', 'unknown')
                raw = result.get('raw_result') or {}
                eligible = bool(raw.get('reportable', False))
                if decision == 'fake-candidate':
                    state['fake_until'] = now + 1.0
                clear = (decision in ('no-visible-overhang', 'no-localized-overhang')
                         and check.get('status') == 'measured'
                         and bbox is not None and min(bbox[2:]) >= 16 and eligible)
                if not clear or now < state['fake_until']:
                    state.update(start=now, count=0)
                else:
                    state['count'] += 1
                accepted = clear and state['count'] >= 4 and now-state['start'] >= .35 and now >= state['fake_until']
                fake_seen = decision == 'fake-candidate' or now < state['fake_until']
                reason = ('visible symbol overhang' if fake_seen else
                          'experimental visual acceptance; no localized overhang, stable recognized symbol' if accepted else
                          'waiting for stable, sufficiently large plaque without overhang')
                fake = dict(result.get('fake_victim') or {})
                fake.update(enabled=True, is_real=False if fake_seen else True if accepted else None,
                            block_report=not accepted, reason=reason, decision_mode='visual-overhang')
                report = self._make_report('victim', result.get('label'), eligible, fake_victim=fake)
                result.update(fake_victim=fake, report=report, reportable=bool(report['should_report']))
                result['visual_gate'] = {'accepted': accepted, 'reason': reason}
                lines = [line for line in result.get('log_lines', []) if not line.startswith('[REPORT]')]
                lines = [line.replace('[SURFACE-ACTIVE]', '[SURFACE-DEBUG]') for line in lines]
                lines.append('[VISUAL-ACTIVE] decision=%s block=%s frames=%s reason=%s' %
                             ('fake' if fake_seen else 'accepted-experimental' if accepted else 'unknown',
                              not accepted, state['count'], reason))
                lines.append('[REPORT] should_report=%s kind=victim label=%s reason=%s' %
                             (report['should_report'], result.get('label'), report['reason']))
                result['log_lines'] = lines
                return result

            # Movement-facing metadata and partial centers.

            def _active_debug_return(self, result, data):
                """Describe identity and image centre without steering or report changes."""
                def pair(value):
                    try:
                        if value is None or len(value) != 2:
                            return None
                        values = [float(v) for v in value]
                        return [round(v, 2) for v in values] if all(map(math.isfinite, values)) else None
                    except (TypeError, ValueError, OverflowError):
                        return None

                def supported(candidate):
                    if candidate.get('kind') == 'target':
                        bands = candidate.get('bands') or []
                        return ((candidate.get('structure') or {}).get('ncolors', 0) >= 2
                                or (len(bands) == 5 and len(set(bands)) >= 2
                                    and float(candidate.get('read_conf') or 0) >= self.COG_BAND_CONF_MIN))
                    if candidate.get('kind') == 'victim':
                        template = candidate.get('template') or {}
                        return ((candidate.get('token') or {}).get('reason') == 'token'
                                or (template.get('label_guess') in self.VICTIM_NAMES
                                    and float(template.get('iou') or 0) >= self.LETTER_IOU_MIN))
                    return False

                raw = result.get('raw_result') or {}
                best = raw.get('best') or {}
                report = result.get('report') or {}
                policy = result.get('target_policy') or {}
                source = policy.get('source', 'legacy')
                kind, label = result.get('kind'), result.get('label')
                center, bbox = result.get('center'), best.get('bbox')
                identity = self._save_code(kind, label) if kind == 'victim' else label
                eligible = bool(report.get('should_report'))
                status = 'READY' if eligible else 'DETECTED' if kind else 'NO_DETECTION'
                center_type, center_source = 'DETECTED', 'detector'
                if policy.get('source') == 'secondary_pending':
                    kind, label, identity = 'target', None, None
                    center, bbox = policy.get('center'), policy.get('bbox')
                    best = {}
                    status, center_type, center_source = 'VERIFYING', 'ESTIMATED', 'secondary_geometry'
                elif not kind and report.get('fake_target') is not None:
                    kind, identity, status = 'target', 'FAKE', 'REJECTED'
                    center = report['fake_target']
                    candidates = (raw.get('target') or {}).get('candidates') or []
                    if candidates and pair(center):
                        best = min((c for c in candidates if pair(c.get('center'))),
                                   key=lambda c: math.dist(c['center'], center), default={})
                        bbox = best.get('bbox')
                elif not kind:
                    candidates = [c for group in ('victim', 'target')
                                  for c in (raw.get(group) or {}).get('candidates', [])
                                  if supported(c) and pair(c.get('center'))]
                    secondary = (result.get('new_only_debug') or {}).get('result') or {}
                    secondary_best = (secondary.get('raw_result') or {}).get('best') or {}
                    secondary_check = (secondary_best.get('observation') or {}).get('new_structure_check') or {}
                    if (secondary.get('kind') == 'target' and secondary_best.get('accepted')
                            and secondary_check.get('status') == 'PASS' and pair(secondary_best.get('center'))):
                        # Localize a validated view even when the final report policy
                        # still withholds an identity. Do not promote its label here.
                        candidates.append(secondary_best)
                    if candidates:
                        best = max(candidates, key=lambda c: (
                            ((c.get('observation') or {}).get('new_structure_check') or {}).get('status') == 'PASS',
                            float(c.get('confidence') or 0)))
                        kind, center, bbox = best['kind'], best['center'], best.get('bbox')
                        status, center_type, center_source = 'CANDIDATE', 'ESTIMATED', 'candidate_geometry'
                        check = (best.get('observation') or {}).get('new_structure_check') or {}
                        if check.get('status') == 'PASS':
                            center = pair(check.get('sampling_center_px')) or center
                            center_source = 'validated_candidate_geometry'
                    else:
                        partial = secondary.get('partial_geometry_debug') or {}
                        if len(set(partial.get('initial_bands') or [])) >= 3 and pair(partial.get('core_center')):
                            kind, center = 'target', partial['core_center']
                            bbox = None
                            best = {'partial': bool(partial.get('clipped'))}
                            status, center_type, center_source = 'CANDIDATE', 'ESTIMATED', 'visible_target_core'
                width, height = raw.get('frame_size') or (self.cam_w, self.cam_h)
                partial = bool(best.get('edge') or best.get('partial'))
                if bbox:
                    x, y, w, h = bbox
                    partial |= x <= 0 or y <= 0 or x+w >= width or y+h >= height
                if partial and center_type == 'DETECTED':
                    center_type = 'ESTIMATED' if kind == 'target' else 'VISIBLE'
                if kind == 'victim' and (result.get('fake_victim') or {}).get('is_real') is False:
                    status = 'REJECTED'
                center = pair(center)
                colors = []
                if kind == 'target':
                    if center_source == 'secondary_geometry':
                        secondary = (result.get('new_only_debug') or {}).get('result') or {}
                        color_candidate = ((secondary.get('raw_result') or {}).get('best') or {})
                        colors = list(color_candidate.get('bands') or [])
                    elif center_source == 'visible_target_core':
                        secondary = (result.get('new_only_debug') or {}).get('result') or {}
                        partial_geometry = secondary.get('partial_geometry_debug') or {}
                        colors = list(partial_geometry.get('initial_bands') or [])
                    else:
                        colors = list(best.get('bands') or [])
                report_types = (self.VICTIM_NAMES if kind == 'victim' else
                                self.TARGET_NAMES if kind == 'target' else {})
                report_label = label if eligible and label in report_types else None
                # A full recognized plaque may be too small to resolve fake
                # relief. Preserve only a moving-look hint, never an identity
                # vote, map stamp, remembered report or permission to pause.
                overhang = result.get('overhang_debug') or {}
                look_hint = bool(kind == 'victim' and label in self.VICTIM_NAMES
                    and status != 'REJECTED' and not partial and center_type == 'DETECTED'
                    and best.get('accepted')
                    and overhang.get('decision') != 'fake-candidate'
                    and (result.get('fake_victim') or {}).get('is_real') is not False)
                return {'inspection_hint': look_hint,
                        'kind': kind, 'label': label, 'id': identity, 'report_label': report_label,
                        'save_code': self._save_code(result.get('kind'), result.get('label')),
                        'status': status, 'can_report': eligible, 'source': source,
                        'image_center_px': center, 'center_type': center_type if center else None,
                        'center_source': center_source if center else None,
                        'colors_inner_to_outer': colors,
                        'partial': bool(partial),
                        'center_in_frame': bool(0 <= center[0] < width and 0 <= center[1] < height)
                                           if center else None,
                        'bbox_px': [round(float(v), 2) for v in bbox] if bbox else None}

            def _new_only_debug_analysis(self, frame, data, side, active):
                """Analyze a secondary view with separate geometry and visual history.

        The final target policy may accept this result after verification.
        Keep the historical method name for caller compatibility.
        """
                try:
                    contexts = getattr(self, "_new_only_debug_contexts", {})
                    self._new_only_debug_contexts = contexts
                    if side not in contexts:
                        context = TargetVictimDetector(_secondary=True, camera_size=(self.cam_w, self.cam_h),
                                             camera_fov=self.cam_fov)
                        context._victim_visual_tracks = {}
                        contexts[side] = context
                    context = contexts[side]
                    # Both decoders use the identical primary pixel fit. The
                    # secondary's additional fit and all histories stay separate.
                    context._primary_geometry_cache=getattr(self,'_primary_geometry_cache',{})
                    new = context.analyze_target_victim_frame(frame)
                    if new.get("kind") == "victim" and getattr(self, "VICTIM_VISUAL_ACTIVE", False):
                        bbox = ((new.get("raw_result") or {}).get("best") or {}).get("bbox")
                        new["overhang_debug"] = self._victim_overhang_debug(frame, bbox)
                        new = context._apply_victim_visual_check(new, data, side)
                    elif new.get("kind") != "victim":
                        context._victim_visual_tracks.pop(side, None)
                    would_report = bool((new.get("report") or {}).get("should_report"))
                    old_report = active.get("report") or {}
                    old_key = (active.get("kind"), active.get("label"), bool(old_report.get("should_report")))
                    new_key = (new.get("kind"), new.get("label"), would_report)
                    old_bands = (((active.get("raw_result") or {}).get("best") or {}).get("bands") or [])
                    new_bands = (((new.get("raw_result") or {}).get("best") or {}).get("bands") or [])
                    # Hypothetical eligibility is stored explicitly. No shadow result
                    # has a reportable flag that a caller could accidentally emit.
                    new["report"]["should_report"] = False
                    new["reportable"] = False
                    return {"time": float(data.get("time", 0)), "would_report": would_report,
                            "sending_enabled": False,
                            "comparison": "SAME" if old_key == new_key and old_bands == new_bands else "DIFFERENT",
                            "active": {"kind": old_key[0], "label": old_key[1], "eligible": old_key[2]},
                            "result": new}
                except Exception as exc:
                    return {"error": "%s: %s" % (type(exc).__name__, exc),
                            "sending_enabled": False, "comparison": "ERROR", "would_report": False}

            def _new_only_debug_lines(self, result, side):
                check = result.get("new_only_debug")
                if not check:
                    return []
                prefix = "[SECONDARY] [%s]" % side.upper()
                if check.get("error"):
                    return ["%s status=ERROR | %s | active_unchanged=True" % (prefix, check["error"])]
                new = check.get("result") or {}
                raw = new.get("raw_result") or {}
                best = raw.get("best") or {}
                kind, label = new.get("kind"), new.get("label")
                if not best:
                    rejected = ((raw.get("target") or {}).get("candidates") or [])
                    if rejected:
                        best = max(rejected, key=lambda c: float(c.get("confidence") or 0))
                active = check.get("active") or {}
                comparison = ("active=%s %s/%s | new=%s %s/%s | comparison=%s | sending=OFF" %
                              (active.get("kind") or "none", active.get("label") or "-",
                               "READY" if active.get("eligible") else "BLOCKED",
                               kind or "none", label or "-",
                               "READY" if check.get("would_report") else "BLOCKED", check["comparison"]))
                if kind == "victim":
                    victim = new.get("victim_debug") or {}
                    lines = ["%s VICTIM %s | centered=%s confidence=%s | shape=%s" %
                             (prefix, label, victim.get("centered"), new.get("confidence"),
                              victim.get("soft_reason", "unknown"))]
                    lines.append("%s CHECK | %s" % (prefix, (new.get("report") or {}).get("reason")))
                elif best.get("kind") == "target":
                    bands = "-".join({"red": "R", "green": "G", "blue": "B", "yellow": "Y", "black": "K"}
                                     .get(color, "?") for color in best.get("bands", []))
                    structure = (best.get("observation") or {}).get("new_structure_check") or {}
                    state = "ACCEPTED" if kind == "target" else (
                        "WAITING" if structure.get("status") == "WAITING" else "REJECTED")
                    lines = ["%s TARGET %s | status=%s | colors=%s confidence=%.3f" %
                             (prefix, label or best.get("label_guess") or "-",
                              state, bands,
                              float(best.get("confidence") or 0))]
                    lines.append("%s CHECK | %s" %
                                 (prefix, structure.get("reason") or best.get("reason") or "no read"))
                else:
                    return ["%s No detection | %s" % (prefix, comparison)]
                lines.append("%s %s" % (prefix, comparison))
                return lines

            # Shared per-camera pipeline and compact return.

            def _analyze_target_victim_sides(self, data):
                """Analyze both cameras once and share results with reporting/debug."""
                if getattr(self,'_external_frames',False):return self._target_victim_cache
                now = float(data.get("time", 0.0))
                if (
                    self._target_victim_cache
                    and now >= self._target_victim_cache_t
                    and now - self._target_victim_cache_t < self.TARGET_VICTIM_ANALYZE_PERIOD_S-1e-9
                ):
                    return self._target_victim_cache

                results = {}
                for side in ("left", "right"):
                    buf = data["cameras"].get(side)
                    if buf is None:
                        results[side] = {"error": "no frame", "result": None}
                        continue
                    try:
                        frame = self._camera_frame(data,side)
                        result = self.analyze_target_victim_frame(
                            frame,
                            enable_fake_victim_check=True,
                            lidar_ranges=self._side_lidar_ranges_m(data, side),
                            lidar_fov_rad=float(data.get("lidar_fov", 2.0 * math.pi)),
                            camera_fov_rad=float(data.get("cam_fov", self.cam_fov)),
                            camera_width_px=self.cam_w,
                        )
                        if self.VICTIM_OVERHANG_DEBUG or getattr(self, 'VICTIM_VISUAL_ACTIVE', False):
                            bbox = ((result.get('raw_result') or {}).get('best') or {}).get('bbox') if result.get('kind') == 'victim' else None
                            result["overhang_debug"] = self._victim_overhang_debug(frame, bbox)
                        if not self.VICTIM_VISUAL_ACTIVE:
                            result = self._apply_victim_wall_geometry_check(result, data, side)
                        if getattr(self, 'VICTIM_VISUAL_ACTIVE', False):
                            result = self._apply_victim_visual_check(result, data, side)
                        if self.debug_enabled and self.VICTIM_PLAQUE_DEBUG and result.get("kind") == "victim":
                            try:
                                result["plaque_depth_debug"] = self._victim_plaque_depth_debug(frame, data, side, result)
                            except Exception as exc:
                                result["plaque_depth_debug"] = {
                                    "status": "error", "decision": "unknown", "alignment": "unverified",
                                    "black_n": 0, "white_n": 0, "median_mm": None, "p80_mm": None,
                                    "layers": [], "reason": "%s: %s" % (type(exc).__name__, exc)}
                        comparison = None
                        if self.SECONDARY_TARGET_ENABLED and result.get('kind') is None:
                            result["new_only_debug"] = self._new_only_debug_analysis(frame, data, side, result)
                            comparison = result['new_only_debug']
                        result = self._fuse_target(result, comparison, side, now)
                        results[side] = {"error": None, "result": result}
                    except Exception as exc:
                        import traceback
                        tb = traceback.extract_tb(exc.__traceback__)
                        where = " <- ".join(f"{fr.name}:{fr.lineno}" for fr in tb[-3:])
                        results[side] = {
                            "error": "%s | at %s" % (exc, where),
                            "result": None,
                        }
                self._target_victim_cache_t = now
                self._target_victim_cache = results
                self._target_victim_capture_data = {key:(data[key].copy() if isinstance(data[key], np.ndarray) else copy.deepcopy(data[key])) for key in (
                    'time','position','orientation','camera_intrinsics','cameras',
                    'lidar_raw_m','lidar_hres','lidar_fov','lidar_vertical_fov','lidar_vfov','lidar_layers',
                    'lidar_min_range_m','lidar_max_range_m','cam_w','cam_h','cam_fov') if key in data} if self.debug_enabled else {}
                return results

            def get_detection_return(self, data):
                """Return a dictionary for robot code; console text is display only.

        Read observations[side]: kind, report_label and image_center_px.
        report_label is the eligible Erebus character, otherwise None.
        No packet is sent here; report_output_enabled describes the send switch.
        """
                results = self._analyze_target_victim_sides(data)
                frame_time = float(self._target_victim_cache_t)
                payload = {'schema_version': 3, 'time_s': frame_time,
                           'valid_until_s': round(frame_time+self.FINAL_RETURN_VALID_FOR_S, 6),
                           'report_output_enabled': bool(self.AUTO_REPORT_ENABLED), 'observations': {}}
                for side in ('left', 'right'):
                    entry = results.get(side) or {}
                    result = entry.get('result')
                    if result is None or entry.get('error'):
                        summary = {'kind': None, 'id': None, 'label': None, 'report_label': None,
                                   'save_code': None,
                                   'status': ('UNAVAILABLE' if entry.get('error') == 'no frame' else
                                              'ERROR' if entry.get('error') else 'PENDING'),
                                   'can_report': False, 'source': 'none', 'image_center_px': None,
                                   'center_type': None, 'center_source': None, 'partial': False,
                                   'colors_inner_to_outer': [],
                                   'center_in_frame': None, 'bbox_px': None,
                                   'error':entry.get('error')}
                    else:
                        summary = self._active_debug_return(result, data)
                    summary['side'] = side
                    payload['observations'][side] = summary
                return payload

            @staticmethod
            def _compact_detection_return(payload):
                """Format camera, type, eligible Erebus report and pixel center."""
                rows = []
                for side, row in (payload.get('observations') or {}).items():
                    center = row.get('image_center_px')
                    if center is None:
                        if row.get('status') == 'ERROR':
                            rows.append('%s: center=unknown [ERROR]' % side.upper())
                        continue
                    kind = row.get('kind')
                    if kind not in ('target', 'victim'):
                        continue
                    report_label = row.get('report_label') or 'NONE'
                    point = '('+', '.join('%g' % round(float(v), 1) for v in center)+')'
                    rows.append('%s: type=%s, report=%s, center=%s px' %
                                (side.upper(), kind, report_label, point))
                return 'RETURN | '+('; '.join(rows) if rows else 'No center')

            def debug_target_victim(self, data):
                """Print one compact detection line; return the complete movement payload."""
                payload = self.get_detection_return(data)
                if self.debug_enabled:
                    sys.stdout.write('\033[34m'+self._compact_detection_return(payload)+'\033[0m\n')
                    sys.stdout.flush()
                return payload

            def save_alignment_capture(self, data):
                """Save both images, scan and detailed detection decisions."""
                return self._save_lidar_alignment(data) if self.debug_enabled else None

        class _Engine(TargetVictimDetector):
            """Hero sensor adapter, localization, verification queue and report gates.

    Recognition is inherited unchanged. Call every control tick, pass the actual
    robot.json explicitly, and send only packets returned by report_packet().
    No motors, map cells, planner destinations or supervisor nodes are touched.
    """
            REPORT_RADIUS_M = .06  # 2026 rules half-tile; installed judge allows .09.
            STOP_DURATION_S = 1.1
            TRACK_TTL_S = 30.0
            MAX_TRACKS = 128

            def __init__(self, robot_json, *, debug_enabled=False, **kwargs):
                if not isinstance(robot_json, dict):
                    with open(robot_json, encoding='utf-8') as stream:
                        robot_json = json.load(stream)
                super().__init__(robot_json, **kwargs)
                self.debug_enabled = bool(debug_enabled)
                gps = next((d for d in robot_json.values() if d.get('customName') == 'gps'), None)
                if gps is None:
                    raise ValueError('localization requires the GPS device in robot.json')
                self._gps_offset = np.clip([gps[k] for k in ('x','y','z')],
                                           [-370,-100,-370], [370,370,370])/10000
                self._gps_offset[1] += .0185
                self._tracks = {}
                self.token_map = getattr(self.mapper, "token_map", None)
                if self.token_map is None:
                    raise ValueError("integrated RescueVision requires a mapper with token_map")
                self._associations = {}
                self._next_track = 1
                self._motion_previous = None
                self._stopped_since = None
                self._snapshot = None
                self._located_time = None
                self._projection_cache = {}
                self._locations = {}
                self._latest_payload = None
                # Import the existing secondary solver before the control loop. Native
                # profiling found its first lazy import stalls a camera tick >200 ms.
                if self.SECONDARY_TARGET_ENABLED:
                    self._ring_fit_solver()

            @staticmethod
            def _attitude(data):
                """Same Webots NUE YZX Euler convention as Hero.read_all_sensors."""
                o = data['orientation']
                yaw = math.radians(float(o['yaw_raw']))
                roll, pitch = float(o['roll']), float(o['pitch'])
                if not all(map(math.isfinite, (yaw, roll, pitch))):
                    raise ValueError('nonfinite orientation')
                return (TargetVictimDetector.axis_rotation((0,1,0,yaw))
                        @ TargetVictimDetector.axis_rotation((0,0,1,pitch))
                        @ TargetVictimDetector.axis_rotation((1,0,0,roll)))

            def _navigation_xy(self, world_xz_m):
                """Hero planner coordinates are relative to the mapper's GPS origin."""
                mapper = self.mapper
                if (mapper is None or getattr(mapper, 'origin_x', None) is None
                        or getattr(mapper, 'origin_y', None) is None):
                    return None
                x,z = world_xz_m
                return [x*1000-mapper.origin_x,
                        getattr(mapper, 'GPS_Y_SIGN', -1)*z*1000-mapper.origin_y]

            def _body_pose(self, data):
                attitude = self._attitude(data)
                position = data['position']
                gps = np.asarray(position['raw_xyz_m'], float).copy()
                raw_gps = gps.copy()
                position_source = 'raw-gps'
                sigma = .0025  # installed GPS accuracy, metres; not a hard bound
                if position.get('map_physical_pose_ready'):
                    gps[[0,2]] = [position['map_x_float']/1000, position['map_y_float']/1000]
                    sigma = float(position['map_position_sigma_mm'])/1000
                    position_source = 'hero-physical-gps-filter'
                elif position.get('map_pose_ready'):
                    gps[[0,2]] = [position['map_x']/1000, position['map_y']/1000]
                    sigma = .0025/math.sqrt(49)+.0005
                    position_source = 'hero-mapping-gps-filter'
                if (gps.shape != (3,) or not np.isfinite(gps).all() or not np.isfinite(raw_gps).all()
                        or not math.isfinite(sigma) or sigma < 0):
                    raise ValueError('missing finite world GPS pose')
                # Hero's seven-sigma registration convention, now checked against
                # the current independent raw GPS sample. A ready flag alone cannot
                # certify a filter after relocation or an interrupted sensor stream.
                if np.linalg.norm((gps-raw_gps)[[0,2]]) > 7*math.hypot(.0025,sigma):
                    gps = raw_gps; sigma = .0025
                    position_source = 'raw-gps/filter-disagreement'
                body = gps-attitude @ self._gps_offset
                return attitude, body, sigma, position_source

            def _curved_surface_hit(self, data, side, points, layers, ray):
                """Validate a measured vertical arc after the local plane fails."""
                origin,rotation=self._camera_world(data,side)
                xy=(points@rotation.T+origin)[:,[0,2]]
                direction=(rotation@ray)[[0,2]];start=origin[[0,2]]
                def fit(samples):
                    mean=samples.mean(axis=0);local=samples-mean
                    coefficients,_,rank,_=np.linalg.lstsq(np.column_stack((2*local,np.ones(len(local)))),
                                                        np.sum(local*local,axis=1),rcond=None)
                    squared=coefficients[2]+coefficients[:2]@coefficients[:2]
                    if rank<3 or squared<=0:raise ValueError('unresolved curved surface')
                    centre=mean+coefficients[:2];radius=math.sqrt(squared)
                    residual=abs(np.linalg.norm(samples-centre,axis=1)-radius)
                    if np.max(residual)>.002:raise ValueError('curved surface discontinuity')
                    delta=start-centre
                    roots=np.roots([direction@direction,2*delta@direction,delta@delta-radius*radius])
                    roots=[float(t.real)for t in roots if abs(t.imag)<1e-8 and t.real>0]
                    if not roots:raise ValueError('camera ray misses measured arc')
                    scale=min(roots);radial=(start+scale*direction-centre)/radius
                    relative=samples-centre
                    angles=np.arctan2(relative[:,0]*radial[1]-relative[:,1]*radial[0],relative@radial)
                    if not(angles.min()<=0<=angles.max() and np.ptp(angles)<math.pi):
                        raise ValueError('camera ray outside measured arc')
                    return scale,radial,residual,centre,radius
                scale,radial,residual,centre,_=fit(xy)
                # Interleaved angular sets cover the same observed arc with
                # different raw returns and multiple layers on each side.
                order=np.argsort(np.arctan2(points[:,0],points[:,2]),kind='stable')
                groups=(order[::2],order[1::2])
                for training,heldout in(groups,groups[::-1]):
                    if (len(training)<6 or len(heldout)<6 or
                            len(np.unique(layers[training]))<2 or len(np.unique(layers[heldout]))<2):
                        raise ValueError('insufficient independent curved support')
                    other,_,_,other_centre,other_radius=fit(xy[training])
                    error=abs(np.linalg.norm(xy[heldout]-other_centre,axis=1)-other_radius)
                    if np.max(error)>.002 or abs(other-scale)*np.linalg.norm(direction)>.004:
                        raise ValueError('independent curved samples disagree')
                    residual=np.r_[residual,error]
                incidence=abs(radial@direction)/np.linalg.norm(ray)
                if incidence<.25:raise ValueError('curved surface is too grazing')
                normals=(xy-centre)/np.linalg.norm(xy-centre,axis=1)[:,None]
                jacobian=np.column_stack((-normals,-np.ones(len(xy))))
                gradient=np.r_[radial,1.]/abs(radial@direction)
                leverage=max(1.,math.sqrt(float(gradient@np.linalg.pinv(jacobian.T@jacobian)@gradient)))
                normal=rotation.T@np.array([radial[0],0.,radial[1]])
                return ray*scale,normal,residual,incidence,leverage

            def _metric_target_location(self, data, side, old_location):
                try:
                    result = (self._target_victim_cache.get(side) or {}).get('result') or {}
                    best = (result.get('raw_result') or {}).get('best') or {}
                    bands = best.get('bands') or []
                    if result.get('kind') != 'target' or len(bands) != 5:
                        return None
                    frame = self._camera_frame(data, side)
                    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
                    height, width = frame.shape[:2]
                    focal = width/(2*math.tan(self.cam_fov/2))
                    names = ('red', 'green', 'blue', 'yellow', 'black', 'unknown')
                    hue, sat, value = cv2.split(hsv)
                    labels = np.full((height, width), 5, np.uint8)
                    for color, mask in enumerate(((hue < 10) | (hue >= 170), (hue >= 40) & (hue < 85),
                                                  (hue >= 100) & (hue < 130), (hue >= 20) & (hue < 35))):
                        labels[mask & (sat >= self.GRAY_S_MAX)] = color
                    labels[value < self.BLACK_V_MAX] = 4
                    choices = {}
                    for index in range(1, 5):
                        if bands[index-1] != bands[index]:
                            pair = tuple(sorted((names.index(bands[index-1]), names.index(bands[index]))))
                            choices.setdefault(pair, []).append(index)
                    for background in (4,5):
                        outer_color=names.index(bands[-1])
                        if outer_color!=background:
                            choices.setdefault(tuple(sorted((outer_color,background))),[]).append(5)
                    if len(choices) < 2:
                        return None
                    x0, y0, bw, bh = best['bbox']
                    points, levels = [], []
                    for dy, dx in ((0, 1), (1, 0)):
                        first = labels[:height-dy, :width-dx]
                        second = labels[dy:, dx:]
                        yy, xx = np.nonzero(first != second)
                        for y, x in zip(yy, xx):
                            if not (x0 <= x+dx/2 <= x0+bw and y0 <= y+dy/2 <= y0+bh):
                                continue
                            pair = tuple(sorted((int(first[y, x]), int(second[y, x]))))
                            if pair in choices:
                                points.append((x+dx/2, y+dy/2))
                                levels.append(choices[pair])
                    if len(points) < 32:
                        return None
                    points = np.asarray(points, float)
                    rays = np.column_stack(((points[:,0]-(width-1)/2)/focal,
                                            (points[:,1]-(height-1)/2)/focal, np.ones(len(points))))
                    origin, ruf = self._camera_world(data, side)
                    rdf = ruf @ np.diag([1., -1., 1.])
                    up = rdf.T @ np.array([0., 1., 0.])
                    nx, nz = old_location['surface_normal_xz']
                    normal0 = rdf.T @ np.array([nx, 0., nz])
                    normal0 /= np.linalg.norm(normal0)
                    tangent0 = np.cross(up, normal0)
                    tangent0 /= np.linalg.norm(tangent0)
                    initial = rdf.T @ (np.asarray(old_location['world_xyz_m'])-origin)
                    # Native Erebus26.1 uses a40mm plane and a1024px generated
                    # texture: radius512, decremented by integer102 per band.
                    # This is asset calibration, not the rules document's nominal50mm.
                    side_m=.04;texture_size=1024;outer_radius_px=512;ring_width_px=102
                    radii = np.asarray([(outer_radius_px-ring_width_px*(5-index))*side_m/texture_size for index in range(1, 6)])
                    def geometric(parameters):
                        center = parameters[:3]
                        normal = math.cos(parameters[3])*normal0+math.sin(parameters[3])*tangent0
                        denominator = np.sum(rays*normal,axis=1)
                        safe = np.where(abs(denominator) > .05, denominator, np.copysign(.05, denominator))
                        distance = normal @ center
                        projected = rays*(distance/safe)[:,None]
                        delta = projected-center
                        radial = np.linalg.norm(delta, axis=1)
                        gradients = []
                        for axis in (0, 1):
                            basis = np.zeros(3); basis[axis] = 1/focal
                            dp = distance*(basis[None,:]*safe[:,None]-rays*(normal[axis]/focal))/(safe[:,None]**2)
                            gradients.append(np.sum(delta*dp,axis=1)/np.maximum(radial,1e-8))
                        scale = np.maximum(1e-6,np.hypot(*gradients))
                        return radial, scale, normal
                    best_fit = None
                    for seed_scale in (1., .8, 1.2):
                        parameters = np.r_[initial*seed_scale, 0.]
                        selected = None
                        for iteration in range(2 if seed_scale==1. else 3):
                            radial, scale, _ = geometric(parameters)
                            assignment = np.asarray([min(group, key=lambda index: abs(radial[j]-radii[index-1])) for j,group in enumerate(levels)])
                            if iteration == 0:
                                keep = abs(radial-radii[assignment-1])/scale < 4.
                            else:
                                keep = abs(radial-radii[assignment-1])/scale < 1.5
                            unique, counts = np.unique(assignment[keep], return_counts=True)
                            if len(unique) < 2 or min(counts) < 8:
                                selected = None; break
                            train = keep & (np.arange(len(points)) % 4 != 0)
                            validation = keep & ~train
                            weights=1/scale
                            def geometry_only(p):
                                center=p[:3]
                                normal=math.cos(p[3])*normal0+math.sin(p[3])*tangent0
                                denominator=np.sum(rays*normal,axis=1)
                                denominator=np.where(abs(denominator)>.05,denominator,np.copysign(.05,denominator))
                                distance=normal@center
                                delta=rays*(distance/denominator)[:,None]-center
                                radial=np.maximum(1e-8,np.linalg.norm(delta,axis=1))
                                return radial,normal,denominator,distance,delta
                            def residual(p):
                                radial,_,_,_,_=geometry_only(p)
                                return ((radial-radii[assignment-1])*weights)[train]
                            def jacobian(p):
                                radial,normal,denominator,distance,delta=geometry_only(p)
                                dot=np.sum(delta*rays,axis=1)/radial
                                center_derivative=dot[:,None]*normal[None,:]/denominator[:,None]-delta/radial[:,None]
                                change=-math.sin(p[3])*normal0+math.cos(p[3])*tangent0
                                angular=dot*((change@p[:3])*denominator-distance*np.sum(rays*change,axis=1))/(denominator**2)
                                return (np.column_stack((center_derivative,angular))*weights[:,None])[train]
                            fit = self._ring_fit_solver()(residual, parameters,jac=jacobian,
                                bounds=([-.5,-.5,.004,-math.pi/6],[.5,.5,.5,math.pi/6]),
                                max_nfev=50, loss='soft_l1',f_scale=.5)
                            if not fit.success:
                                selected = None; break
                            parameters = fit.x
                            selected = (fit, assignment, train, validation)
                        if selected is None:
                            continue
                        fit, assignment, train, validation = selected
                        radial, scale, normal = geometric(parameters)
                        errors = abs(radial-radii[assignment-1])/scale
                        if not np.any(validation) or np.quantile(errors[validation],.9) > .75 or np.quantile(errors[train],.9) > .75:
                            continue
                        # Validate the original pixels inside every visible colour run.
                        # A fit cannot silently discard an incompatible boundary as an
                        # outlier and then borrow a valid sum from the remaining rings.
                        yy, xx = np.indices((height,width))
                        pixel_rays=np.column_stack(((xx.ravel()-(width-1)/2)/focal,
                            (yy.ravel()-(height-1)/2)/focal,np.ones(width*height)))
                        denominator=np.sum(pixel_rays*normal,axis=1)
                        denominator=np.where(abs(denominator)>.05,denominator,np.copysign(.05,denominator))
                        distance=normal@parameters[:3]
                        plane=pixel_rays*(distance/denominator)[:,None]
                        delta=plane-parameters[:3]
                        pixel_radius=np.linalg.norm(delta,axis=1)
                        gradients=[]
                        for axis in (0,1):
                            basis=np.zeros(3);basis[axis]=1/focal
                            dp=distance*(basis[None,:]*denominator[:,None]-pixel_rays*(normal[axis]/focal))/(denominator[:,None]**2)
                            gradients.append(np.sum(delta*dp,axis=1)/np.maximum(pixel_radius,1e-8))
                        pixel_scale=np.maximum(1e-6,np.hypot(*gradients))
                        visible_runs=[];run_start=0;consistent=True
                        for end in range(1,6):
                            if end<5 and bands[end]==bands[run_start]:continue
                            lower=0. if run_start==0 else radii[run_start-1]
                            upper=radii[end-1]
                            mask=(pixel_radius>lower)&(pixel_radius<upper)&((upper-pixel_radius)/pixel_scale>.75)
                            if run_start:mask&=(pixel_radius-lower)/pixel_scale>.75
                            count=int(np.count_nonzero(mask))
                            if count>=8:
                                agreement=float(np.count_nonzero(labels.ravel()[mask]==names.index(bands[run_start]))/count)
                                visible_runs.append(dict(start=run_start+1,end=end,pixels=count,agreement=agreement))
                                if agreement<.85:consistent=False
                            run_start=end
                        if not consistent or len(visible_runs)<2:continue
                        if len(visible_runs)==2:
                            # Two colour runs can determine scale only when the
                            # outer circle has measured support in every sector.
                            # Cropped / core-only conics retain the strict fallback.
                            outer=(assignment==5)&(errors<=.75)
                            ray_denominator=rays@normal
                            if np.count_nonzero(outer)<24:continue
                            boundary=rays*((normal@parameters[:3])/ray_denominator)[:,None]-parameters[:3]
                            tangent=np.cross(up,normal);tangent/=np.linalg.norm(tangent)
                            theta=np.arctan2(boundary@up,boundary@tangent)
                            sectors=np.floor((theta+math.pi)*8/(2*math.pi)).astype(int)%8
                            if np.min(np.bincount(sectors[outer],minlength=8))<2:continue
                        if np.linalg.matrix_rank(fit.jac) < 4:
                            continue
                        sensitivity = np.linalg.pinv(fit.jac)
                        bound = np.abs(sensitivity[:3]).sum(axis=1)*.75
                        # A half-texture-pixel edge allowance propagates as relative
                        # radius/scale error; use the smallest independently seen edge.
                        minimum_radius=float(np.min(radii[np.unique(assignment[train])-1]))
                        radius_allowance=.5*side_m/texture_size
                        uncertainty = float(np.linalg.norm(bound))+float(np.linalg.norm(parameters[:3]))*radius_allowance/minimum_radius
                        score = float(np.quantile(errors[validation],.9))
                        candidate = dict(world_xyz_m=(origin+rdf@parameters[:3]).tolist(),
                            normal_xz=(rdf@normal)[[0,2]].tolist(),pixel_model_uncertainty_m=uncertainty,
                            model_mismatch=score,boundary_count=int(np.count_nonzero(train)),
                            levels=np.unique(assignment[train]).tolist(),pixel_runs=visible_runs,normal_change_degrees=math.degrees(parameters[3]))
                        return candidate
                    return best_fit
                except (KeyError, TypeError, ValueError, ZeroDivisionError, np.linalg.LinAlgError):
                    return None

            def locate_victim(self, data, side, image_center_px, bbox_px):
                """Intersect the calibrated pixel ray with its locally measured surface.

        q=[(u-cx)/f, -(v-cy)/f, 1] in camera Right-Up-Forward.
        Fit 1/Z=a*X/Z+b*Y/Z+c; lambda=1/([a,b,c] dot q).
        p_world=p_GPS+R_body*(t_cam-t_GPS+R_cam*B*q*lambda),
        B=RDF_FROM_FLU.T*diag(1,-1,1). Never substitute wall distance
        for robot-to-token distance. Failure returns an explicit unknown.
        """
                unknown = {'status': 'UNKNOWN', 'world_xz_m': None, 'navigation_xy_mm': None,
                           'distance_m': None, 'uncertainty_m': None}
                try:
                    if side not in ('left', 'right'):
                        raise ValueError('invalid camera side')
                    center = np.asarray(image_center_px, float)
                    box = np.asarray(bbox_px, float)
                    if (center.shape != (2,) or box.shape != (4,) or not np.isfinite(center).all()
                            or not np.isfinite(box).all() or min(box[2:]) <= 0):
                        raise ValueError('missing or invalid image geometry')
                    info = (data.get('camera_intrinsics') or {}).get(side) or {}
                    width = int(info.get('width', data.get('cam_w', self.cam_w)))
                    height = int(info.get('height', data.get('cam_h', self.cam_h)))
                    fov = float(info.get('fov', data.get('cam_fov', self.cam_fov)))
                    name = 'camL' if side == 'left' else 'camR'
                    if self.sizes[name] != (width, height):
                        raise ValueError('live camera dimensions differ from robot.json')
                    if not (0 <= center[0] < width and 0 <= center[1] < height):
                        raise ValueError('center outside camera')
                    # Reuse only inside the current synchronized snapshot, never by time
                    # alone: direct callers may supply a second scan at the same time.
                    key = side
                    projected = self._projection_cache.get(key) if data is self._snapshot else None
                    if projected is None:
                        projected = self._project_plaque_lidar(data, side, width, height,
                                                               self.camera_calibration(side),
                                                               include_outside_vertical=True)
                        if data is self._snapshot:
                            self._projection_cache[key] = projected
                    u, v, xyz = (projected[k] for k in ('u', 'v', 'xyz'))
                    # Use only a small observed surface footprint inside the sign.
                    radius = np.minimum(box[2:]*.3, 5.)
                    use = (abs(u-center[0]) <= radius[0]) & (abs(v-center[1]) <= radius[1])
                    points = xyz[use]
                    continued = False
                    if len(points) < 6 or len(np.unique(projected['layers'][use])) < 2:
                        # Side-camera tilt can put the symbol below all LiDAR layers.
                        # Estimate its intersection with the nearby measured wall plane;
                        # this is explicitly NOT depth evidence about the symbol.
                        # Keep the original horizontal footprint before widening it.
                        # A whole-plaque-width continuation can include another wall
                        # at a corner and make one valid local plane look discontinuous.
                        for span in (radius[0], min(width/2, max(box[2], 5.))):
                            use = abs(u-center[0]) <= span
                            points = xyz[use]
                            if len(points)>=6 and len(np.unique(projected['layers'][use]))>=2:
                                break
                        continued = True
                    if len(points) < 6 or len(np.unique(projected['layers'][use])) < 2:
                        raise ValueError('insufficient multi-layer wall support')
                    pixels = np.column_stack((u[use], v[use])).astype(np.float32)
                    inside = cv2.pointPolygonTest(cv2.convexHull(pixels), tuple(center), True) >= -.5
                    if not inside:
                        if not (u[use].min() <= center[0] <= u[use].max()):
                            raise ValueError('center outside horizontal wall support')
                        continued = True
                    design = np.column_stack((points[:, :2]/points[:, 2:3], np.ones(len(points))))
                    coefficients, _, rank, _ = np.linalg.lstsq(design, 1/points[:, 2], rcond=None)
                    if rank < 3:
                        raise ValueError('degenerate surface support')
                    residual = abs(points @ coefficients-1)/np.linalg.norm(coefficients)
                    focal = width/(2*math.tan(fov/2))
                    q = np.array([(center[0]-(width-1)/2)/focal,
                                  -(center[1]-(height-1)/2)/focal, 1.])
                    curved=bool(np.max(residual)>.002)
                    curve_failure=None
                    if curved:
                        try:
                            hit,normal_camera,residual,incidence,leverage=self._curved_surface_hit(
                                data,side,points,projected['layers'][use],q)
                        except (ValueError,np.linalg.LinAlgError) as error:
                            # A discontinuous wall cannot establish token depth.
                            # Its local plane may seed the independent pixel fit;
                            # NEVER return that seed as a measured location.
                            curve_failure=error
                            denom=coefficients@q
                            incidence=abs(denom)/(np.linalg.norm(coefficients)*np.linalg.norm(q))
                            if denom<=0 or incidence<.25:raise error
                            hit=q/denom
                            normal_camera=coefficients/np.linalg.norm(coefficients)
                            leverage=max(1.,math.sqrt(float(q@np.linalg.pinv(design.T@design)@q)))
                    else:
                        denom = coefficients @ q
                        incidence = abs(denom)/(np.linalg.norm(coefficients)*np.linalg.norm(q))
                        if denom <= 0 or incidence < .25:
                            raise ValueError('surface is behind camera or too grazing')
                        hit = q/denom
                        normal_camera=coefficients/np.linalg.norm(coefficients)
                        leverage=max(1.,math.sqrt(float(q@np.linalg.pinv(design.T@design)@q)))
                    if curve_failure is None and not continued and not points[:, 2].min()-.002 <= hit[2] <= points[:, 2].max()+.002:
                        raise ValueError('surface intersection extrapolates depth support')
                    attitude, body, sigma, position_source = self._report_body_pose(data)
                    rc, tc = self.poses[name]
                    basis = self.RDF_FROM_FLU.T @ np.diag([1.,-1.,1.])
                    world = body+attitude @ (tc+rc @ basis @ hit)
                    normal = attitude @ rc @ basis @ normal_camera
                    # Orient the normal toward this observed side for association only.
                    if normal @ (body-world) < 0:
                        normal = -normal
                    # Estimated allowance, not a statistical guarantee. Includes centre
                    # sampling, surface residual, GPS noise and 1 mm profile allowance.
                    uncertainty = (3*sigma+.001+float(np.max(residual))*leverage
                                   + np.linalg.norm(hit)*.75/focal/incidence)
                    distance = float(np.linalg.norm((world-body)[[0,2]]))
                    result=dict(status='ESTIMATED' if continued else 'MEASURED',
                                surface_support='measured-curved-wall'if curved else 'nearby-wall-plane'if continued else 'center-footprint',
                                position_source=position_source,
                                symbol_depth_observed=not continued,
                                world_xyz_m=world.tolist(),
                                world_xz_m=world[[0,2]].tolist(),
                                navigation_xy_mm=self._navigation_xy(world[[0,2]]),
                                distance_m=distance, uncertainty_m=float(uncertainty),
                                uncertainty_kind='model allowance; not a location-error bound',
                                surface_normal_xz=normal[[0,2]].tolist(),
                                gps_world_xz_m=(body+attitude@self._gps_offset)[[0,2]].tolist(),
                                camera_world_xz_m=(body+attitude@tc)[[0,2]].tolist(),
                                position_sigma_m=sigma,
                                support_rays=len(points), support_layers=len(np.unique(projected['layers'][use])),
                                max_surface_residual_m=float(np.max(residual)),
                                reason=('measured curved wall continued to pixel ray; symbol depth unobserved'if curved and continued else 'measured curved wall intersection'if curved else 'nearby wall plane continued to pixel ray; symbol depth unobserved' if continued
                                        else 'local calibrated surface intersection; position uncertainty is an estimate'))
                    metric=self._metric_target_location(data,side,result)
                    if curve_failure is not None and metric is None:raise curve_failure
                    if metric is not None:
                        point=np.asarray(metric['world_xyz_m'],float)
                        result.update(status='MEASURED',world_xyz_m=point.tolist(),
                            world_xz_m=point[[0,2]].tolist(),navigation_xy_mm=self._navigation_xy(point[[0,2]]),
                            surface_normal_xz=metric['normal_xz'],
                            distance_m=float(np.linalg.norm((point-body)[[0,2]])),
                            uncertainty_m=metric['pixel_model_uncertainty_m']+3*sigma+.001,
                            position_source='calibrated-ring-metric',surface_support='measured-ring-boundaries',
                            metric_localized=True,circle_metric=metric,
                            reason='known ring radii and held-out original pixel boundaries locate the physical token centre')
                    return result
                except (KeyError, ValueError, TypeError, IndexError, np.linalg.LinAlgError) as exc:
                    return dict(unknown, reason=str(exc))

            def _motion(self, data, now):
                """Require continuous velocity, encoder and attitude evidence for a stop."""
                previous = self._motion_previous
                stopped = False
                commands=np.asarray(data.get('motor_command_radps',()),float)
                if commands.shape!=(2,) or not np.isfinite(commands).all():
                    self._stopped_since=None;self._motion_previous=None
                    return 0.
                if previous is not None and now == previous[0]:
                    if any(data.get('motor_command_radps', ())):
                        self._stopped_since = None
                    return 0. if self._stopped_since is None else now-self._stopped_since
                try:
                    velocity = np.asarray(data['gps_velocity_mps'], float)
                    angles = np.array([math.radians(data['orientation']['yaw_raw']),
                                       data['orientation']['roll'], data['orientation']['pitch']])
                    wheels = np.array([data['encoders']['left'], data['encoders']['right']], float)
                    current = (now, angles, wheels)
                    if previous is not None:
                        dt = now-previous[0]
                        if 0 < dt <= max(.05, 1.5*float(data.get('sensor_period_s', .016))):
                            delta = (angles-previous[1]+math.pi) % (2*math.pi)-math.pi
                            stopped = (velocity.shape == (3,) and np.isfinite(velocity).all()
                                       and np.linalg.norm(velocity) < .0008
                                       and np.max(abs(delta/dt)) < .0008
                                       and np.max(abs((wheels-previous[2])/dt)) < .005
                                       and not any(data.get('motor_command_radps', ())))
                    if not np.isfinite(angles).all() or not np.isfinite(wheels).all():
                        stopped = False
                    self._motion_previous = current
                except (KeyError, ValueError, TypeError):
                    self._motion_previous = None
                if not stopped:
                    self._stopped_since = None
                elif self._stopped_since is None:
                    self._stopped_since = now
                return 0. if self._stopped_since is None else now-self._stopped_since

            def _associate(self, row, now, seen):
                location = row['location']
                point = np.asarray(location['world_xz_m'])
                normal = np.asarray(location['surface_normal_xz'])
                matches = [t for t in self._tracks.values()
                           if t['track_id'] not in seen and t['kind'] == row['kind']
                           and np.linalg.norm(point-np.array(t['world_xz_m'])) < .012
                           and normal @ np.array(t['surface_normal_xz']) > .5]
                track = min(matches, key=lambda t: math.dist(point,t['world_xz_m']), default=None)
                if track is None:
                    if len(self._tracks) >= self.MAX_TRACKS:
                        del self._tracks[min(self._tracks, key=lambda k:self._tracks[k]['last_seen_s'])]
                    track = dict(track_id=self._next_track, kind=row['kind'], first_seen_s=now,
                                 label=None, count=0, label_since_s=now, last_seen_s=-math.inf)
                    self._tracks[self._next_track] = track
                    self._next_track += 1
                # Consolidate provisional histories once they match the same measured
                # feature under the existing association rule. Do not combine votes.
                for other in matches:
                    if other is not track:
                        del self._tracks[other['track_id']]
                label = row['label'] if row['visual_can_report'] else None
                if label is None or label != track['label'] or now-track['last_seen_s'] > self.MAX_GAP_S:
                    track.update(count=0, label_since_s=now)
                if now > track['last_seen_s'] and label is not None:
                    track['count'] += 1
                track.update(label=label, last_seen_s=now, world_xz_m=point.tolist(),
                             navigation_xy_mm=location['navigation_xy_mm'], surface_normal_xz=normal.tolist(),
                             uncertainty_m=location['uncertainty_m'], side=row['side'],
                             location_status=location['status'], image_center_type=row.get('center_type'),
                             visual_identity_eligible=row['visual_can_report'],
                             rejected=row['status']=='REJECTED')
                seen.add(track['track_id'])
                return track

            def _camera_world(self, data, side):
                attitude, body, _, _ = self._body_pose(data)
                rc, tc = self.poses['camL' if side=='left' else 'camR']
                basis = self.RDF_FROM_FLU.T @ np.diag([1.,-1.,1.])
                return body+attitude@tc, attitude@rc@basis

            def project_token(self, data, side, world):
                origin, rotation = self._camera_world(data, side)
                p = rotation.T @ (np.array(world)-origin)
                if p[2]<=0:
                    return None
                w,h = self.sizes['camL' if side=='left' else 'camR']
                focal = w/(2*math.tan(self.cam_fov/2))
                return [focal*p[0]/p[2]+(w-1)/2, (h-1)/2-focal*p[1]/p[2], float(p[2])]

            def _map_location(self, data, side, center):
                try:
                    origin, rotation = self._camera_world(data, side)
                    w,h = self.sizes['camL' if side=='left' else 'camR']
                    focal = w/(2*math.tan(self.cam_fov/2))
                    ray = rotation @ np.array([(center[0]-(w-1)/2)/focal,
                                              -(center[1]-(h-1)/2)/focal,1.])
                    xy = self.token_map.xy(origin)
                    if xy is None:
                        return None
                    m = self.mapper
                    hit = self.token_map.ray_hit(xy,np.array([ray[0],m.GPS_Y_SIGN*ray[2]])*1000)
                    if hit is None:
                        return None
                    lam,point,normal = hit
                    world = origin+lam*ray
                    _,body,_,_=self._report_body_pose(data)
                    return dict(status='ESTIMATED',world_xyz_m=world.tolist(),world_xz_m=world[[0,2]].tolist(),
                                navigation_xy_mm=point.tolist(),distance_m=float(np.linalg.norm((world-body)[[0,2]])),
                                uncertainty_m=2*m.GRID_MM/1000,position_source='measured-map-ray',
                                symbol_depth_observed=False,surface_normal_xz=[normal[0],normal[1]*m.GPS_Y_SIGN],
                                reason='map-only location needs a local range observation')
                except (KeyError,ValueError,TypeError,IndexError,np.linalg.LinAlgError):
                    return None

            def _recent_range_location(self, data, side, row):
                """Retain a recent measured static token through a brief LiDAR gap.

                A new full blob must uniquely match its projected physical point.
                This never creates identity, extends the range-observation age,
                bypasses current fake checks, or supplies missing image evidence.
                """
                if row.get('partial') or row.get('center_type')!='DETECTED' or not row.get('label'):
                    return None
                now=float(data['time']);box=row.get('bbox_px');center=row.get('image_center_px')
                if not box or not center:return None
                try:attitude,body,sigma,source=self._report_body_pose(data)
                except (KeyError,ValueError,TypeError):return None
                if source!='hero-physical-gps-filter':return None
                matches=[]
                for record in self.token_map.records.values():
                    age=now-record.get('range_observed_s',-math.inf)
                    if not 0<=age<=self.MAX_GAP_S or record.get('view_rejected') or record['kind']!=row['kind']:
                        continue
                    if row['label']not in(record.get('label'),record.get('guessed_label')):continue
                    point=np.asarray(record['world_xyz_m']);normal=np.asarray(record['normal_xz'])
                    if (body-point)[[0,2]]@normal<=0:continue
                    pixel=self.project_token(data,side,point)
                    if pixel is None:continue
                    allowance=record['allowance_m']+3*sigma
                    corners=[self.project_token(data,side,point+np.array([dx,0.,dz])*allowance)
                             for dx,dz in((-1,-1),(-1,1),(1,-1),(1,1))]
                    if any(p is None for p in corners):continue
                    budget=np.max(np.abs(np.asarray(corners)[:,:2]-pixel[:2]),axis=0)+.75
                    x,y,w,h=box
                    if (not(x<=pixel[0]<=x+w and y<=pixel[1]<=y+h)
                            or np.any(abs(np.asarray(center)-pixel[:2])>budget)):continue
                    matches.append(dict(status='ESTIMATED',retained_range=True,local_range_age_s=age,
                        position_source='recent-local-range-track',symbol_depth_observed=False,
                        world_xyz_m=point.tolist(),world_xz_m=point[[0,2]].tolist(),
                        navigation_xy_mm=self._navigation_xy(point[[0,2]]),
                        distance_m=float(np.linalg.norm((point-body)[[0,2]])),uncertainty_m=allowance,
                        surface_normal_xz=normal.tolist(),position_sigma_m=sigma,
                        gps_world_xz_m=(body+attitude@self._gps_offset)[[0,2]].tolist(),
                        camera_world_xz_m=self._camera_world(data,side)[0][[0,2]].tolist(),
                        reason='unique current full blob retains recent measured token range'))
                return matches[0]if len(matches)==1 else None

            def _overhang_resolvable(self, data, side, location):
                """Can this calibrated view resolve the stock raised-symbol relief?

        Erebus Fake meshes have4.8mm relief, a12.8mm glyph and16mm plaque.
        Project their lower boundaries at the SAME measured plane, retaining
        its position allowance. A1px continuation plus two .75px raster
        boundaries must be visible before separated ink can prove a flat glyph.
        This predicts observability only; it never assigns a victim identity.
        """
                try:
                    point=np.asarray(location['world_xyz_m'],float)
                    normal=np.asarray(location['surface_normal_xz'],float)
                    normal/=np.linalg.norm(normal)
                    allowance=float(location['uncertainty_m'])
                    extensions=[]
                    for dx,dz in ((-1,-1),(-1,1),(1,-1),(1,1)):
                        center=point+np.array([dx,0.,dz])*allowance
                        plaque=center+np.array([0.,-.008,0.])
                        raised=center+np.array([.0048*normal[0],-.0064,.0048*normal[1]])
                        a=self.project_token(data,side,plaque);b=self.project_token(data,side,raised)
                        if a is None or b is None:return False
                        extensions.append(b[1]-a[1])
                    return bool(min(extensions)>=2.5)
                except (KeyError,TypeError,ValueError,ZeroDivisionError):return False

            def _qualifying_victim_view(self, side, row):
                """Use unchanged classifier/plaque gates; accumulate time per token.

        Missing, partial and guessed glyphs supply no positive identity vote.
        """
                result=(self._target_victim_cache.get(side) or {}).get('result') or {}
                check=result.get('overhang_debug') or {};box=check.get('bbox')
                return bool(row['kind']=='victim' and not row['partial']
                    and row['label'] in self.VICTIM_NAMES and result.get('label')==row['label']
                    and (result.get('raw_result') or {}).get('reportable')
                    and (result.get('fake_victim') or {}).get('is_real') is not False
                    and check.get('status')=='measured' and box and min(box[2:])>=self.MIN_SQUARE_SIDE
                    and check.get('decision') in ('no-visible-overhang','no-localized-overhang'))

            def _report_body_pose(self, data):
                """Reporting/registration needs the current trusted physical pose.

        A raw sample that contradicts the registered filter is not stronger
        evidence than that filter. Keep visual perception passive until they
        agree again; do not reinterpret the fallback as report authority.
        """
                pose=self._body_pose(data)
                if pose[3]!='hero-physical-gps-filter':
                    raise ValueError('trusted registered physical pose unavailable')
                return pose

            def _record_report_range(self, data, record):
                """Use the retained physical centre and its full uncertainty at NOW."""
                try:
                    _, body, sigma, _ = self._report_body_pose(data)
                    point = np.asarray(record['world_xyz_m'], float)
                    allowance = float(record['allowance_m'])
                    if point.shape != (3,) or not np.isfinite(point).all() or not math.isfinite(allowance) or allowance < 0:
                        raise ValueError('invalid retained token location')
                    return float(np.linalg.norm((point-body)[[0,2]])), allowance+3*sigma
                except (KeyError, ValueError, TypeError):
                    return math.inf, math.inf

            def get_detection_return(self, data):
                """One persistent token list; metadata is exposed only with debug_enabled."""
                now=float(data['time'])
                if not math.isfinite(now):raise ValueError('time must be finite')
                rewind=self._motion_previous is not None and now<self._motion_previous[0]
                relocated=bool(data.get('supervisor_relocated') or data.get('gps_filter_reset'))
                if rewind or relocated:
                    self._target_victim_cache={};self._fusion_tracks.clear();self._victim_visual_tracks.clear()
                    self._tracks.clear();self._associations.clear();self._located_time=None
                    self._motion_previous=None;self._stopped_since=None
                    if rewind:self.token_map.records.clear()
                stopped=self._motion(data,now)
                try:
                    attitude,body,sigma,source=self._body_pose(data)
                    self.token_map.update_pose(data,(body+attitude@self._gps_offset)[[0,2]],sigma,source,
                                               (attitude@self._gps_offset)[[0,2]])
                except (KeyError,ValueError,TypeError):
                    pass  # Missing pose cannot refine an earlier measurement.
                normalized=dict(data,lidar_vertical_fov=data.get('lidar_vertical_fov',data.get('lidar_vfov')))
                # At travel speed a plaque can be readable for only~0.2s.
                # Boost only while an actual token/plaque candidate is visible;
                # the quiet-field cadence, classifier and fake geometry stay unchanged.
                candidate=any((entry.get('result')or{}).get('kind')in('victim','target')
                    or any(((entry.get('result')or{}).get('raw_result')or{}).get(k,{}).get('count',0)>0
                           for k in('victim','target'))for entry in self._target_victim_cache.values())
                period=float(data.get('sensor_period_s',.016))
                base_period=TargetVictimDetector.TARGET_VICTIM_ANALYZE_PERIOD_S
                self.TARGET_VICTIM_ANALYZE_PERIOD_S=(period if getattr(self,'_buffer_worker',False) else min(base_period,2*period)
                    if candidate and math.isfinite(period) and 0<period<=.064 else base_period)
                payload=super().get_detection_return(normalized);image_time=payload['time_s']
                fresh=self._located_time!=image_time
                if fresh:
                    source=getattr(self,'_external_snapshot',None) or normalized
                    self._snapshot=dict(source,position=dict(source.get('position') or {}),
                                        orientation=dict(source.get('orientation') or {}),
                                        lidar_raw_m=np.array(source.get('lidar_raw_m',()),copy=True))
                    self._projection_cache.clear();self._locations.clear();self._associations.clear();self._located_time=image_time
                seen=set();current={};unlocated=[];cameras={}
                for side,row in payload['observations'].items():
                    cameras[side]=row['status']
                    if not row['kind']:continue
                    row.update(visual_can_report=row['can_report'],visual_report_label=row['report_label'],visual_status=row['status'])
                    if side not in self._locations:
                        loc=RescueVision._clone_payload(getattr(self,'_external_locations',{}).get(side))
                        if loc is None:loc=self.locate_victim(self._snapshot,side,row['image_center_px'],row['bbox_px'])
                        if loc.get('world_xyz_m') is None:
                            loc=self._recent_range_location(self._snapshot,side,row) or loc
                        if loc.get('world_xyz_m') is None and row['image_center_px'] is not None:
                            loc=self._map_location(self._snapshot,side,row['image_center_px']) or loc
                        self._locations[side]=loc
                    row['location']=RescueVision._clone_payload(self._locations[side]);loc=row['location'];record=None
                    if loc.get('world_xyz_m') is not None:
                        track=self._associate(row,image_time,seen) if fresh else next((t for t in self._tracks.values() if t['side']==side and t['last_seen_s']==image_time),None)
                        stable=bool(track and track['count']>=self.MIN_FRAMES and image_time-track['label_since_s']+1e-9>=self.MIN_SPAN_S)
                        clear_view=self._qualifying_victim_view(side,row)
                        check=((self._target_victim_cache.get(side) or {}).get('result') or {}).get('overhang_debug') or {}
                        # Broad dark background obscures the symbol's lower edge. Keep
                        # distant aliases as inspection hints until a fresh close view.
                        # Range schedules confirmation; it never classifies a fake.
                        ink_resolved=True
                        if check.get('ink_threshold',55)<55:
                            key=('overhang-resolution',side)
                            if key not in self._projection_cache:
                                self._projection_cache[key]=self._overhang_resolvable(self._snapshot,side,loc)
                            ink_resolved=self._projection_cache[key]
                        confirmation_view=clear_view and ink_resolved and (check.get('decision')=='no-visible-overhang' or
                            (loc.get('distance_m',math.inf)+loc.get('uncertainty_m',math.inf)<=self.REPORT_RADIUS_M))
                        identity_view=(confirmation_view if row['kind']=='victim' else
                            row['visual_can_report'] and row['label'] in self.TARGET_NAMES
                            and row['visual_status']!='REJECTED')
                        row['validated_geometry']=clear_view or (row['kind']=='target' and identity_view)
                        row['sensor_period_s']=period
                        row['identity_stable']=stable and row['visual_can_report'] and (row['kind']=='target' or confirmation_view)
                        row['confirmation_vote']=row['label'] if (clear_view or identity_view) else None
                        row['confirmation_authorized']=identity_view
                        if fresh:
                            record,association=self.token_map.observe(row,image_time,
                                project=lambda p,s=side:self.project_token(self._snapshot,s,p))
                            self._associations[side]=(None if record is None else record['id'],association)
                        else:
                            key,association=self._associations.get(side,(None,'unassociated'))
                            record=self.token_map.records.get(key)
                        if record is not None:
                            row['identity_stable'] |= bool(identity_view and record['confirmed']
                                and not record.get('view_rejected') and record['label']==row['label'])
                            if getattr(self,'_buffer_worker',False):continue
                            # A newer heuristic ray must not replace a retained
                            # higher-quality centre or shrink its range allowance.
                            # Both synchronous and buffered views use the current body.
                            report_distance,report_allowance=self._record_report_range(data,record)
                            gates=[(not record['sent'],'locally sent'),
                                   (row['visual_status']!='REJECTED','fake rejected'),
                                   (row['identity_stable'],'visual identity or independent frames unresolved'),
                                   (loc['position_source']!='measured-map-ray','local range observation required'),
                                   (now-image_time<=self.FINAL_RETURN_VALID_FOR_S,'stale image'),
                                   (report_distance+report_allowance<self.REPORT_RADIUS_M,'outside conservative reporting range'),
                                   (stopped>=self.STOP_DURATION_S,'robot must be stopped for 1.1 continuous seconds')]
                            reason=next((why for ok,why in gates if not ok),'ready')
                            current[record['id']]=dict(can_report=reason=='ready',
                                identity_ready=all(ok for ok,_ in gates[:5]),
                                inspection_ready=bool((record.get('recognized') or record.get('vote_count',0)>=1) and clear_view and
                                    all(ok for ok,_ in (gates[0],gates[1],gates[3],gates[4]))),
                                ready_to_stop=all(ok for ok,_ in gates[:-1]),visible=True,reason=reason,camera=side,
                                source='recognition',image_center_px=row['image_center_px'],bbox_px=row['bbox_px'])
                            continue
                        reason=association
                    else:reason=loc.get('reason','unlocalized')
                    unlocated.append(dict(id=None,label=row['label'],location=None if loc.get('world_xz_m') is None else
                        dict(world_xz_m=loc['world_xz_m'],navigation_xy_mm=loc.get('navigation_xy_mm')),surface=None,sent=False,
                        can_report=False,needs_verification=row['visual_status']!='REJECTED',visible=True,reason=reason,camera=side,
                        bbox_px=row['bbox_px'],image_center_px=row['image_center_px'],partial=row.get('partial'),
                        visual_status=row['visual_status']))
                if fresh and (getattr(self,'_buffer_worker',False) or self.debug_enabled):
                    self._observation_rows=payload['observations']
                # A mission-authorized return to a previously CONFIRMED token
                # needs its measured position, not another camera view. Only
                # the main coordinator can select this one ID; the worker has
                # no reporting authority. Current pose/motors/rest still gate it.
                remembered=self.token_map.records.get(getattr(self,'_remembered_report_id',None))
                if (remembered is not None and remembered['confirmed'] and not remembered['sent']
                        and not relocated and not rewind
                        and not remembered.get('view_rejected') and remembered.get('range_observed_s') is not None):
                    try:
                        _,body,sigma,_=self._report_body_pose(data)
                        delta=(body-np.asarray(remembered['world_xyz_m']))[[0,2]]
                        inside=(float(np.linalg.norm(delta))+remembered['allowance_m']+3*sigma<self.REPORT_RADIUS_M
                            and float(delta@remembered['normal_xz'])>0)
                    except (KeyError,ValueError,TypeError):inside=False
                    if inside:
                        current[remembered['id']]=dict(can_report=stopped>=self.STOP_DURATION_S,
                            identity_ready=True,inspection_ready=True,ready_to_stop=True,visible=False,
                            reason='ready'if stopped>=self.STOP_DURATION_S else'robot must be stopped for 1.1 continuous seconds',
                            camera=None,source='confirmed measured token return',image_center_px=None,bbox_px=None)
                self._tracks={k:t for k,t in self._tracks.items() if now-t['last_seen_s']<=self.TRACK_TTL_S}
                self.token_map.records={k:r for k,r in self.token_map.records.items()
                    if r['confirmed'] or r.get('vote_count',0)>0 or now-r['last_seen_s']<=self.TRACK_TTL_S}
                if getattr(self,'_buffer_worker',False):return {}
                objects=[]
                for record in self.token_map.records.values():
                    obj=self.token_map.public(record);view=current.get(record['id'])
                    obj.update(can_report=bool(view and view['can_report']),
                        identity_ready=bool(view and view['identity_ready']),
                        inspection_ready=bool(view and view['inspection_ready']),
                        ready_to_stop=bool(view and view['ready_to_stop']),needs_verification=not record['sent'] and
                        (not record['confirmed'] or record['surface'] is None or not view or
                         view['reason'] not in ('ready','robot must be stopped for 1.1 continuous seconds')),
                        visible=bool(view and view['visible']),reason=view['reason'] if view else 'not observed in this image',
                        status='REPORTED' if record['sent'] else 'READY' if view and view['can_report'] else 'VERIFYING',
                        image_center_px=None if view is None else view['image_center_px'],
                        bbox_px=None if view is None else view['bbox_px'],
                        confirmation_frames=record.get('vote_count',0),
                        confirmation_span_s=max(0.,image_time-record.get('vote_since',image_time)),
                        camera=None if view is None else view['camera'],source=None if view is None else view['source'],
                        last_seen_s=record['last_seen_s'])
                    objects.append(obj)
                objects.extend(unlocated)
                result=dict(objects=objects,time_s=now,image_time_s=image_time,stopped_s=stopped,camera_status=cameras,
                    analysis_period_s=self.TARGET_VICTIM_ANALYZE_PERIOD_S)
                self._latest_payload=RescueVision._clone_payload(result)
                if self.debug_enabled:return result
                return RescueVision._clone_payload({'objects':[{k:o[k] for k in
                    ('id','label','location','surface','sent','can_report','needs_verification')} for o in objects]})

            @staticmethod
            def _compact_detection_return(payload):
                if 'objects' not in payload:
                    return TargetVictimDetector._compact_detection_return(payload)
                rows = ['%s: %s/%s %s report=%s' %
                        ((o.get('camera') or '?').upper(), o.get('kind','token'), o['label'], o.get('status','?'), o['can_report'])
                        for o in payload['objects'] if o['visible']]
                return 'RETURN | '+('; '.join(rows) if rows else 'No detection')

            def report_packet(self, track_id, data):
                """Recheck all gates with current sensors; return 9 bytes or None.

        Call mark_report_sent only after emitter.send succeeds. No implicit
        acknowledgement, automatic retry or supervisor score is fabricated.
        """
                import struct
                payload = self.get_detection_return(data)
                obj = next((o for o in payload['objects']
                            if o['id']==track_id and o['can_report']), None)
                if obj is None:
                    return None
                x,z = (round(v*100) for v in obj['location']['world_xz_m'])
                return struct.pack('i i c', x,z, obj['label'].encode('ascii'))

            def mark_report_sent(self, track_id):
                obj = next((o for o in (self._latest_payload or {}).get('objects', ())
                            if o['id']==track_id and o['can_report']), None)
                if obj is None:
                    raise ValueError('token has no currently eligible report')
                record = self.token_map.records[track_id]
                record.update(sent=True, reported_label=obj['label'])
                obj.update(sent=True, status='REPORTED', reason='locally sent',
                           can_report=False, needs_verification=False)

        if calibration is not None and not isinstance(calibration, dict):
            raise TypeError('calibration must be a device dictionary, not a path')
        profile = copy.deepcopy(self._CALIBRATION if calibration is None else calibration)
        if getattr(mapper, 'token_map', None) is None:
            mapper.token_map = TokenMap(mapper)
        self._engine = _Engine(profile, mapper=mapper, camera_size=camera_size,
            camera_fov=camera_fov, debug_enabled=debug_enabled, capture_dir=capture_dir)
        self._async_enabled=bool(async_processing)
        if getattr(self,'_async_enabled',False):
            import socket,subprocess
            self._frames=deque(maxlen=128)  # Bounded2s RAM history, never image files.
            self._pipeline_epoch=0;self._frame_seq=0;self._last_frame_time=None;self._last_reset=False
            self._last_submitted=-1;self._worker_more=False;self._ipc_busy=False
            self._pipeline_stats={};self._pipeline_error=None
            self._ipc_out=memoryview(b'');self._ipc_in=bytearray()
            self._worker_config=dict(profile=profile,camera_size=camera_size,camera_fov=camera_fov,
                mapper=dict(origin_x=None,origin_y=None,GPS_Y_SIGN=mapper.GPS_Y_SIGN,
                    GRID_MM=mapper.GRID_MM,QUARTER_MM=mapper.QUARTER_MM,TILE_MM=mapper.TILE_MM,
                    FRONT_INDEX=getattr(mapper,'FRONT_INDEX',1024)))
            self._initial_worker_config=copy.deepcopy(self._worker_config)
            self._worker_mode='process';self._worker_mode_reason=None;self._vision_process=None
            parent,child=self._ram_channel();parent.setblocking(False)
            self._vision_socket=parent
            # Execute only this embedded class from the SAME controller file.
            # No controller/Webots/renderer imports or generated helper files.
            boot=("import sys; p=sys.argv[1]; s=open(p).read(); "
                  "a='# BEGIN EMBEDDED RescueVision class'+chr(10); b=chr(10)+'# END EMBEDDED RescueVision class'; "
                  "s=s.split(a,1)[1].split(b,1)[0] if a in s else s; "
                  "n={'__name__':'hero_vision_worker','__file__':p}; exec(compile(s,p,'exec'),n); "
                  "n['RescueVision']._process_main(int(sys.argv[2]))")
            env=dict(os.environ,OPENBLAS_NUM_THREADS='1',OMP_NUM_THREADS='1')
            try:
                self._vision_process=subprocess.Popen([sys.executable,'-c',boot,__file__,str(child.fileno())],
                    pass_fds=(child.fileno(),),stdin=subprocess.DEVNULL,stdout=subprocess.DEVNULL,
                    stderr=subprocess.PIPE,env=env)
            except (OSError,ValueError) as error:self._thread_fallback(error)
            child.close();self._engine._external_frames=True

    @property
    def debug_enabled(self):
        return self._engine.debug_enabled

    @debug_enabled.setter
    def debug_enabled(self, enabled):
        self._engine.debug_enabled = bool(enabled)

    @staticmethod
    def _presence_pixels(bgr):
        import cv2
        import numpy as np
        if bgr.shape != (64,64,3):
            raise ValueError('Expected real 64x64 BGR camera pixels')
        hsv=cv2.cvtColor(bgr,cv2.COLOR_BGR2HSV)
        gray=cv2.cvtColor(bgr,cv2.COLOR_BGR2GRAY)
        proposals=[]; rejected=[]
        # Plaques are bright bounded regions containing substantial dark ink.
        white=cv2.inRange(hsv,(0,0,140),(180,65,255))
        contours,_=cv2.findContours(white,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
        for c in contours:
            x,y,w,h=cv2.boundingRect(c); area=cv2.contourArea(c)
            if min(w,h)<3 or max(w,h)>45 or area<3 or max(w,h)>4*min(w,h):continue
            touches_edge=(x==0 or y==0 or x+w==64 or y+h==64)
            rect=cv2.minAreaRect(c); rw,rh=rect[1]
            if rw*rh<=0 or area/(rw*rh)<.10:continue
            roi=gray[y:y+h,x:x+w]; mask=np.zeros((h,w),np.uint8)
            cv2.drawContours(mask,[c-[x,y]],-1,255,-1)
            inner=np.zeros_like(mask);inner[1:-1,1:-1]=255
            bright=float(np.percentile(roi[mask>0],80))
            if touches_edge and bright<205:continue
            ink=(roi<min(100.,bright-45)) & (inner>0)
            count=int(ink.sum()); frac=count/max(1,w*h)
            if count>=3 and .04<=frac<=.65:
                proposals.append(dict(kind='glyph',bbox=[x,y,w,h],ink=count,contrast=bright-float(roi[ink].mean())))
        # Ring tokens contain a compact chromatic disc. A broad colored floor or
        # wall patch is not a token; require a smooth closed outer contour.
        color=cv2.inRange(hsv,(0,200,160),(180,255,255))
        contours,_=cv2.findContours(color,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
        for c in contours:
            area=cv2.contourArea(c); x,y,w,h=cv2.boundingRect(c)
            if area<12 or min(w,h)<5 or max(w,h)>3.5*min(w,h):continue
            peri=cv2.arcLength(c,True); circ=4*np.pi*area/max(1,peri*peri)
            approx=cv2.approxPolyDP(c,.035*peri,True)
            if circ<.40 or len(approx)<5:continue
            mask=np.zeros((h,w),np.uint8); cv2.drawContours(mask,[c-[x,y]],-1,255,-1)
            pixels=hsv[y:y+h,x:x+w][mask>0]
            local=hsv[y:y+h,x:x+w]
            bounds=[]
            for lo,hi in ((0,10),(20,40),(45,85),(100,140),(170,180)):
                ys,xs=np.where((local[:,:,0]>=lo)&(local[:,:,0]<=hi)&(local[:,:,1]>=200)&(local[:,:,2]>=160)&(mask>0))
                if len(xs)>=3: bounds.append((int(xs.min()),int(ys.min()),int(xs.max()+1),int(ys.max()+1)))
            nested=False
            for ai,a in enumerate(bounds):
                for b in bounds[ai+1:]:
                    overlap=max(0,min(a[2],b[2])-max(a[0],b[0]))*max(0,min(a[3],b[3])-max(a[1],b[1]))
                    if overlap>=.65*min((a[2]-a[0])*(a[3]-a[1]),(b[2]-b[0])*(b[3]-b[1])):nested=True
            curved=False
            if not nested:
                pts=c.reshape(-1,2)
                arc=pts[(pts[:,0]>0)&(pts[:,0]<63)&(pts[:,1]>0)&(pts[:,1]<63)]
                if len(arc)>=6:
                    (ex,ey),(ew,eh),angle=cv2.fitEllipse(arc)
                    if min(ew,eh)>=5 and max(ew,eh)<=96 and max(ew,eh)<=3.5*min(ew,eh):
                        rad=np.deg2rad(angle);d=arc-[ex,ey]
                        u=(d[:,0]*np.cos(rad)+d[:,1]*np.sin(rad))/(ew*.5)
                        v=(-d[:,0]*np.sin(rad)+d[:,1]*np.cos(rad))/(eh*.5)
                        curved=float(np.mean(np.abs(u*u+v*v-1)))<.13
                if not curved and (circ<.70 or len(approx)<6 or x==0 or y==0 or x+w==64 or y+h==64):continue
            # Circular monochrome targets are legal; classification is later.
            proposals.append(dict(kind='ring',bbox=[x,y,w,h],circularity=float(circ),vertices=len(approx)))
        return {'present':bool(proposals),'proposals':proposals}

    def presence_flags(self, data):
        """Fast two-frame wall-token ROI signal; no identity or motor authority."""
        import math
        import numpy as np
        now=float(data['time'])
        cameras=data.get('cameras') or {}
        reset=bool(data.get('supervisor_relocated') or data.get('gps_filter_reset'))
        key=(now,id(cameras),reset)
        if key==getattr(self,'_presence_cache_key',None):return self._presence_cache
        previous=getattr(self,'_presence_previous',{})
        if reset or now<getattr(self,'_presence_time',-math.inf):previous={}
        sides={}
        for side in ('left','right'):
            raw=cameras.get(side)
            entry=dict(candidate=False,flag=False,proposals=[],status='UNAVAILABLE')
            try:
                if not math.isfinite(now):raise ValueError('nonfinite camera time')
                width=int(data.get('cam_w',getattr(self._engine,'cam_w',64)));height=int(data.get('cam_h',getattr(self._engine,'cam_h',64)))
                if (width,height)!=(64,64):raise ValueError('presence requires native64x64')
                if raw is None:raise LookupError('camera image unavailable')
                if isinstance(raw,(bytes,bytearray,memoryview)):
                    if len(raw)!=16384:raise ValueError('presence requires16384BGRA bytes')
                    image=np.frombuffer(raw,np.uint8).reshape(64,64,4)[:,:,:3]
                else:
                    image=np.asarray(raw)
                    if image.dtype!=np.uint8 or image.shape not in ((64,64,3),(64,64,4)):
                        raise ValueError('presence requires native64x64 color pixels')
                    image=image[:,:,:3]
                proposal=self._presence_pixels(image)
                matching=[];old=previous.get(side)
                if old and 0<now-old[0]<=.100:
                    for item in proposal['proposals']:
                        x,y,w,h=item['bbox']
                        for earlier in old[1]:
                            if item['kind']!=earlier['kind']:continue
                            ox,oy,ow,oh=earlier['bbox']
                            if math.hypot(x+w*.5-ox-ow*.5,y+h*.5-oy-oh*.5)<=max(6.,.5*max(w,h,ow,oh)):
                                matching.append(item);break
                previous[side]=(now,proposal['proposals'])
                entry.update(candidate=proposal['present'],flag=bool(matching),proposals=proposal['proposals'],
                             matched_proposals=matching,status='PRESENT' if matching else 'CANDIDATE' if proposal['present'] else 'CLEAR')
            except (ValueError,TypeError,LookupError):
                previous.pop(side,None)
                if raw is not None:entry['status']='INVALID'
            sides[side]=entry
        result=dict(time_s=now,flag=any(v['flag'] for v in sides.values()),
                    candidate=any(v['candidate'] for v in sides.values()),sides=sides)
        self._presence_previous=previous;self._presence_time=now
        self._presence_cache_key=key;self._presence_cache=result
        return result

    def get_detection_return(self, data):
        """Observe one synchronized Hero sensor snapshot; never sends packets."""
        presence=self.presence_flags(data)
        if getattr(self,'_async_enabled',False):self._pipeline_tick(dict(data,token_presence=presence))
        result=self._engine.get_detection_return(data)
        if self.debug_enabled:result['presence']=self._clone_payload(presence)
        return result

    def _pipeline_tick(self, data):
        """Bounded immutable capture and nonblocking result handoff, main thread only."""
        import copy,math
        import numpy as np
        now=float(data['time']);reset=bool(data.get('supervisor_relocated')or data.get('gps_filter_reset'))
        if self._last_frame_time!=now:
            if (self._last_frame_time is not None and now<self._last_frame_time)or(reset and not self._last_reset):
                self._pipeline_epoch+=1;self._frames.clear();self._worker_more=False
                self._engine._target_victim_cache={};self._engine._located_time=None
                self._engine._external_locations={};self._engine._external_snapshot=None
            self._last_reset=reset;self._last_frame_time=now;self._frame_seq+=1
            keys=('time','position','orientation','encoders','gps_velocity_mps','sensor_period_s',
                  'supervisor_relocated','gps_filter_reset','camera_intrinsics','cam_w','cam_h','cam_fov','token_presence',
                  'lidar_hres','lidar_fov','lidar_vfov','lidar_vertical_fov','lidar_layers',
                  'lidar_min_range_m','lidar_max_range_m','motor_command_radps')
            frame=self._clone_payload({k:data[k]for k in keys if k in data})
            frame['cameras']={s:bytes(v)if isinstance(v,(bytes,bytearray,memoryview))else np.array(v,copy=True)
                              for s,v in data.get('cameras',{}).items()if v is not None}
            for name,dtype in(('lidar_raw_m',np.float32),('lidar',np.int32)):
                if name in data:frame[name]=np.array(data[name],dtype=dtype,copy=True);frame[name].flags.writeable=False
            m=self._engine.mapper;frame['_origin']=(m.origin_x,m.origin_y)
            self._frames.append((self._frame_seq,frame))
            while self._frames and now-self._frames[0][1]['time']>2.:self._frames.popleft()
        batch=self._pump_worker()
        if batch is not None:
            if batch is not None and batch['epoch']==self._pipeline_epoch:
                self._worker_more=batch['more'];self._pipeline_stats=batch['stats'];self._pipeline_error=None
                for source,rows in batch['evidence']:
                    for side,row in rows.items():
                        if not row.get('kind')or not(row.get('location')or{}).get('world_xyz_m'):continue
                        record,_=self._engine.token_map.observe(row,source['time'],
                            project=lambda p,d=source,s=side:self._engine.project_token(d,s,p))
                        if record is not None and row.get('inspection_hint'):
                            record['look_view'] = dict(side=side,time=source['time'],
                                label=row['label'],
                                data={k:copy.deepcopy(source[k])for k in('time','position','orientation')},
                                location=copy.deepcopy(row['location']))
                        if record is not None and row.get('validated_geometry') and row.get('confirmation_vote'):
                            # Qualified partial rings are intentional detector output.
                            # Victim qualification already requires a full plaque.
                            # Keep their measured return position just like full rings.
                            record['buffer_view']=dict(side=side,time=source['time'],
                                data={k:copy.deepcopy(source[k])for k in('time','position','orientation')},
                                location=copy.deepcopy(row['location']))
                latest=batch['latest']
                if latest is not None:
                    source,results,locations=latest;e=self._engine
                    e._target_victim_cache=results;e._target_victim_cache_t=source['time']
                    e._external_snapshot=source;e._external_locations=locations;e._located_time=None
        if not self._engine._target_victim_cache:
            self._engine._target_victim_cache={s:dict(error=None,result=None)for s in('left','right')}
            self._engine._target_victim_cache_t=now;self._engine._external_snapshot=None
        if not self._ipc_busy and self._frames and(self._frame_seq!=self._last_submitted or self._worker_more):
            import pickle,struct
            # One request in flight. Preserve chronological token evidence before
            # ordinary recent context; never send more than eight raw frames.
            fresh=[f for f in self._frames if f[0]>self._last_submitted]
            candidates=[f for f in fresh if (f[1].get('token_presence')or{}).get('candidate')]
            fresh=candidates[:8] if candidates else fresh[-8:]
            request=dict(window=fresh,epoch=self._pipeline_epoch,
                priority=getattr(self,'_inspection',None)is not None,config=self._worker_config)
            self._worker_config=None
            payload=pickle.dumps(request,protocol=5)
            self._ipc_out=memoryview(struct.pack('!I',len(payload))+payload)
            self._ipc_busy=True
            if fresh:self._last_submitted=fresh[-1][0]
            self._pump_worker(send_only=True)

    def _pump_worker(self, send_only=False):
        """Nonblocking bounded RAM IPC. Never wait for recognition or process startup."""
        import pickle,struct
        try:
            if self._ipc_out:
                try:
                    sent=self._vision_socket.send(self._ipc_out[:262144]);self._ipc_out=self._ipc_out[sent:]
                except BlockingIOError:pass
            if send_only:return None
            try:
                chunk=self._vision_socket.recv(262144)
                if not chunk:raise OSError('vision process closed its RAM channel')
                self._ipc_in.extend(chunk)
            except BlockingIOError:pass
            if len(self._ipc_in)<4:return None
            size=struct.unpack('!I',self._ipc_in[:4])[0]
            if size>16777216:raise ValueError('oversized vision result')
            if len(self._ipc_in)<size+4:return None
            batch=pickle.loads(self._ipc_in[4:4+size]);del self._ipc_in[:4+size]
            self._ipc_busy=False
            if batch.get('error'):raise RuntimeError(batch['error'])
            batch['stats'].update(worker_mode=self._worker_mode,backend_reason=self._worker_mode_reason)
            return batch
        except (OSError,ValueError,EOFError,RuntimeError,pickle.PickleError) as error:
            self._pipeline_error=repr(error)
            if self._worker_mode=='process':self._thread_fallback(error)
            return None

    def _thread_fallback(self, error):
        """Restricted host: same bounded background pipeline, no launch dependency."""
        import copy,socket,threading
        self._worker_mode='thread';self._worker_mode_reason=repr(error)
        self._vision_socket.close()
        process=self._vision_process
        if process is not None and process.poll()is None:process.terminate()
        self._vision_process=None
        parent,child=self._ram_channel();parent.setblocking(False);self._vision_socket=parent
        self._ipc_out=memoryview(b'');self._ipc_in=bytearray();self._ipc_busy=False
        self._last_submitted=-1;self._worker_config=copy.deepcopy(self._initial_worker_config)
        self._vision_thread=threading.Thread(target=self._process_main,args=(child.detach(),),
            name='HeroVision',daemon=True)
        self._vision_thread.start()

    @staticmethod
    def _clone_payload(value):
        """Isolated primitive/NumPy metadata copy in C; never a file or image spool."""
        if value is None:return None
        import pickle
        return pickle.loads(pickle.dumps(value,protocol=5))

    @staticmethod
    def _ram_channel():
        import socket
        pair=socket.socketpair()
        # macOS defaults to8KiB: that fragments one sensor frame across
        # control ticks. Bounded256KiB buffers carry a normal batch at once.
        for channel in pair:
            try:
                channel.setsockopt(socket.SOL_SOCKET,socket.SO_SNDBUF,262144)
                channel.setsockopt(socket.SOL_SOCKET,socket.SO_RCVBUF,262144)
            except OSError:pass  # Restricted host retains safe partial nonblocking transfers.
        return pair

    @staticmethod
    def _process_main(fd):
        """Dedicated interpreter/core opportunity; no robot devices or disk images."""
        import socket,struct,pickle,threading
        from collections import deque
        from types import SimpleNamespace
        import cv2
        cv2.setNumThreads(1)
        channel=socket.socket(fileno=fd);vision=None;history=deque(maxlen=128);last_epoch=None
        def receive(size):
            chunks=bytearray()
            while len(chunks)<size:
                chunk=channel.recv(size-len(chunks))
                if not chunk:raise EOFError
                chunks.extend(chunk)
            return chunks
        try:
            while True:
                size=struct.unpack('!I',receive(4))[0]
                if size>16777216:raise ValueError('oversized vision input')
                request=pickle.loads(receive(size));config=request.get('config')
                if vision is None:
                    mapper=SimpleNamespace(**config['mapper'])
                    vision=RescueVision(mapper,camera_size=config['camera_size'],camera_fov=config['camera_fov'],
                        calibration=config['profile'])
                    vision._worker=vision._engine;vision._worker._buffer_worker=True
                    mapper.token_map.attachment=lambda record:None
                    vision._worker._map_location=lambda *args:None
                    vision._worker_epoch=-1;vision._worker_scan=-1;vision._worker_heavy=-1;vision._worker_pending={}
                    vision._pipeline_stop=threading.Event()
                epoch=request['epoch']
                if epoch!=last_epoch:history.clear();last_epoch=epoch
                for item in request['window']:
                    if not history or item[0]>history[-1][0]:history.append(item)
                if history:
                    now=history[-1][1]['time']
                    while history and now-history[0][1]['time']>2.:history.popleft()
                try:
                    batch=vision._process_window(list(history),epoch,request['priority'])
                    # Return capture-time metadata and results, never images or
                    # LiDAR copies. Full raw inputs stay in the worker RAM ring.
                    def metadata(source):
                        return {k:v for k,v in source.items()if k not in('cameras','lidar','lidar_raw_m')}
                    batch['evidence']=[(metadata(source),rows)for source,rows in batch['evidence']]
                    if batch['latest']is not None:
                        source,results,locations=batch['latest'];batch['latest']=(metadata(source),results,locations)
                except Exception as error:batch=dict(error=repr(error))
                payload=pickle.dumps(batch,protocol=5)
                channel.sendall(struct.pack('!I',len(payload))+payload)
        except (EOFError,BrokenPipeError,ConnectionResetError):pass
        finally:channel.close()

    def _process_window(self, window, epoch, priority):
        """Cheap scan first, then at most four chronological detailed frames."""
        import copy,time,math
        import cv2,numpy as np
        started=time.perf_counter_ns();worker=self._worker
        if epoch!=self._worker_epoch:
            worker._target_victim_cache={};worker._fusion_tracks.clear();worker._victim_visual_tracks.clear()
            worker._tracks.clear();worker._associations.clear();worker.token_map.records.clear()
            worker._motion_previous=None;worker._located_time=None;worker._stopped_since=None
            self._worker_epoch=epoch;self._worker_scan=-1;self._worker_heavy=-1;self._worker_pending={};self._worker_last_time=-math.inf;self._worker_fast_until=-math.inf
        frames=dict(window);newest=window[-1][0];oldest=window[0][0]
        self._worker_pending={s:frames[s]for s in self._worker_pending if s in frames and s>self._worker_heavy}
        candidates=[]
        for seq,frame in reversed(window):
            if seq<=self._worker_scan:break
            interesting=bool((frame.get('token_presence')or{}).get('candidate'))
            if interesting:
                candidates.append(seq)
                continue
            for side in('left','right'):
                if frame['cameras'].get(side)is None:continue
                image=worker._camera_frame(frame,side);hsv=cv2.cvtColor(image,cv2.COLOR_BGR2HSV)
                white=cv2.inRange(hsv,(0,0,worker.WHITE_V_MIN),(180,worker.WHITE_S_MAX,255))
                contours,_=cv2.findContours(cv2.morphologyEx(white,cv2.MORPH_CLOSE,np.ones((3,3),np.uint8)),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
                for contour in contours:
                    x,y,w,h=cv2.boundingRect(contour)
                    if min(w,h)>=6 and cv2.contourArea(contour)>=8:
                        patch=hsv[y:y+h,x:x+w]
                        if np.count_nonzero(patch[:,:,2]<worker.BLACK_V_MAX)>=4:interesting=True;break
                # A coloured token can contain repeated bands. Scheduling needs
                # only a plausible colour patch; full ring/fake checks stay strict.
                if np.count_nonzero((hsv[:,:,1]>=worker.GRAY_S_MAX)&(hsv[:,:,2]>=worker.BLACK_V_MAX))>=20:
                    interesting=True
                if interesting:break
            if interesting:candidates.append(seq)
        self._worker_scan=newest
        for seq in candidates:
            for near in range(max(oldest,seq-2),min(newest,seq+2)+1):
                if near>self._worker_heavy and near in frames:self._worker_pending[near]=frames[near]
        # Keep proposed evidence until consumed or evicted by bounded history.
        # Main motion never waits; detailed work remains four frames per batch.
        if priority:
            for seq in range(max(oldest,newest-3),newest+1):
                if seq>self._worker_heavy and seq in frames:self._worker_pending[seq]=frames[seq]
        if not self._worker_pending and newest>self._worker_heavy:
            previous=getattr(self,'_worker_last_time',-math.inf)
            if window[-1][1]['time']-previous>=.1:self._worker_pending[newest]=window[-1][1]
        scan_done=time.perf_counter_ns()
        evidence=[];latest=None
        for seq in sorted(self._worker_pending):
            if self._pipeline_stop.is_set():break
            source=self._worker_pending.pop(seq)
            self._worker_heavy=seq
            # A qualified glyph/ring can be readable for only three sensor
            # frames. Keep the cheap32ms cadence except for the next two
            # sensor intervals after a localized glyph/ring hint. This is
            # scheduling only: partial hints still cannot vote or report.
            period=float(source.get('sensor_period_s',.016))
            if (source.get('token_presence')or{}).get('flag') and math.isfinite(period) and 0<period<.032:
                self._worker_fast_until=max(self._worker_fast_until,source['time']+2*period)
            fast=source['time']<=self._worker_fast_until and math.isfinite(period) and 0<period<.032
            cadence=period if fast else .032
            if source['time']-self._worker_last_time<cadence-1e-9:continue
            worker.mapper.origin_x,worker.mapper.origin_y=source['_origin']
            worker.get_detection_return(source)
            rows=getattr(worker,'_observation_rows',{})
            if math.isfinite(period) and 0<period<.032 and any(
                    (row.get('confirmation_vote') and row.get('validated_geometry')
                     or row.get('label') in ('H','S','U','F','P','C','O')
                        and (row.get('location')or{}).get('world_xyz_m') is not None
                        and (row.get('location')or{}).get('position_source')!='measured-map-ray')
                    and not row.get('identity_stable') and row.get('visual_status')!='REJECTED'
                    for row in rows.values()):
                self._worker_fast_until=source['time']+2*period
            evidence.append((source,self._clone_payload(rows)))
            latest=source
            self._worker_heavy=seq;self._worker_last_time=source['time']
            if len(evidence)>=4:break
        if latest is not None:
            latest=(latest,self._clone_payload(worker._target_victim_cache),self._clone_payload(worker._locations))
        return dict(epoch=epoch,evidence=evidence,latest=latest,more=bool(self._worker_pending),
                    stats=dict(window_frames=len(window),candidate_frames=len(candidates),detailed_frames=len(evidence),
                        pending_frames=len(self._worker_pending),last_image_time=None if latest is None else latest[0]['time'],
                        worker_ms=(time.perf_counter_ns()-started)/1e6,
                        scan_ms=(scan_done-started)/1e6,detail_ms=(time.perf_counter_ns()-scan_done)/1e6,
                        token_records=len(worker.token_map.records),active_tracks=len(worker._tracks),
                        fusion_tracks=len(worker._fusion_tracks),visual_tracks=len(worker._victim_visual_tracks)))

    def process(self, data, emitter):
        """Observe and send eligible reports; mark sent only after send succeeds."""
        result = self.get_detection_return(data)
        for token in result['objects']:
            if not token['can_report']:
                continue
            packet = self.report_packet(token['id'], data)
            if packet is not None:
                emitter.send(packet)
                self.mark_report_sent(token['id'])
                token.update(sent=True, can_report=False, needs_verification=False)
        return result

    def report_packet(self, token_id, data):
        """Recheck current eligibility; omit Stable reports without changing recognition."""
        packet = self._engine.report_packet(token_id, data)
        # Apply the policy to the final report class, after all existing visual,
        # range and stopped-time gates. Both reporting entry points use this.
        return None if packet is not None and packet[-1:] == b'S' else packet

    def mark_report_sent(self, token_id):
        """Call only after a successful emitter.send; sent is local state."""
        self._engine.mark_report_sent(token_id)

    def locate_victim(self, data, side, image_center_px, bbox_px):
        """Advanced calibrated local ray/plane location, including explicit unknown."""
        return self._engine.locate_victim(data, side, image_center_px, bbox_px)

    def save_alignment_capture(self, data):
        """Explicit diagnostic capture; returns None when debug is disabled."""
        return self._engine.save_alignment_capture(data)

    def _mission_init(self):
        # Mission state stays inside the same public, embedded vision class.
        if hasattr(self,'_inspection'):return
        self._inspection=None
        self._inspection_attempts={}
        self._inspection_visual_attempts={}
        self._inspection_approach_attempts={}
        self._observation_attempted=set()
        self._buffered_attempts={}
        self._report_time=None
        self._last_diagnostic=None

    def _event(self, exploration, event, **values):
        exploration.log.event(event,**values)

    def _inspection_evidence(self, nav, obj, record):
        """Retry failed local work only after identity or nearby geometry changes."""
        import hashlib
        import numpy as np
        digest=None
        if hasattr(nav,'cell_state'):
            x,y=np.rint(np.asarray(obj['location']['navigation_xy_mm'])/30+nav.c).astype(int)
            crop=np.s_[max(0,y-4):min(nav.n,y+5),max(0,x-4):min(nav.n,x+5)]
            h=hashlib.blake2b(digest_size=12)
            for array in (nav.cell_state[crop]!=nav.CELL_UNKNOWN,nav.floor[crop],nav.wall_h[crop],nav.wall_v[crop]):
                h.update(array.tobytes())
            # Exact inspection queries also depend on the native10mm free / occupied
            # evidence. Newly proved clearance can change without changing a30mm
            # cell's known status. Hash the bounded local travel corridor as well;
            # visitation-only changes still cannot revive a rejected query.
            mapper=getattr(nav,'_planning_mapper',None)
            if mapper is not None and hasattr(mapper,'logodds'):
                endpoints=np.asarray((obj['location']['navigation_xy_mm'],nav.current_world_mm),float)
                lower=np.floor((endpoints.min(axis=0)-120.)/mapper.GRID_MM+mapper.c).astype(int)
                upper=np.ceil((endpoints.max(axis=0)+150.)/mapper.GRID_MM+mapper.c).astype(int)
                lower=np.maximum(lower,0);upper=np.minimum(upper,mapper.n)
                high=mapper.logodds[lower[1]:upper[1],lower[0]:upper[0]]
                h.update((high<=mapper.FREE_T).tobytes())
                h.update((high>=mapper.OCC_T).tobytes())
            digest=h.digest()
        # A failed approach is not a ban on this physical victim. A materially
        # different wall-face viewing sector, or reaching report range, supplies
        # a new local approach; time alone and visitation do not.
        import math
        delta=np.asarray(nav.current_world_mm)-obj['location']['navigation_xy_mm']
        normal=self._engine.token_map.normal(record.get('normal_xz',(1.,0.)))
        sector=round(math.atan2(normal[0]*delta[1]-normal[1]*delta[0],float(normal@delta))/(math.pi/4))
        return obj['id'],bool(record['confirmed']),record.get('label'),digest,sector,bool(np.linalg.norm(delta)<60.)

    def _release(self, bot, exploration, reason, failed=False):
        active=self._inspection
        if active is None:return
        if active.get('cancel') is not None:active['cancel'].set()
        if failed:
            key=active['evidence']
            self._inspection_attempts[key]=self._inspection_attempts.get(key,0)+1
            if reason in ('no_qualifying_view_after_stop','inspection_alignment_timeout','inspection_budget_expired'):
                key=active.get('visual_evidence')
                if key is not None:
                    self._inspection_visual_attempts[key]=self._inspection_visual_attempts.get(key,0)+1
            key=active.get('approach_evidence',active.get('visual_evidence'))
            point=getattr(bot.navigator,'current_world_mm',None)
            if key is not None and point is not None:
                import math
                previous=self._inspection_approach_attempts.get(key)
                radius=float(getattr(bot.navigator,'ROBOT_RADIUS_MM',37.))
                if previous is None or math.dist(point,previous[0])>=radius:
                    self._inspection_approach_attempts[key]=(tuple(point),1)
                else:self._inspection_approach_attempts[key]=(previous[0],previous[1]+1)
        # Cleanup must not overwrite a higher-priority recovery/manual command
        # which handle_keys has already applied during this same control step.
        if active.get('owns_motors',True) and bot.driving._recovery is None and bot.driving._autonomous:
            bot.driving._stop_task()
            bot.driving._attempted_request=None
        self._event(exploration,'token_inspection_end',time=active['now'],id=active['id'],reason=reason,failed=failed,
            visual_failures=self._inspection_visual_attempts.get(active.get('visual_evidence'),0),
            approach_failures=self._inspection_approach_attempts.get(active.get('approach_evidence',active.get('visual_evidence'))))
        self._inspection=None
        self._engine._remembered_report_id=None

    def _inspection_heading(self, data, record, side):
        """Fresh measured blob feedback, with the classifier's broad deadband."""
        import math
        import numpy as np
        payload=self._engine._latest_payload or {}
        view=next((o for o in payload.get('objects',()) if o.get('id')==record['id']
            and o.get('visible') and o.get('camera')==side and o.get('image_center_px') is not None),None)
        if view is None:
            return None
        width,height=self._engine.sizes['camL' if side=='left' else 'camR']
        u,v=map(float,view['image_center_px']);fov=float(data.get('cam_fov',self._engine.cam_fov))
        age=data['time']-payload.get('image_time_s',-math.inf)
        if age>float(data.get('sensor_period_s',.016))+1e-9:
            if not getattr(self,'_async_enabled',False) or age>self._engine.FINAL_RETURN_VALID_FOR_S:return None
            # Compensate the captured blob's measured3D point for known motion.
            # Never steer using an old pixel as though it were captured now.
            loc=self._engine._locations.get(side)or{}
            point=loc.get('world_xyz_m')
            projected=None if point is None else self._engine.project_token(data,side,point)
            if projected is None:return None
            u,v=projected[:2]
        if not math.isfinite(u) or not math.isfinite(v) or not 0<=u<width or not 0<=v<height or not 0<fov<math.pi:return None
        mapper=self._engine.token_map.mapper
        current=math.pi/2-math.radians(mapper.YAW_OFFSET_DEG+mapper.YAW_SIGN*data['orientation']['yaw_raw'])
        tolerance=(self._engine.TARGET_CENTER_TOL_FRAC if record['kind']=='target' else self._engine.CENTER_TOL_FRAC)*width
        bbox=view.get('bbox_px')
        horizontal_fit=(bbox is None or (bbox[0]>=1 and bbox[0]+bbox[2]<=width-2))
        if horizontal_fit and abs(u-width/2)<=tolerance:return current
        # Calibrated camera axes give the turn sign for either side camera and
        # its fixed tilt. Only the measured pixel determines the correction.
        _,rotation=self._engine._camera_world(data,side)
        focal=width/(2*math.tan(fov/2))
        vertical=((height-1)/2-v)/focal
        ray=rotation@np.array([(u-(width-1)/2)/focal,vertical,1.])
        # Aim inside the accepted region, not exactly at the optical centre.
        target_u=width/2+math.copysign(tolerance/2,u-width/2)
        forward=rotation@np.array([(target_u-(width-1)/2)/focal,vertical,1.])
        a=np.array([forward[0],mapper.GPS_Y_SIGN*forward[2]])
        b=np.array([ray[0],mapper.GPS_Y_SIGN*ray[2]])
        correction=math.atan2(a[0]*b[1]-a[1]*b[0],float(a@b))
        correction*=mapper.YAW_SIGN/mapper.GPS_Y_SIGN
        if not math.isfinite(correction) or abs(correction)>fov/2:return None
        return current+correction

    @staticmethod
    def _query_inspection(worker, poses, cancel, headings=None, revisit=False):
        """Bounded exact local queries in an isolated normal planner engine.

        No planner policy, sensor clearance or unknown-space gate is changed.
        An exact inspection pose avoids snapping a50mm stand-off back to60mm.
        """
        import math,time,sys
        import numpy as np
        engine=None;started=time.perf_counter_ns();rejected=[]
        try:
            worker._check_plan_cancelled();worker._planning_maps()
            native=sys.modules[type(worker._native_collision_engine).__module__]
            # The normal query prepares its45degree engine the same way.
            engine,_,_=native.prepare(worker,45,lambda fn:(fn(),{}))
            source=np.asarray(worker.current_world_mm,float)
            yaw=math.pi/2-math.radians(worker._planning_mapper.YAW_OFFSET_DEG+
                worker._planning_mapper.YAW_SIGN*worker.current_yaw_deg)
            for i,pose in enumerate(poses[:3]):
                worker._check_plan_cancelled()
                points,cost=engine.query(source,pose,yaw)
                final_heading=(math.atan2(*(points[-1]-points[-2])[::-1])if revisit and len(points)>1 else
                    yaw if headings is None else headings[i])
                forward=bool(len(points) and(revisit or RescueVision._inspection_forward_route(points,yaw,final_heading)))
                checked=points if revisit else np.vstack((points,points[-1]+[math.cos(final_heading),math.sin(final_heading)]))if len(points)else points
                safe=bool(forward and engine.check(checked,yaw))
                if safe:
                    length=float(np.linalg.norm(np.diff(points,axis=0),axis=1).sum())
                    if length<=300.:
                        return dict(points=points.tolist(),view_heading=final_heading,cost_mm=cost,length_mm=length,
                            ms=(time.perf_counter_ns()-started)/1e6)
                if getattr(worker._planning_mapper,'DEBUG_MAPPING_DUMP',False):
                    rejected.append(dict(pose_mm=np.asarray(pose).tolist(),points_mm=np.asarray(points).tolist(),
                        forward=forward,safe=safe,status=engine.stats.get('status'),heading=yaw,view_heading=final_heading))
                if engine.stats.get('status')==5:break
            return dict(points=[],reason='no_safe_local_inspection_route',native_status=dict(engine.stats),rejected_queries=rejected,
                source_mm=source.tolist(),poses_mm=np.asarray(poses).tolist(),ms=(time.perf_counter_ns()-started)/1e6)
        finally:
            if engine is not None:engine.close()
            cached=getattr(worker,'_native_collision_engine',None)
            if cached is not None:cached.close();worker._native_collision_engine=None

    @staticmethod
    def _inspection_forward_route(points, heading, view_heading=None):
        """Optional approaches must not turn back to a token just passed."""
        import math
        import numpy as np
        legs=np.diff(np.asarray(points,float),axis=0)
        legs=legs[np.linalg.norm(legs,axis=1)>1.]
        # A compass leg perpendicular to a measured heading can differ by the
        # driver's .15degree heading tolerance. Do not call that a reversal.
        return bool(len(legs) and np.all(legs@np.array([math.cos(heading),math.sin(heading)])
            >=-np.linalg.norm(legs,axis=1)*math.sin(math.radians(.15))-1e-6)
            and (view_heading is None or abs(math.remainder(view_heading-
                math.atan2(legs[-1,1],legs[-1,0]),2*math.pi))<=math.pi/2+.05))

    @staticmethod
    def _inspection_poses(point, body, normal, allowance, heading):
        """Forward wall-side poses inside the reporting circle, with yaw retained."""
        import math
        import numpy as np
        radius=58.-allowance
        if radius<=42.:return []
        tangent=np.array([-normal[1],normal[0]])
        if tangent@np.array([math.cos(heading),math.sin(heading)])<0:tangent=-tangent
        along=float((body-point)@tangent)
        poses=[]
        # Tangential retreat is not necessarily backwards driving: moving
        # toward the wall can keep the WHOLE correction in the forward half
        # plane. Project onto the reporting disk and permitted wall stand-off
        # before trying the existing along-wall alternatives.
        safe_radius=radius-.5
        delta=np.asarray(body)-point
        if safe_radius>=42. and np.linalg.norm(delta)>0:
            nearest=delta*min(1.,safe_radius/np.linalg.norm(delta))
            stand_off=float(np.clip(nearest@normal,42.,55.))
            span=math.sqrt(max(0.,safe_radius*safe_radius-stand_off*stand_off))
            pose=point+normal*stand_off+tangent*np.clip(nearest@tangent,-span,span)
            if (pose-body)@np.array([math.cos(heading),math.sin(heading)])>=-1e-6:
                poses.append(pose)
        for offset in (max(8.,along+4.),max(0.,along)):
            if abs(offset)>=radius:continue
            stand_off=min(55.,math.sqrt(radius*radius-offset*offset)-.5)
            if stand_off>=42.:
                poses.append(point+normal*stand_off+tangent*offset)
        return poses

    @staticmethod
    def _remembered_poses(point, body, normal, allowance):
        """Closest safe report-circle samples for an already identified token."""
        import math
        import numpy as np
        radius=57.5-allowance
        if radius<42.:return []
        delta=np.asarray(body)-point
        length=float(np.linalg.norm(delta))
        candidates=[]
        if length>0:
            nearest=point+delta*min(1.,radius/length)
            if (nearest-point)@normal>=42.:candidates.append(nearest)
        tangent=np.array([-normal[1],normal[0]])
        for offset in (0.,8.,-8.):
            if abs(offset)>=radius:continue
            width=math.sqrt(radius*radius-offset*offset)
            if width>=42.:candidates.append(point+normal*width+tangent*offset)
        unique=[]
        for pose in candidates:
            if not any(np.linalg.norm(pose-other)<.5 for other in unique):unique.append(pose)
        return unique[:3]

    @staticmethod
    def _observation_poses(point, body, normal, heading, max_distance=60.):
        """Small forward-only pass to acquire another view, never a report goal."""
        import math
        import numpy as np
        forward=np.array([math.cos(heading),math.sin(heading)])
        tangent=np.array([-normal[1],normal[0]])
        if tangent@forward<0:tangent=-tangent
        delta=np.asarray(body)-point
        along=float(delta@tangent)
        # The incoming classifier has measured a full glyph, but its close
        # authenticity test may still be unobservable. Repeating the current
        # distance (or moving outward) cannot increase its apparent relief.
        # Try the existing report-approach stand-off first. The normal planner
        # still proves clearance; no fake, uncertainty or report gate changes.
        stand_off=float(np.clip(delta@normal,60.,100.))
        poses=[]
        for distance in (55.,stand_off):
            pose=point+normal*distance+tangent*(along+20.)
            if (pose-body)@forward>=0 and 5.<=np.linalg.norm(pose-body)<max_distance:
                poses.append(pose)
        return poses

    def _inspection_pose_heading(self, data, pose, record, side):
        """Yaw for the SAME camera at a proposed pose, from measured calibration.

        This projects a confirmed token; it is never sensor or identity input.
        Solve the horizontal pixel equation including camera/body lever arm.
        """
        import math
        import numpy as np
        engine=self._engine;mapper=engine.token_map.mapper
        _,body,_,_=engine._body_pose(data)
        origin,rotation=engine._camera_world(data,side)
        offset=origin-body
        candidate=body.copy()
        candidate[[0,2]]=[(pose[0]+mapper.origin_x)/1000,
            mapper.GPS_Y_SIGN*(pose[1]+mapper.origin_y)/1000]
        q=np.asarray(record['world_xyz_m'])-candidate;right=rotation[:,0]
        a=right[0]*q[0]+right[2]*q[2]
        b=right[2]*q[0]-right[0]*q[2]
        c=float(right@offset-right[1]*q[1]);r=math.hypot(a,b)
        if not r or abs(c)>r:return None
        phase=math.atan2(b,a);angle=math.acos(c/r)
        current=math.pi/2-math.radians(mapper.YAW_OFFSET_DEG+mapper.YAW_SIGN*data['orientation']['yaw_raw'])
        width,height=engine.sizes['camL' if side=='left' else 'camR']
        focal=width/(2*math.tan(engine.cam_fov/2))
        for delta in sorted((math.remainder(phase-angle,2*math.pi),
                             math.remainder(phase+angle,2*math.pi)),key=abs):
            if abs(delta)>engine.cam_fov/2:continue
            turn=engine.axis_rotation((0,1,0,delta))
            pixel=(turn@rotation).T@(q-turn@offset)
            if pixel[2]>0 and 1<=(height-1)/2-focal*pixel[1]/pixel[2]<=height-2:
                return current-mapper.YAW_SIGN*delta
        return None

    def prepare_inspection(self, data, bot, exploration, keys=(), previous_keys=()):
        """Observe, choose at most one stable local inspection, never move here."""
        import math,threading,time
        import numpy as np
        self._mission_init();now=float(data['time'])
        self.debug_enabled=bool(bot.DEBUG_ENABLED)
        result=self.get_detection_return(data)
        bot.token_result=result
        payload=self._engine._latest_payload or {}
        # Manual takeover, a latched home return, relocation and a committed
        # narrow crossing outrank optional inspection. Never fight their motors.
        manual=bool(set(keys)&{315,317,314,316,ord(' '),ord('E'),ord('G')})
        allowed=(not manual and exploration.enabled and not exploration.done
            and not getattr(exploration,'encounter_holding',False)
            and not exploration.returning and exploration.startup_ready()
            and data.get('position',{}).get('map_physical_pose_ready',False)
            and bot.driving._autonomous and not bot.driving._recovery_failed
            and bot.driving._recovery is None and not data.get('supervisor_relocated')
            and not data.get('gps_filter_reset'))
        try:self._engine._report_body_pose(data)
        except (KeyError,ValueError,TypeError):allowed=False
        active=self._inspection
        if active is not None:
            active['now']=now
            if not allowed:
                self._release(bot,exploration,'encounter_yield' if getattr(exploration,'encounter_holding',False)
                    else 'manual_return_or_pose_reset');return False
            record=self._engine.token_map.records.get(active['id'])
            if record is None or record.get('view_rejected') or not (record['confirmed'] or record.get('recognized') or active.get('observing') and (record.get('vote_count',0)>=1 or active.get('inspection_hint'))):
                self._release(bot,exploration,'identity_lost_or_fake_rejected',True);return False
            if record['sent']:
                if active.get('sent_at') is None:active['sent_at']=now
                # Leave radio packets two complete control steps at rest.
                if now-active['sent_at']>=2*float(data.get('sensor_period_s',.016)):
                    self._release(bot,exploration,'report_sent');return False
                return True
            # Simulation pauses/render stalls are not failed inspection motion.
            # The coordinator separately keeps the official real-time home reserve.
            if now-active['started']>8.:
                self._release(bot,exploration,'inspection_budget_expired',True);return False
            if active['phase'] in ('planning','approach'):
                view=next((o for o in payload.get('objects',())if o.get('id')==active['id']),None)
                if view is not None and view.get('ready_to_stop'):
                    if active.get('cancel') is not None:active['cancel'].set()
                    active['owns_motors']=True
                    if getattr(self,'_async_enabled',False) and record['confirmed']:
                        self._engine._remembered_report_id=record['id']
                    bot.driving._stop_task()
                    self._start_alignment(data,bot,active,record,exploration)
            if active['phase']=='planning' and active['future'].done():
                try:route=active['future'].result()
                except Exception as error:
                    self._event(exploration,'token_inspection_query_error',time=now,error=repr(error))
                    self._release(bot,exploration,'inspection_query_failed',True);return False
                self._event(exploration,'token_inspection_route',time=now,id=active['id'],**route)
                points=np.asarray(route['points'],float).reshape(-1,2)
                if len(points)>=2:
                    nav=bot.navigator
                    nav._planning_maps();engine=nav._native_collision_engine
                    yaw=bot.driving.frame(data).heading
                    # A background result has an older source. Reuse the
                    # planner's exact current-pose attachment, not a shifted
                    # first vertex which silently breaks its compass geometry.
                    import sys
                    native=sys.modules[type(engine).__module__]
                    encoded=points/30.+nav.c
                    connector=nav._compass_connector(np.asarray(nav.current_world_mm)/30.+nav.c,
                        encoded[1],encoded[2:])
                    if connector is None:
                        self._release(bot,exploration,'inspection_source_not_reconnectable',True);return False
                    points=native.nav_world([*connector,*encoded[2:]],nav.c,nav.current_world_mm)
                    points,_=engine.simplify_route(points,yaw)
                    if not exploration.inspection_affordable(now,points):
                        self._release(bot,exploration,'inspection_trip_exceeds_home_reserve');return False
                    final_heading=route.get('view_heading',active['observation_heading'])
                    forward=bool(active.get('revisit')or self._inspection_forward_route(points,active['approach_heading'],final_heading))
                    checked=points if active.get('revisit')else np.vstack((points,points[-1]+[math.cos(final_heading),math.sin(final_heading)]))
                    safe=bool(forward and engine.check(checked,yaw))
                    if safe:
                        bot.driving.set_route(points[1:]);active['phase']='approach'
                        active['owns_motors']=True
                        active['observation_heading']=final_heading
                        active['route']=points.tolist();active['phase_since']=now
                        return True
                    self._event(exploration,'token_inspection_adoption_rejected',time=now,id=active['id'],
                        forward=forward,safe=safe,points_mm=points.tolist(),heading=yaw,view_heading=final_heading)
                self._release(bot,exploration,'local_route_not_adoptable',True);return False
            if (active['phase']=='settle' and payload.get('image_time_s',-math.inf)>active['phase_since']
                    and payload.get('stopped_s',0.)>=2*float(data.get('sensor_period_s',.016))):
                self._start_alignment(data,bot,active,record,exploration,settled=True)
            if active['phase']=='verify':
                obj=next((o for o in payload.get('objects',()) if o.get('id')==active['id']),None)
                if obj is None or not obj.get('ready_to_stop'):
                    self._release(bot,exploration,'report_view_or_range_lost',True);return False
            if active['phase']=='verify' and now-active['phase_since']>3.:
                self._release(bot,exploration,'no_qualifying_view_after_stop',True);return False
            return True
        if not allowed or getattr(bot.driving,'committed_crossing',False):return False
        nav=bot.navigator
        if nav.current_world_mm is None or not nav._planning_pose_current():return False
        if not exploration.inspection_affordable(now,[nav.current_world_mm]):return False
        rows=[o for o in payload.get('objects',()) if o.get('id') is not None
            and not o['sent'] and (o.get('identity_ready') or o.get('inspection_ready')) and o.get('location')
            and o.get('reason')!='fake rejected']
        if getattr(self,'_async_enabled',False):
            listed={o['id']for o in rows}
            for record in self._engine.token_map.records.values():
                view=record.get('buffer_view') or record.get('look_view')
                if (record['id']in listed or record['sent']or record.get('view_rejected')
                        or not view or now<view['time']):continue
                hint=not record['confirmed']
                # A recognized glyph can still lack close authenticity evidence.
                # Losing it at the image edge must not erase its bounded chance
                # to approach. This remains an inspection hint, never permission
                # to report from memory or skip the live fake/range checks.
                weak_look = bool(hint and view is record.get('look_view'))
                if hint and (now-view['time']>2. or
                        record.get('vote_count',0)<1 and not weak_look):continue
                xy=self._engine.token_map.xy(record['world_xyz_m'])
                if xy is None or math.dist(nav.current_world_mm,xy)>(300. if weak_look or not hint else 180.):continue
                rows.append(dict(id=record['id'],sent=False,inspection_ready=True,identity_ready=not hint,
                    camera=view['side'],location=dict(navigation_xy_mm=xy.tolist()),buffered=not hint,
                    buffered_observation=hint,inspection_hint=weak_look,inspection_view=view,
                    reason='measured full glyph recovered for a moving look' if hint else
                        'confirmed token recovered from measured token memory'))
        rows.sort(key=lambda o:(math.dist(nav.current_world_mm,o['location']['navigation_xy_mm']),o['id']))
        for obj in rows:
            record=self._engine.token_map.records.get(obj['id'])
            if record is None or record.get('view_rejected'):continue
            observing=not record['confirmed']
            if observing and record.get('vote_count',0)<1 and not obj.get('inspection_hint'):continue
            if not(record.get('label') or observing and (record.get('vote_label') or obj.get('inspection_hint'))):continue
            # A stable full glyph may request a safe approach while its close
            # authenticity check remains unresolved. Weak hints stay passive.
            loc=((obj.get('inspection_view') or record.get('buffer_view') or {})['location']if obj.get('buffered') or obj.get('buffered_observation')else self._engine._locations.get(obj.get('camera')))or{}
            if loc.get('position_source')=='measured-map-ray' or loc.get('world_xyz_m') is None:continue
            action='observe'if observing else'revisit'if obj.get('buffered')else'report'if obj.get('ready_to_stop')else'approach'
            evidence=(self._inspection_evidence(nav,obj,record),action)
            if observing and evidence in self._observation_attempted:continue
            look_key=(obj['id'],'unconfirmed-plaque-look')
            if obj.get('inspection_hint') and self._buffered_attempts.get(look_key,0)>=2:continue
            buffered_key=(record['id'],evidence[0][3] if len(evidence[0])>3 else None) if obj.get('buffered') else None
            if buffered_key is not None and self._buffered_attempts.get(buffered_key,0)>=2:continue
            if self._inspection_attempts.get(evidence,0)>=2:continue
            visual_evidence=(obj['id'],bool(record['confirmed']),record.get('label'))
            # Map-boundary jitter and a different camera must not restart the
            # same failed visual experiment. A newly confirmed identity is new
            # evidence; ordinary observation remains active after this limit.
            if self._inspection_visual_attempts.get(visual_evidence,0)>=2:continue
            approach_evidence=(*visual_evidence,action)
            previous=self._inspection_approach_attempts.get(approach_evidence)
            if (previous is not None and previous[1]>=2
                    and math.dist(nav.current_world_mm,previous[0])<float(getattr(nav,'ROBOT_RADIUS_MM',37.))):continue
            point=np.asarray(obj['location']['navigation_xy_mm'],float)
            body=np.asarray(nav.current_world_mm,float);distance=float(np.linalg.norm(point-body))
            if distance>(300. if obj.get('buffered')or obj.get('inspection_hint')else 180.):continue
            normal=self._engine.token_map.normal(record['normal_xz'])
            if float((body-point)@normal)<=0:continue
            active=dict(id=obj['id'],evidence=evidence,visual_evidence=visual_evidence,started=now,now=now,
                approach_evidence=approach_evidence,phase='verify',phase_since=now,side=obj['camera'],heading=None,cancel=None,
                owns_motors=False)
            active['revisit']=bool(obj.get('buffered'))
            active['observing']=observing
            active['inspection_hint']=bool(obj.get('inspection_hint'))
            if active['inspection_hint']:self._buffered_attempts[look_key]=self._buffered_attempts.get(look_key,0)+1
            if observing:self._observation_attempted.add(evidence)
            if buffered_key is not None:self._buffered_attempts[buffered_key]=self._buffered_attempts.get(buffered_key,0)+1
            frame=bot.driving.frame(data)
            if frame is None:continue
            active['observation_heading']=frame.heading
            active['approach_heading']=frame.heading
            self._inspection=active
            if getattr(self,'_async_enabled',False) and record['confirmed']:
                self._engine._remembered_report_id=record['id']
            # Cancel only optional selector work. Its original committed goal
            # and route remain intact throughout the inspection.
            if exploration.worker is not None:exploration.worker._plan_cancel_event.set()
            self._event(exploration,'token_inspection_start',time=now,id=obj['id'],distance_mm=distance,
                confirmed=bool(record['confirmed']),reason=obj.get('reason'))
            allowance=max(0.,float(record['allowance_m'])*1000)
            # The live camera/range gates already prove reporting eligibility.
            # Do not replace that proof with the tighter reserve used only to
            # construct a NEW approach pose (and move an eligible token away).
            if not observing and not obj.get('buffered')and(obj.get('ready_to_stop') or distance+allowance<58.):
                if not record['confirmed']:
                    self._release(bot,exploration,'close_view_not_authenticated',True);return False
                active['owns_motors']=True
                bot.driving._stop_task()
                self._start_alignment(data,bot,active,record,exploration)
            else:
                if obj.get('buffered'):
                    # A remembered report has no fresh camera/range correction.
                    # Reserve its current-pose uncertainty in the goal as well
                    # as in the report gate, plus the driver's2mm capture.
                    try:allowance+=3000*float(self._engine._body_pose(data)[2])
                    except (KeyError,ValueError,TypeError):
                        self._release(bot,exploration,'reporting_pose_unavailable',True);return False
                # A query is not a committed motion or an eligible report.
                # Keep the normal driver in control until a safe route is adopted.
                # Keep the largest useful camera stand-off inside the report
                # circle. Moving closer unnecessarily clips near-field plaques
                # vertically on the fixed, tilted side cameras. Keep the goal
                # inside the same uncertainty-adjusted reporting circle.
                if obj.get('buffered'):
                    # A confirmed token needs no image-restoring pose. If
                    # already in range, report here; otherwise sample the
                    # closest part of its uncertainty-reserved reporting disk.
                    if distance+allowance<58.:
                        active.update(phase='verify',owns_motors=True)
                        bot.driving._stop_task()
                        return True
                    poses=self._remembered_poses(point,body,normal,allowance)
                    viewing_data=record['buffer_view']['data']
                else:
                    poses=(self._observation_poses(point,body,normal,frame.heading,300. if obj.get('inspection_hint')else 60.) if observing else
                        self._inspection_poses(point,body,normal,allowance,frame.heading));viewing_data=data
                views=[(pose,frame.heading if obj.get('buffered')else
                    self._inspection_pose_heading(viewing_data,pose,record,obj['camera']))for pose in poses]
                poses=[pose for pose,heading in views if heading is not None]
                headings=[heading for pose,heading in views if heading is not None]
                if not poses:
                    self._release(bot,exploration,'no_forward_reporting_pose',True);return False
                cancel=threading.Event();worker=nav._planning_snapshot();worker._plan_cancel_event=cancel
                worker._map_display=None
                active.update(phase='planning',cancel=cancel)
                active['future']=exploration.executor.submit(self._query_inspection,worker,poses,cancel,headings,active['revisit'])
            return True
        return False

    def _start_alignment(self, data, bot, active, record, exploration=None, *, settled=False):
        import math
        # A complete, sufficiently large current image is already the desired
        # view. Do not turn merely to centre its estimated3D position, or force
        # a return to the camera that first saw it before the approach.
        payload=self._engine._latest_payload or {}
        if (getattr(self._engine,'_remembered_report_id',None)==record.get('id') and
                any(o.get('id')==record.get('id')and o.get('ready_to_stop')for o in payload.get('objects',()))):
            active.update(phase='verify',heading=None,phase_since=float(data['time']))
            return
        if not settled and payload.get('stopped_s',0.)<2*float(data.get('sensor_period_s',.016)):
            # Approach completion does not imply the camera has observed the
            # stopped pose. Wait for a fresh image before choosing any yaw.
            active.update(phase='settle',heading=None,phase_since=float(data['time']))
            return
        view=next((o for o in payload.get('objects',()) if o.get('id')==active['id']
            and o.get('visible') and o.get('bbox_px') and o.get('camera') in ('left','right')),None)
        if view is not None and view.get('ready_to_stop'):
            # Preserve the recognizer's approved partial-ring policy. A valid
            # report view needs no exact centering, rotation or extra approach.
            active.update(phase='verify',side=view['camera'],heading=None,phase_since=float(data['time']))
            return
        if view is not None and (view.get('ready_to_stop') or
                data['time']-payload.get('image_time_s',-math.inf)<=float(data.get('sensor_period_s',.016))+1e-9):
            x,y,w,h=view['bbox_px'];width,height=self._engine.sizes['camL' if view['camera']=='left' else 'camR']
            if min(w,h)>=16 and x>=1 and y>=1 and x+w<=width-2 and y+h<=height-2:
                active.update(phase='verify',side=view['camera'],heading=None,phase_since=float(data['time']))
                return
        frame=bot.driving.frame(data)
        if frame is None:
            active.update(phase='verify',heading=None,phase_since=float(data['time']))
            return
        current=frame.heading
        # Never seek a remembered point with another camera. A missing fresh
        # blob supplies no steering evidence and cannot authorize a search.
        if view is None:
            self._release(bot,exploration,'alignment_blob_lost',True)
            return
        active['side']=view['camera']
        angle=self._inspection_heading(data,record,active['side'])
        if angle is None:
            self._release(bot,exploration,'alignment_blob_lost',True)
            return
        if abs(math.remainder(angle-current,2*math.pi))>math.radians(3):
            active.update(phase='align',heading=angle,phase_since=float(data['time']),
                align_image_time=payload['image_time_s'])
            active.setdefault('align_started',float(data['time']))
            active.setdefault('align_last_heading',current)
            active.setdefault('align_travel',0.)
            active.setdefault('align_limit',float(data.get('cam_fov',self._engine.cam_fov))/2)
            bot.driving._inspection_turn_previous=None
        else:
            active.update(phase='verify',phase_since=float(data['time']))

    def drive_inspection(self, data, bot, exploration, keys=(), previous_keys=(), turbo=False):
        """Publish display-only ownership after the final motor decision."""
        bot.navigator._vision_motor_status=None
        result=self._drive_inspection(data,bot,exploration,keys,previous_keys,turbo)
        active=self._inspection
        if active is not None and active.get('owns_motors',True) and bot.driving._recovery is None:
            bot.navigator._vision_motor_status=dict(phase=active['phase'],token_id=active['id'],
                wheels=list(bot._motor_command),time=float(data['time']))
        return result

    def _drive_inspection(self, data, bot, exploration, keys=(), previous_keys=(), turbo=False):
        """One owner: confirmed-token approach/alignment or reporting hold."""
        import math
        import numpy as np
        active=self._inspection
        # Time-based home return may have latched in exploration.update.
        if active is not None and exploration.returning:
            self._release(bot,exploration,'home_reserve_preempts_inspection');active=None
        kwargs=dict(manual=bool(set(keys)&{315,317,314,316}),turbo=turbo,ready=exploration.startup_ready())
        if getattr(exploration,'encounter_holding',False):
            return bot.driving.handle_keys(data,keys,previous_keys,**kwargs,encounter_hold=True)
        if exploration.report_home_holding(data):
            # Use the existing deliberate-rest path, including manual takeover
            # and safety recovery, rather than introducing another motor owner.
            return bot.driving.handle_keys(data,keys,previous_keys,**kwargs,inspection_hold=True)
        if active is None:
            return bot.driving.handle_keys(data,keys,previous_keys,**kwargs,
                mission_active=False if not exploration.startup_ready() else
                ((exploration.enabled and not exploration.done and exploration.target is not None)
                if exploration.status!='manual_destination' else None))
        if active['phase']=='approach':
            frame=bot.driving.frame(data)
            endpoint=active['route'][-1]
            # These millimetre-scale inspection corrections end inside the
            # reporting circle. Capture within2mm rather than chasing a
            # passed point and pivoting away. Navigation bend tolerances stay
            # unchanged; a fresh view/range still gates the actual report.
            if frame is not None and math.dist((frame.x,frame.y),endpoint)<=2.:
                record=self._engine.token_map.records.get(active['id'],{})
                if active.get('observing'):
                    # This endpoint was selected for viewing, outside report
                    # range. Resume exploration immediately; a confirmed token
                    # can request its normal measured reporting approach next.
                    self._release(bot,exploration,'moving_observation_complete')
                    return self._drive_inspection(data,bot,exploration,keys,previous_keys,turbo)
                if not record.get('confirmed'):
                    # Never park to classify an uncertain glyph. Recognition
                    # continues during travel; only a real report earns a hold.
                    self._release(bot,exploration,'close_view_not_authenticated',True)
                    return self._drive_inspection(data,bot,exploration,keys,previous_keys,turbo)
                bot.driving._stop_task()
                active.update(phase='verify'if getattr(self._engine,'_remembered_report_id',None)==active['id']else'restore_view',phase_since=float(data['time']))
                bot.driving._inspection_turn_previous=None
        result=bot.driving.handle_keys(data,keys,previous_keys,**kwargs,
            mission_active=True,inspection_hold=active['phase']!='approach' and active.get('owns_motors',True))
        # Hole recovery and explicit takeover always win over reporting.
        if result.state not in ('inspection_hold','complete','drive','align','maneuver','throat','curve','handoff') and not (
                active['phase']=='planning' and result.state=='idle'):
            if result.state not in ('waiting_for_plan',):self._release(bot,exploration,'driver_'+result.state,True)
            return result
        if active['phase']=='restore_view' and result.state=='inspection_hold':
            # This is the final orientation of a confirmed-token approach,
            # not a search for an unseen or uncertain plaque.
            payload=self._engine._latest_payload or {}
            ready=(data['time']-payload.get('image_time_s',-math.inf)<=float(data.get('sensor_period_s',.016))+1e-9
                and any(o.get('id')==active['id'] and o.get('ready_to_stop')
                        for o in payload.get('objects',())))
            if ready:
                # A fresh valid report view is sufficient; do not finish an
                # unnecessary exact-centering rotation after it is obtained.
                active.update(phase='verify',heading=None,phase_since=float(data['time']))
            else:
                frame=bot.driving.frame(data);heading=active['observation_heading']
                limit=math.pi if active.get('revisit')else math.pi/2+.05
                if (frame is None or abs(math.remainder(heading-frame.heading,2*math.pi))>limit+.001
                        or data['time']-active['phase_since']>(2. if active.get('revisit')else 1.)):
                    self._release(bot,exploration,'inspection_orientation_unavailable',True);return result
                nav=bot.navigator;nav._planning_maps();point=np.array([frame.x,frame.y])
                if not nav._native_collision_engine.check(np.array([point,point+[math.cos(heading),math.sin(heading)]]),frame.heading):
                    self._release(bot,exploration,'inspection_turn_known_sweep_blocked',True);return result
                result=bot.driving.inspection_turn(data,heading)
                if result.reason=='inspection_heading_reached':
                    active.update(phase='settle',heading=None,phase_since=float(data['time']))
        elif active['phase']=='align' and result.state=='inspection_hold':
            frame=bot.driving.frame(data);heading=active['heading']
            if frame is None:
                self._release(bot,exploration,'inspection_frame_unavailable',True)
                return bot.driving._Command(state='blocked',reason='inspection_frame_unavailable')
            active['align_travel']+=abs(math.remainder(frame.heading-active['align_last_heading'],2*math.pi))
            active['align_last_heading']=frame.heading
            if active['align_travel']>=active['align_limit']:
                self._release(bot,exploration,'alignment_fov_budget_exhausted',True);return result
            payload=self._engine._latest_payload or {}
            if payload.get('image_time_s',-math.inf)>active['align_image_time']:
                angle=self._inspection_heading(data,self._engine.token_map.records[active['id']],active['side'])
                if angle is None:
                    self._release(bot,exploration,'alignment_blob_lost',True);return result
                # One image-derived target per inspection. Fresh images may
                # stop it early, never extend it or reverse it for centering.
                if abs(math.remainder(angle-frame.heading,2*math.pi))<=math.radians(3):
                    active.update(phase='verify',heading=None,phase_since=float(data['time']))
                    return result
                active['align_image_time']=payload['image_time_s']
            nav=bot.navigator;nav._planning_maps();engine=nav._native_collision_engine
            point=np.array([frame.x,frame.y]);end=point+np.array([math.cos(heading),math.sin(heading)])
            if not engine.check(np.array([point,end]),frame.heading):
                self._release(bot,exploration,'inspection_turn_known_sweep_blocked',True);return result
            if data['time']-active['align_started']>2.:
                self._release(bot,exploration,'inspection_alignment_timeout',True);return result
            result=bot.driving.inspection_turn(data,heading)
            if result.reason=='inspection_heading_reached':
                active.update(phase='verify',phase_since=float(data['time']))
        elif active['phase']=='approach' and result.state=='complete':
            # A position-only destination can leave a side camera facing away
            # after even a few millimetres of lateral correction. Complete the
            # saved observation pose before asking for a fresh camera image.
            active.update(phase='verify'if getattr(self._engine,'_remembered_report_id',None)==active['id']else'restore_view',phase_since=float(data['time']))
            bot.driving._inspection_turn_previous=None
        if result.state in ('blocked','recovered'):
            self._release(bot,exploration,result.reason,True)
        self._event(exploration,'token_inspection_drive',time=data['time'],id=active['id'],phase=active['phase'],
            camera=active['side'],desired_heading=active.get('heading'),command=result.__dict__)
        return result

    def process_mission(self, data, bot, exploration):
        """Recheck against the FINAL wheel command, send at most one per step."""
        data['motor_command_radps']=list(bot._motor_command)
        self._mission_init()
        result=self.get_detection_return(data)
        presence=result.get('presence') or {}
        presence_state=(presence.get('flag',False),tuple((side,(presence.get('sides')or{}).get(side,{}).get('flag',False))for side in('left','right')))
        if presence_state!=getattr(self,'_last_presence_diagnostic',None):
            self._last_presence_diagnostic=presence_state
            self._event(exploration,'vision_presence',time=data['time'],presence=presence)
        if self._report_time!=data['time'] and exploration.reports_allowed:
            for obj in result['objects']:
                if not obj['can_report']:continue
                packet=self.report_packet(obj['id'],data)
                if packet is None:continue
                try:bot.emitter.send(packet)
                except Exception as error:
                    self._event(exploration,'token_report_send_failed',time=data['time'],id=obj['id'],error=repr(error))
                    break  # Remains unsent; next stopped tick can retry.
                self.mark_report_sent(obj['id']);self._report_time=data['time']
                obj.update(sent=True,can_report=False,needs_verification=False)
                exploration.on_report_sent(data,obj['id'])
                self._event(exploration,'token_report_sent',time=data['time'],id=obj['id'],label=obj['label'],
                    location=obj['location'],packet_hex=packet.hex(),stopped_s=self._engine._latest_payload['stopped_s'],
                    reporting_geometry=dict(zip(('distance_m','allowance_m'),self._engine._record_report_range(data,self._engine.token_map.records[obj['id']]))),
                    retained_geometry={key:self._engine.token_map.records[obj['id']].get(key) for key in
                        ('world_xyz_m','allowance_m','normal_xz','position_quality','pose_anchor','range_observed_s')})
                break
        diagnostic=(self._engine._latest_payload or {}).get('image_time_s')
        if diagnostic!=self._last_diagnostic:
            self._last_diagnostic=diagnostic
            self._event(exploration,'vision_observation',time=data['time'],payload=self._engine._latest_payload)
            if self.debug_enabled:
                # Numeric provenance only: never serialize images, crops, masks
                # or LiDAR arrays. The ordinary DEBUG switch disables this log.
                gates={}
                for side,row in getattr(self._engine,'_observation_rows',{}).items():
                    raw=(self._engine._target_victim_cache.get(side)or{}).get('result')or{}
                    best=(raw.get('raw_result')or{}).get('best')or{}
                    gates[side]=dict(observation={k:row.get(k)for k in
                        ('kind','label','partial','center_type','bbox_px','image_center_px','visual_status',
                         'visual_can_report','visual_report_label','validated_geometry','confirmation_vote',
                         'confirmation_authorized','identity_stable','location')},
                        overhang={k:(raw.get('overhang_debug')or{}).get(k)for k in
                            ('status','decision','bbox','ink_threshold','ink_split','below_max_width','reason')},
                        classifier={k:best.get(k)for k in
                            ('kind','label','label_guess','accepted','reportable','centered','confidence',
                             'template','soft_guess','path','bands','reason')})
                self._event(exploration,'vision_gates',time=data['time'],image_time_s=diagnostic,sides=gates)
        if getattr(self,'_async_enabled',False):
            pipeline_state=(diagnostic,self._pipeline_error)
            if pipeline_state!=getattr(self,'_last_pipeline_diagnostic',None):
                self._last_pipeline_diagnostic=pipeline_state
                self._event(exploration,'vision_pipeline',time=data['time'],**self._pipeline_stats,
                    image_age_s=None if diagnostic is None else data['time']-diagnostic,error=self._pipeline_error)
        bot.token_result=result
        return result

    def _record_camera_input(self, data, bot, exploration):
        """Camera frames remain in RAM, including with DEBUG enabled."""
        return None

    def close(self):
        if getattr(self,'_async_enabled',False):
            self._vision_socket.close()
            if self._vision_process is not None and self._vision_process.poll()is None:self._vision_process.terminate()
        active=getattr(self,'_inspection',None)
        if active and active.get('cancel') is not None:active['cancel'].set()
# END EMBEDDED RescueVision class

class OccupancyMapper:
    """Light, fast occupancy-grid mapper for RoboCup Junior Rescue (Erebus).

    Pipeline (all units in millimetres):
      1. Project the front LiDAR layer using GPS, full IMU attitude and mounting.
      2. Update a numpy log-odds occupancy grid (free along each ray,
         occupied at each hit) - fully vectorised, no per-cell Python loops.
      3. Vote LiDAR hits onto the 120 mm tile-edge lines to detect walls.
      4. Export an Erebus competition matrix (4 matrix cells per tile, walls
         marked "1", start-tile quarters marked "5") and a high-resolution view.

    The Erebus scorer aligns by the start tile ("5") and tries all 4 rotations.
    Projection uses this robot's unchanged mounting and the world's NUE frame;
    map coordinates are East/North and are anchored at the starting GPS pose.
    """

    # --- geometry ---
    TILE_MM = 120
    QUARTER_MM = 60
    GRID_MM = 10                  # high-resolution occupancy cell size
    HALF_WORLD_MM = 3000          # supported half-extent around the start tile

    # --- lidar ---
    FRONT_INDEX = 1024            # selects layer 2 of the configured 4x512 lidar
    # Unchanged custom_robot.proto: parent (-.58,.58,.58,2.09),
    # child (0,0,1,3.14159), translation (0,.0555,0) metres.
    LIDAR_MOUNT_ROTATION = (scipy_Rotation.from_rotvec(
        np.array([-.58,.58,.58])*2.09/math.sqrt(3*.58**2)).as_matrix()
        @ scipy_Rotation.from_euler('z',3.14159).as_matrix())
    LIDAR_MOUNT_MM = np.array([0.0,55.5,0.0])
    LIDAR_VERTICAL_FOV = 0.2     # verified Webots default; live API overrides it
    ANGLE_SIGN = 1.0             # (unused in projection; kept for reference)
    YAW_OFFSET_DEG = 0.0         # robot-heading convention for floor/nav geometry
    MIN_RANGE_MM = 35          # reject camera/body returns inside the 35 mm chassis
    MAX_RANGE_MM = 400            # map nearby walls (~3 tiles) without far phantoms
    BEAM_STEP = 1                 # use every beam (dense, continuous walls)
    FREE_SAMPLES = 10             # samples along each ray for free-space carving
    FREE_GAP_MM = 25             # stop carving this far before the hit (protect walls)
    YAW_SIGN = -1.0             # rotation sense for IMU yaw (derived from E/S GPS axes)

    # --- log-odds ---
    L_OCC = 3
    L_FREE = 1
    L_CLAMP = 30
    OCC_T = 6                     # cell >= OCC_T  -> occupied
    FREE_T = -4                   # cell <= FREE_T -> free

    # --- wall extraction ---
    EDGE_TOL_MM = 18              # how close an occupied cell must be to a tile edge
    WALL_MIN_COVER = 4           # distinct cells that must span an edge to confirm it
    WALL_MIN_SPAN_MM = 50        # occupied points must span this far along the edge
    FREE_EDGE_TOL_MM = 8         # free cells this close to an edge => seen-through (no wall)
    MATRIX_CELL_MIN_HITS = 2     # occupied grid cells needed to mark a matrix wall cell
    MATRIX_CELL_STRONG_HITS = 12 # a small blob with this many hits survives the denoise pass
    MATRIX_MIN_COMPONENT = 3     # drop connected wall blobs smaller than this (lidar speckle)
    MATRIX_SNAP_MM = 30          # snap wall points to nearest 60 mm line (=half spacing -> no curve gaps)
    MATRIX_FIX_CURVES = True      # Erebus rule: a curved wall's bend vertex is '0' (corner has no wall material)
    MATRIX_DENOISE = True         # strip double-wall smear and short vertex-only fragments from the matrix
    MATRIX_DROP_ISOLATED_WALLS = True  # drop matrix '1' cells with no orthogonal wall neighbour: a real wall
                                       # is >=60 mm (3 cells) so it is never a lone cell. Removes obstacle-body
                                       # /speckle false walls; high-res keeps the raw shape.
    MATRIX_AUTO_RESCUE = True     # every frame, complete internal walls the matrix dropped but the high-res
                                 # occupancy clearly shows (curve arcs, grazing half-walls, single-cell lidar
                                 # holes). High-res backed (a template 'wall-gap-wall' can be a real passage,
                                 # so grid rules alone can't fill it), so it only restores REAL walls. Keeps
                                 # internal walls whole -> 'F' closure is then needed only for the outer border.

    # --- scan-time curve evidence for matrix and navigation geometry ---
    # Off-grid returns are candidates, not proof: objects and pose error can
    # produce them too. Local surface geometry rejects grazing/discontinuous
    # views; the quarter-circle fit and measured endpoints confirm a quadrant.
    # Keep that confirmation across occlusion, while permitting contradictory
    # measured geometry to revoke a wrong historical label.
    MATRIX_ARC_CURVES = True
    # At the convex face (radius 65), a 60 mm quarter-circle is only 14.04 mm
    # off both axes even at its midpoint. A 14 mm gate misses almost the entire
    # outside view. Shape validation below separates arcs from noisy wall hits.
    ARC_BAND_MM = 14
    ARC_CONVEX_BAND_MM = 10  # weaker outer-face evidence; requires a seen-free bend
    # Minimum scan votes, in addition to fitted geometry and both endpoints.
    ARC_SOFT_HITS = 10
    # 5 mm wall half-thickness + 7.1 mm cell quantisation + 2.5 mm GPS accuracy.
    ARC_RADIUS_TOL_MM = 15.0

    # --- matrix grid-lock (GPS-drift correction, MATRIX ONLY) ---
    # In areas 1-3 every straight wall sits on a 60 mm sub-grid line, so an
    # axis-aligned wall cell's distance to its nearest 60 mm line is exactly the
    # accumulated GPS drift along that axis. We measure that drift from the wall
    # population and remove it before rasterising the matrix, so slow GPS drift
    # over a big map can never push walls onto the wrong tile line. The high-res
    # map is never touched. Estimate is a robust median and is hard-bounded, so
    # when walls are already aligned the correction is ~0 (output unchanged).
    MATRIX_GRID_LOCK = True
    MATRIX_GRID_LOCK_BAND_MM = 22    # only cells this close to a 60 mm line vote for the drift
    MATRIX_GRID_LOCK_MIN_VOTES = 30  # need this many in-band cells per axis to trust the estimate
    MATRIX_GRID_LOCK_MAX_MM = 28     # never shift more than ~half the 60 mm spacing

    # --- border / wall closure (MATRIX ONLY, runs only when toggled with 'F') ---
    # Lidar sometimes never reaches a wall (grazing angle, occlusion), so the
    # rasteriser drops it even though the arena is fully enclosed by walls. The
    # closure pass uses the live high-res occupancy (NOT a saved artefact, so it
    # is sharper than any PNG) to (1) RESCUE matrix wall cells that the denoise /
    # curve passes removed but whose high-res clearly shows a wall connected to
    # the existing structure, and (2) CLOSE the border where an explored/free
    # cell sits against never-observed space across a tile-edge line. It is gated
    # behind 'F' so the normal auto-matrix and already-correct maps are never
    # touched; press 'F' once exploration is done to finish the border.
    CLOSURE_RESCUE_MIN_OCC = 4   # high-res occupied cells (10 mm) in the 30 mm matrix cell to rescue a wall
    CLOSURE_RESCUE_MAX_FREE = 5  # but skip if the cell reads mostly free (it was genuinely seen through)
    CLOSURE_BORDER_FREE_MIN = 4  # free cells on the explored side to treat an edge as a real border
    CLOSURE_BORDER_UNK_MIN = 5   # unknown cells on the far side to treat the far side as outside
    CLOSURE_FREE_BLOCK_MIN = 3   # >= this many free 10 mm cells (with 0 occupied) => never wall it

    # --- legacy occupancy-component obstacle fallback ---
    # These limits govern compact, isolated components off the60mm grid with
    # an observed-free perimeter. Bounded raw-face models independently identify
    # objects whose voxels merge with a nearby wall. The object's centre selects
    # its120mm tile; its body may cross a tile edge. Both paths preserve physical
    # occupancy for navigation, exclude identified body cells from semantic wall
    # conversion, and mark the tile's four interior quarters with 'x'.
    OBSTACLE_DETECT = True
    OBST_MAX_MM = 118            # bounding box > this in either axis => it spans a tile => wall
    OBST_MIN_SIDE_MM = 20        # both sides must be >= this (else a 1-cell wall-thickness sliver)
    OBST_MIN_CELLS = 6           # ignore tiny speckle (< this many 10 mm cells)
    OBST_MAX_CELLS = 200         # safety cap - a bigger blob is a wall network
    OBST_TILE_TOUCH_MM = 8       # if the blob's surface reaches within this of a 120 mm tile boundary => wall
    OBST_LINE_BAND_MM = 15       # a cell within this of a 60 mm line counts as "on the grid"
    OBST_MIN_OFFGRID = 0.10      # >= this fraction of cells must be OFF the 60 mm grid (else a straight-wall fragment)
    OBST_MIN_FILL = 0.35         # blob must fill >= this of its bbox (a box is solid; a stray wall arc is thin)

    # --- legacy camera<->lidar obstacle fusion (disabled) --------------------
    # Its fixed horizon-row/side-bearing assumptions do not match the tilted
    # cameras in robot.json: floor pixels can be assigned to distant lidar hits.
    # Keep the optional diagnostic code, but use LiDAR geometry as authority.
    # A wall has ONE consistent look (a colour/texture the whole arena shares,
    # only lit brighter or in shadow darker). An obstacle is ANY surface whose
    # look departs from that wall look - we never assume the obstacle's own
    # colour (it can be anything). The left/right side cameras are fused with the
    # matching side of the 360 deg lidar: the lidar says WHERE a surface is (and
    # its bearing -> which image column), the camera says WHETHER that surface is
    # the wall material or something else. We work in HSV but throw away V
    # (brightness), so a wall in shadow and a wall in light look identical and
    # only a genuine material change trips it - no colour calibration needed.
    CAM_FUSE = False
    CAM_FOV_RAD = 1.0            # per-camera horizontal FOV (overridden live from the device)
    # Sampling band = a thin horizon stripe AT / just ABOVE the optical centre.
    # A point at camera height projects to the vertical CENTRE of the image at
    # ANY depth, and the floor is always BELOW that (it projects lower the nearer
    # it is). So a band ending at the centre never sees the floor, whatever the
    # range - which is what stops a gray floor tile (identical in colour to a
    # gray obstacle) from ever being sampled. Above the stripe the wall is lit a
    # different hue, below it lies the floor, so the stripe is kept narrow.
    CAM_BAND_FRAC = (0.4375, 0.515)  # rows ~28..32 of a 64 px frame (lidar-height horizon)
    CAM_RANGE_MAX_MM = 1000     # only classify NEAR surfaces (colour reliable); farther => defer
    CAM_RANGE_MIN_MM = 40       # ignore self / absurd close returns
    CAM_S_FLOOR = 0.22          # a column this saturated is chromatic -> feeds the wall reference
    CAM_CHROMA_FRAC = 0.5       # chroma-distance from the wall ref > this*Sw => obstacle (any hue/sat)
    CAM_MIN_SEG = 3             # an obstacle needs >= this many adjacent obstacle columns
    CAM_GAP = 2                 # merge obstacle column runs split by a gap <= this
    CAM_EDGE = 2                # ignore this many extreme edge columns (projection least trustworthy)
    CAM_WALLREF_MIN = 40        # need this many chromatic samples before the wall ref is trusted
    CAM_CONFIRM = 2             # a tile needs obstacle votes in >= this many distinct frames
    # Verticality cross-check: only classify a column where the lidar layer just
    # BELOW the reference layer returns the SAME range (within this tol). A wall
    # or an obstacle is a vertical surface, so the down-tilted beam hits it at
    # the same distance; a flat COLOURED FLOOR TILE (swamp / checkpoint / area
    # boundary) has no vertical extent, so the two layers disagree -> the floor
    # colour is dropped and can never be mistaken for an obstacle.
    CAM_VERT_TOL_MM = 60
    CAM_VICTIM_WHITE_V_MIN = 0.88   # ~224/255; must exceed the ~0.753 arena floor
    CAM_VICTIM_WHITE_S_MAX = 0.22
    CAM_VICTIM_WHITE_ROWS = (0.4375, 0.78)  # horizon band through mid-lower frame
    CAM_VICTIM_WHITE_FRAC = 0.06    # min fraction of probe pixels that look white
    CAM_VICTIM_WHITE_MARGIN = 2     # widen the column probe around the segment
    CAM_VICTIM_REL_DV = 0.10        # ...or this much brighter than nearby wall cols
    CAM_VICTIM_FLUSH_MM = 10.0      # <= this protrusion beyond the wall => a decal
    CAM_FLUSH_FLANK_COLS = 3        # wall columns sampled each side of the segment

    # --- floor tile colour mapping (side cameras look DOWN at the floor) ------
    # The 9 special floor markings and the exact characters MapAnswer stamps on
    # a tile's 4 interior quarter cells (RCJ rules). The six area-connection
    # tiles are strong flat colours; hole is near-black; swamp/checkpoint are
    # textured. We classify by HUE (lighting-invariant) with WIDE bins so a
    # brighter/darker lighting never needs recalibration; V (brightness) is only
    # used to catch the near-black hole and to reject the unlit background.
    #   b 1->2 blue | y 1->3 yellow | g 1->4 green | p 2->3 purple |
    #   o 2->4 orange | r 3->4 red | 2 hole | 3 swamp | 4 checkpoint
    FLOOR_FUSE = True
    # Side-camera floor colour fusion detects coloured / swamp / checkpoint tiles
    # to the LEFT and RIGHT of the robot. Disabled by default because coloured
    # walls/signs can appear in the side cameras and be mistaken for floor tiles.
    # The downward colour sensor + distance/cliff path still handles floor tiles.
    FLOOR_SIDE_CAM_FUSE = False
    # Never commit a black hole ('2') from the side cameras - the downward
    # distance/cliff sensor is the sole, reliable hole source. Side-camera '2'
    # reads (dark wall bases / shadows seen from far) were the main false positive.
    FLOOR_SIDE_CAM_HOLES = False
    FLOOR_HUE_BINS = (               # (char, hue_lo, hue_hi) for S >= conn-sat
        ("r", 345.0, 360.0), ("r", 0.0, 20.0),
        ("o", 20.0, 52.0),
        ("y", 52.0, 78.0),
        ("g", 78.0, 175.0),
        ("b", 200.0, 251.0),
        ("p", 251.0, 300.0),
    )
    FLOOR_CONN_SAT_MIN = 0.62    # >= this + a hue bin => a connection colour
    FLOOR_MIN_V = 0.30          # below this (but above hole) => too dark to trust hue
    FLOOR_SWAMP_SAT_MIN = 0.32  # swamp: medium saturation ...
    FLOOR_SWAMP_HUE = (15.0, 60.0)   # ... in the warm/tan hue range
    FLOOR_CP_SAT_MAX = 0.45     # checkpoint: low saturation ...
    FLOOR_CP_V = (0.18, 0.60)   # ... mid-dark value ...
    FLOOR_CP_BLUE_MIN = 0.03    # ... and a blue tint (B-R); plain floor/shadow is neutral
    FLOOR_HOLE_V_MAX = 0.12      # V below this AND near-neutral => hole '2'. A real
                                 # hole is a BLACK tile (very dark, no chroma). The
                                 # dark shade of the cyan/dark-cyan WALL sits at
                                 # V~=0.125 with a strong cyan tint (S~=0.43): above
                                 # this V and far from neutral, so it can never be
                                 # read as a hole even if the lidar floor gate leaks.
    FLOOR_HOLE_SAT_MAX = 0.35    # a hole must also be near-neutral (no colour cast) -
                                 # this is the robust separator that rejects a dark
                                 # COLOURED wall (cyan, S~=0.43) from ever being a
                                 # hole, while a black tile (S~=0) still passes.
    FLOOR_DEFAULT_RGB = (0.753, 0.753, 0.753)   # plain floor tile as RENDERED (gray ~192/255)
    FLOOR_H = (
        (-0.17413291209216097, -0.13327749776115366, 31.58233693116695),
        (-0.02542259162693492, -0.26077879935972060, 52.81842309486207),
        (-0.00041705692837242, -0.00419557214867722,  1.0),
    )
    FLOOR_ROW0 = 30              # classify / sample the floor region from this row down
    FLOOR_INSET_MM = 52.0       # sample the inner +/- this of the 120 mm tile
    FLOOR_KGRID = 11            # K x K ground sample grid over the tile interior
    FLOOR_RAD_MAX_MM = 150.0    # far cap for a FORWARD / diagonal neighbour tile centre.
    FLOOR_SIDE_F_MAX_MM = 45.0  # |forward offset| below this => a PURE SIDE tile (R/L),
    FLOOR_SIDE_RAD_MAX_MM = 178.0  # ...which is allowed farther (an adjacent column tile
                                # sits up to 120+offset away when the robot hugs the far
                                # edge of its own tile) and uses the near-edge test.
    FLOOR_LAT_MIN_MM = 60.0     # a sample must be at least this far to the side: closer
                                # than that the tile sits under the robot and the
                                # homography projects unreliably. Kept low so a tile the
                                # robot straddles (half under it) is still read from its
                                # visible half; the centre-consistency gate below is what
                                # actually blocks a near plain tile bleeding to a colour.
    FLOOR_AZ_MAX_RAD = 0.55     # only trust a footprint sample whose forward BEARING sits
                                # this far inside the side view. Gating per SAMPLE (not per
                                # tile centre) means a near / own tile still offers plenty
                                # of straight-to-the-side samples while a far / oblique
                                # tile offers none -> detected vs. rejected.
    FLOOR_OCC_MARGIN_MM = 60.0  # a lidar surface only occludes a floor sample if it is
                                # this close to / nearer than the sample's ground range;
                                # a wall BEYOND a near tile no longer hides that tile.
    FLOOR_SIDE_WALL_MIN_MM = 90.0  # side camera must be at least 9 cm from a side wall;
                                   # closer than this, coloured wall pixels can fill the
                                   # floor band and must not become floor-tile votes.
    FLOOR_SIDE_WALL_COLW = 8       # half-width of camera columns used for that side gate
    FLOOR_MIN_PIX = 14          # min reliable-region lidar-floor samples covering the tile
    FLOOR_MIN_CLASS = 10        # min samples that classify to a colour
    FLOOR_MIN_ROWS = 3          # winning colour must span >= this many image rows
                                # (a solid tile fills many rows; a 1-2 px wall/floor
                                # seam of swamp-coloured pixels collapses onto ~1 row)
    FLOOR_COVER_FRAC = 0.45     # colour must cover >= this share of the sampled tile
    FLOOR_VOTE_FRAC = 0.60      # dominant colour >= this share of the classified samples
    FLOOR_CENTER_R_MM = 30.0    # FORWARD/diagonal tiles: their geometric CENTRE (within this
                                # radius) must read the winning colour - kills a neighbour
                                # bleeding onto an edge. Reliable because a forward tile's
                                # centre projects mid-frame.
    FLOOR_NEAR_BAND_MM = 45.0   # PURE SIDE tiles: their NEAR edge (samples within this of
                                # the closest-to-robot sample) must read the winning colour.
                                # Used instead of the geometric centre because a far side
                                # tile's centre sits near the unreliable horizon.
    FLOOR_CENTER_MIN = 3        # min classified samples in the consistency region
    # --- NEAR path (the tile the robot is on / straddling) ---
    # The side cameras are mounted +/-200 mm out, so the very bottom of the frame
    # shows the floor a fixed ~D lateral offset to that side (NOT the robot
    # centre). We read the dominant colour in that bottom band and attribute it to
    # the tile at robot_centre + side * FLOOR_NEAR_D_MM (GPS + IMU) - which is the
    # tile whose floor the camera is actually looking at, whether that is the tile
    # the robot stands on or the one it straddles into.
    FLOOR_NEAR_ROWS = 8         # bottom-of-frame band height (the nearest floor)
    FLOOR_NEAR_COLW = 14        # half column window around the straight-to-side ray
    FLOOR_NEAR_D_MM = 65.0      # effective lateral distance the bottom band looks at
    FLOOR_NEAR_MIN_PIX = 25     # min lidar-floor pixels in the band
    FLOOR_NEAR_COL_FRAC = 0.35  # >= this share of the band must be a colour (not plain)
    FLOOR_NEAR_VOTE = 0.60      # dominant colour >= this share of the coloured pixels
    FLOOR_YAW_SNAP_DEG = 8.0    # if the heading is within this of a grid-aligned cardinal
                                # (start_yaw + k*90), snap to it - removes IMU noise so the
                                # side tile geometry is exact when driving along the grid.
    FLOOR_CONFIRM = 3           # commit a tile only after this many agreeing frames
    # A real special FLOOR tile is floor: the lidar beam passes over it, so its
    # INTERIOR CORE (well inside the 60 mm edge lines, where walls legitimately
    # live) carries no occupancy. If a wall / obstacle body fills that core the
    # "tile" is really a wall - discard the colour even if it classified (a hole
    # "under a wall" cannot exist). This is the lidar-vs-camera fusion safety net.
    FLOOR_CORE_HALF_MM = 40.0   # half-size of the tile core sampled (excludes edges)
    FLOOR_CORE_OCC_MAX = 6      # >= this many occupied core cells => it's a wall, drop
    # --- SIDE-AIM detection: mark the tile the side vector points at ---------- #
    # The side camera aims perpendicular to the robot. Using GPS+IMU we find the
    # 120 mm tile whose CENTRE that vector aims at and read its colour there. This
    # works when the robot is half on the tile or driving diagonally (footprint
    # gating missed both) yet still only ever considers a right/left tile.
    FLOOR_AIM_F_MAX = 45.0      # tile centre must sit within this of the side line
                                # (perpendicular offset) - i.e. the vector aims at it.
    FLOOR_AIM_S_MIN = 30.0      # nearest trustworthy lateral distance to a tile centre
    FLOOR_AIM_S_MAX = 170.0     # farthest (beyond this the tile projects above the
                                # reliable floor band, near the horizon)
    FLOOR_AIM_LAT_FLOOR = 24.0  # drop samples nearer than this (too far under robot)
    FLOOR_AIM_AZ_MAX = 1.2      # per-sample bearing limit (rad); looser than the old
                                # footprint path so a CLOSE tile still offers samples.
    FLOOR_MIN_VIEWS = 2         # a commit needs agreeing votes from at least this many
                                # DISTINCT robot viewpoints (below) - a one-vantage
                                # misread (a wall filling the camera) never accumulates
                                # across positions, a real tile does.
    FLOOR_VIEW_BIN_MM = 35.0    # robot-pose quantisation for "distinct viewpoint": the
                                # robot must have moved ~this far between agreeing views.
    # render colour for each committed special tile on the high-res map (they are
    # stamped on the exact 120 mm cell, same grid the matrix uses).
    FLOOR_HIRES_PALETTE = {
        "2": (0.08, 0.08, 0.08),   # hole (black)
        "3": (0.85, 0.75, 0.20),   # swamp
        "4": (0.30, 0.55, 0.95),   # checkpoint
        "b": (0.20, 0.35, 0.95),   # 1->2 blue
        "y": (0.90, 0.90, 0.15),   # 1->3 yellow
        "g": (0.15, 0.75, 0.20),   # 1->4 green
        "p": (0.55, 0.20, 0.75),   # 2->3 purple
        "o": (0.95, 0.60, 0.15),   # 2->4 orange
        "r": (0.90, 0.20, 0.20),   # 3->4 red
    }

    # --- downward COLOUR SENSOR -> special-tile fusion -----------------------
    # A narrow-FOV colour camera points DOWN and forward at the floor, mounted at
    # the robot's front edge. Per the Erebus robot spec (Kavosh-1.0.json, turned
    # into the proto by ProtoGenerator: json coord / 10000 = metres, and local
    # -z is the driving/forward axis), the colour sensor sits at local
    # z = -370/10000 = 0.037 m ahead of the GPS/robot centre. The tilted optical
    # ray reaches the floor at COLOR_SENSOR_FWD_MM (see below). The colour belongs to the tile
    # at  centre + COLOR_SENSOR_FWD_MM * heading  (GPS position + IMU yaw), NOT
    # the tile under the robot centre. That fused world point is snapped to the
    # fixed 120 mm grid (anchored at the start tile) to get the tile index, so it
    # works for ANY robot orientation and even while the robot straddles tiles.
    COLOR_SENSOR_FUSE = True
    # Camera origin is 37 mm forward and 8.5 mm above the floor. Its optical
    # +X ray tilts forward: the FLOOR sample is another 8.39 mm ahead of its
    # housing (robot.json transform), so project the sample, not the housing.
    COLOR_SENSOR_FWD_MM = 45.39
    CS_MEDIAN_WIN = 5            # low-pass: classify the median of the last N reads
    CS_CONFIRM = 3               # commit only after this many agreeing frames in a row
    CS_CONN_SAT_MIN = 0.55       # S >= this => a vivid connection colour (real 0.69-0.85)
    CS_CONN_V_MIN = 0.45         # ... and bright enough to trust the hue
    # hue -> connection char. Orange (real render [254,239,73], H ~55) sits very
    # close to yellow (real [254,254,73], H 60): the split is at 58 deg. (This is
    # a colour-sensor-specific copy of FLOOR_HUE_BINS so tuning it here never
    # affects the side-camera classify_floor_rgb.)
    CS_HUE_BINS = (
        ("r", 345.0, 360.0), ("r", 0.0, 20.0),   # red    ~0
        ("o", 20.0, 58.0),                         # orange ~55
        ("y", 58.0, 80.0),                         # yellow ~60
        ("g", 80.0, 175.0),                        # green  ~120
        ("b", 200.0, 251.0),                       # blue   ~240
        ("p", 251.0, 300.0),                       # purple ~273
    )
    CS_SWAMP_HUE = (15.0, 52.0)  # warm/tan hue window for swamp (real H ~43)
    CS_SWAMP_SAT = (0.30, 0.55)  # swamp saturation: warm but below a connection tile
    CS_CP_SAT_MAX = 0.45         # checkpoint: low/medium saturation ...
    CS_CP_V_MIN = 0.12          # CorrodedMetal brightness varies with texture/view;
                                # its blue tint distinguishes it from plain floor.
    CS_CP_BLUE_MIN = 0.02        # ... and a BLUE tint (B-R, 0..1): separates the
                                 # checkpoint from the neutral plain floor AND from
                                 # a neutral near-black hole (both have B==R)
    # --- BLACK HOLE via the downward DISTANCE (cliff) sensor -----------------
    # A hole is a PIT: reading it with the colour sensor means the robot is about
    # to fall in. The distance sensor instead reads the FLOOR just ahead (~9-12 cm
    # on the flat floor); over a hole the beam drops into the pit and the reading
    # jumps toward its 80 cm max. So the hole is detected from a safe distance,
    # and the colour sensor is freed from ever calling a dark tile/shadow a hole.
    HOLE_FROM_DISTANCE = True
    # robot.json: sensor at (0,36,-37) mm, optical +X points down/forward
    # with components (0,-0.546535,-0.837430). Flat-floor intersection:
    # 37 + 36 * 0.837430 / 0.546535 = 92.16 mm from the robot centre.
    DIST_SENSOR_FWD_MM = 92.16
    DIST_SENSOR_HEIGHT_CM = 3.6
    DIST_SENSOR_DOWN = 0.546535
    DIST_IR_MIN_REFLECTION = 0.2  # Webots R2023b infra-red response lower bound
    DIST_CLIFF_DELTA_CM = 6.0    # reading this far OVER the learnt floor level =>
                                 # a cliff/hole (floor ~9-12 cm, a pit reads far more)
    DIST_CLIFF_STRONG_CM = 20.0  # without a dark pit colour, require at least this
                                 # much extra range before committing a hole
    DIST_FLOOR_EMA = 0.05        # how fast the learnt floor-distance baseline adapts
    DIST_FLOOR_MIN_CM = 5.0      # only learn baseline from readings in this band ...
    DIST_FLOOR_MAX_CM = 20.0     # ... (flat floor ~9-12 cm; ignore cliff readings)
    CS_CLIFF_CONFIRM = 3         # a cliff must persist this many frames to commit
    CS_CENTER_MARGIN_MM = 55.0   # |offset| along each axis from the 120 mm tile centre
                                 # (tile half-width is 60 mm, so this stays inside it).
    CS_CENTER_R_MM = 75.0        # ...and the radial offset must also be <= this (mm);
    CS_CONNECTION_CHARS = frozenset({"r", "o", "y", "g", "b", "p"})
    CS_COMMIT_GOOD_DIST_MM = 42.0   # sensor projection well inside one tile
    CS_COMMIT_EDGE_DIST_MM = 65.0
    CS_CONFIRM_EDGE = 8             # frames required for an edge read to commit
    CS_LOCK_RUN = 10                # lock after this many agreeing frames ...
    CS_LOCK_DIST_MM = 45.0          # ... or after a commit this well centred
    CS_MAX_SPEED_MMPS = 160.0   # maximum measured translation speed for floor commits
    CS_MAX_YAW_RATE_DPS = 40.0  # low-rate shortcut; moving footprints are checked separately

    # --- grid snap (remove wall-thickness bleed stubs off the 60 mm grid) ---
    MATRIX_SNAP_GRID = True
    SNAP_WIN_CELLS = 4           # high-res half-window (x10 mm) to measure along/across extent
    SNAP_PERP_MIN = 3            # drop an edge cell whose occupancy runs >= this much MORE across than along
    SNAP_WEAK_MIN = 4            # ...or that has < this much occupancy along its own line (and no solid run)

    # --- runtime / output ---
    GPS_Y_SIGN = -1.0            # GPS +y (gps[2]) points South; flip so map +y = North
    MOVE_EPS_MM = 10             # re-integrate only after moving this far (avoids jitter smear)
    YAW_EPS_DEG = 2              # ...or after rotating this much
    # During fast translation the low-lag GPS still trails the true pose by ~1
    # sample, so scans land slightly behind and the walls smear along the path.
    # Skip integration while the robot is moving faster than this; the freshest
    # in-place scan is integrated as soon as it slows, so the live view stays
    # sharp instead of flickering. Tune against the logged 'speed_mmps'.
    MAP_MAX_SPEED_MMPS = 90      # don't paint walls while driving faster than this
    MAP_SETTLE_SPEED_MMPS = 20   # at/below this the robot is treated as settled
    FLOOR_MAX_SPEED_MMPS = 160   # side-camera floor fusion runs up to this speed
                                 # (matches the downward-sensor gate); above it the
                                 # pose is too uncertain to place a side tile
    DRAW_PERIOD_S = 0.3
    SHOW_HIGH_RES_MAP = globals().get("SHOW_MAPS", True)
    SHOW_MATRIX_MAP = globals().get("SHOW_MAPS", True)
    JSON_PATH = 'kavosh_map.json'
    HIGH_RES_MAP_PNG = 'kavosh_map_highres.png'
    MATRIX_MAP_PNG = 'kavosh_map_matrix.png'
    SAVE_MAP_PNG = True

    # --- live map windows (fixed cell size + scroll/zoom) ---
    MAP_HI_PX_PER_CELL = 4         # screen pixels per high-res grid cell (10 mm)
    MAP_MATRIX_PX_PER_CELL = 1     # matrix image is already 16 px/cell; 1 = native
    MAP_FIG_DPI = 96
    MAP_FIG_MAX_W_PX = 960         # cap window width; larger maps scroll inside
    MAP_FIG_MAX_H_PX = 720
    MAP_FIG_MIN_W_PX = 420
    MAP_FIG_MIN_H_PX = 320
    MAP_ZOOM_SCROLL = 1.15         # mouse-wheel zoom factor per notch

    # --- debug dump (handy for tuning lidar projection / wall gaps) ---
    DEBUG_MAPPING_DUMP = globals().get("DEBUG", True)
    DEBUG_GAP_DUMP_FILE = 'mapping_gap_debug.json'
    DEBUG_MAX_ITEMS = 20

    def __init__(self):
        n = 2 * (self.HALF_WORLD_MM // self.GRID_MM) + 1
        self.n = n
        self.c = n // 2                              # grid index of the origin
        self.logodds = np.zeros((n, n), dtype=np.int16)   # [row = y, col = x]
        self.visited = np.zeros((n, n), dtype=bool)
        # scan-time curve evidence: count of occupied hits that landed OFF both
        # nearest 60 mm grid lines (i.e. on a curved wall's arc). Filled at float
        # precision during integration; consumed only by the matrix curve pass.
        self.arc_grid = np.zeros((n, n), dtype=np.int32)
        self.convex_arc_grid = np.zeros((n, n), dtype=np.int32)

        self.origin_x = None
        self.origin_y = None
        self.start_tile = (0, 0)
        self.visited_tiles = {(0, 0)}
        self._current_room = 1
        self._room_tiles = {(0, 0): 1}
        self._room_model_signature = None
        self._area4_tiles = set()
        self._competition_floor_area4 = set()
        self._competition_border_area4 = set()
        self._competition_border_context = None
        self._nonpassage_floor_tiles = set()
        self._nonpassage_read_tile = None
        self._nonpassage_read_run = 0
        self._room_last_tile = (0, 0)
        self._room_entry_tile = None
        self._room_pending_passage = None
        self._room_transitions = []

        self._v_walls = set()       # confirmed walls on the EAST edge of tile (tx, ty)
        self._h_walls = set()       # confirmed walls on the +Y edge of tile (tx, ty)

        self.report_matrix = [["0"]]
        self.wall_hits = 0

        # free-standing obstacles: tiles marked 'x' and the high-res cells of
        # their shapes (kept for navigating around them)
        self.obstacle_tiles = set()
        self._obstacle_mask = None
        self._confirmed_obstacle_mask = None
        self._raw_obstacle_cells = {}
        self._pending_nonwall_cells = set()
        self._raw_obstacle_regions = {}
        self._raw_curve_wall_cells = set()
        self.last_obstacle_fit_debug = []
        self.last_curve_raster_debug = []
        # Current component detections, retained under the old debug field names.
        # Rebuilt each time: later wall evidence can revoke a premature label.
        self._persistent_obstacle_cells = set()   # {(gy, gx)}
        self._persistent_obstacle_tiles = set()   # {(tx, ty)}

        # --- camera<->lidar fusion state ---------------------------------- #
        # Rolling GLOBAL wall-colour reference, learned online from every
        # chromatic near-surface column (walls dominate, so their look is the
        # reference; obstacles are whatever deviates from it). Stored as the
        # running mean chroma vector (Sw*cos Hw, Sw*sin Hw) so it is trivially
        # updatable and hue-wrap safe.
        self._wallref_cx = 0.0
        self._wallref_cy = 0.0
        self._wallref_n = 0
        # per-tile obstacle evidence: how many DISTINCT frames a tile was seen
        # as a non-wall surface by a camera. A tile is only committed once it
        # clears CAM_CONFIRM frames (multi-frame => no one-shot false 'x').
        self._obst_votes = {}                     # {(tx, ty): frame_count}
        self._cam_obstacle_tiles = set()          # committed obstacle tiles
        self.last_cam_fusion = {}                 # debug snapshot of the last fuse pass
        # per-tile FLOOR-colour evidence: how many DISTINCT frames a tile beside
        # the robot classified as each map char. A tile is only committed once a
        # single char clears FLOOR_CONFIRM frames (multi-frame => no false tile).
        self._floor_votes = {}                    # {(tx, ty): {char: frame_count}}
        # distinct robot VIEWPOINTS (quantised pose bins) that voted each char for
        # a tile. A real tile is seen from several positions as the robot maps; a
        # transient misread from one bad vantage is not - so a commit needs votes
        # from >= FLOOR_MIN_VIEWS distinct positions, not just N frames from one spot.
        self._floor_views = {}                    # {(tx, ty): {char: set(pose_bins)}}
        self._committed_floor_tiles = {}          # {(tx, ty): committed char}
        self.last_floor_fusion = {}               # debug snapshot of the last floor pass

        # downward colour-sensor -> special-tile fusion state:
        #  * a short RGB buffer for the median low-pass filter,
        #  * a per-tile, per-colour confirmation run. Adjacent tiles can share
        #    a colour, so evidence and identity must not be merged across cells.
        self._cs_rgb_hist = deque(maxlen=self.CS_MEDIAN_WIN)
        self._cs_last_tile = None                 # last sensor tile (hist reset)
        self._cs_confirm_char = None              # class being confirmed this run
        self._cs_run_len = 0                      # consecutive frames of that class
        self._cs_run_best_tile = None             # best-centred tile during the run
        self._cs_run_best_dist = None             # its centre distance (mm)
        self._cs_commit_dist = {}                 # {tile: best centre-dist at commit}
        # diagnostic log of special-tile COMMITs (and blocked/vetoed events) so a
        # capture shows the exact pose/geometry at the moment a tile was stamped -
        # the single 'S' snapshot never catches that moment. Keyed by the OUTCOME
        # (kind, tile, committed, blocked) and keeps the MOST-centred frame per
        # key, so a robot that sits on one tile for thousands of frames still
        # leaves exactly one entry and never evicts the other specials.
        self._floor_commit_log = {}               # {outcome_key: best record}
        self._floor_commit_seq = 0                # insertion order for the log
        # distance-detected holes with NO black pit-colour confirmation are only
        # PROVISIONAL: a real pit reads a cliff every frame the beam points at it,
        # but a transient distance spike (a tilt while the robot is stuck) reads a
        # cliff once and then normal floor. A provisional hole is removed when the
        # beam later reads normal floor on that same tile, or colour shows floor.
        self._provisional_holes = set()           # {tile} strong-only holes
        self._cs_locked_tiles = set()             # high-confidence commits: no overwrite
        self._cs_prev_yaw = None                  # for the turn-rate stability gate
        self._cs_prev_t = None
        self._dist_floor_cm = None                # learnt flat-floor distance (cliff base)
        self._cs_cliff_tile = None                # hole tile being cliff-confirmed
        self._cs_cliff_run = 0                    # consecutive cliff frames on that tile
        self.last_color_sensor_fusion = {}        # debug snapshot of the last colour read

        self.last_lidar_debug = {}
        self.last_free_space_debug = {}
        self.last_projected_points = []
        self.debug_snapshot = {}

        self.last_x = None
        self.last_y = None
        self.last_yaw = None
        # yaw the robot booted at. By the rules the robot always starts aligned to
        # a cardinal (N/E/W/S) and the whole tile grid is axis-parallel to those
        # axes, so start_yaw + k*90 are the exact grid-aligned headings. We use
        # this to snap out IMU noise/bias when the robot is near grid-aligned,
        # which makes the side floor-tile geometry (which colour tile is where)
        # exact in the common case.
        self._start_yaw = None
        self._yaw_offset_deg = 0.0     # IMU yaw reading at start minus the true cardinal
        self._gps_ref_mm = None        # GPS the 120 mm grid is anchored to (start tile centre)
        self._physical_evidence = None
        self._physical_coarse_guard = set()
        self.last_settled = False
        self.last_draw = -1e9

        # Per-frame position history for the translation-speed gate.
        self._prev_fx = None
        self._prev_fy = None
        self._prev_ft = None
        self.last_speed = 0.0
        self.last_speed_source = "position-difference"

        # last estimated global GPS drift removed from the matrix raster (mm)
        self.last_grid_offset = (0.0, 0.0)

        # border/wall closure toggle (driven by the 'F' key); off by default so
        # the normal matrix and already-correct maps are never altered
        self.closure_enabled = False
        self.last_closure_added = 0
        # Individual closure passes. RESCUE (high-res backed), BORDER (free vs
        # unknown edge) and VERTEX (straight-through) only ever add walls the
        # robot actually has evidence for, so they are safe. The two flood-based
        # passes OUTER and MEMBRANE are OFF: they seal exterior "leaks" through
        # unobserved gaps, which repeatedly bridged an interior wall to the outer
        # boundary and fabricated impossible closed loops (a wall behind the
        # outer wall the robot can never have mapped). Leaving them off keeps the
        # arena outline honest.
        self.CLOSURE_PASS_RESCUE = True
        self.CLOSURE_PASS_BORDER = True
        self.CLOSURE_PASS_OUTER = False
        self.CLOSURE_PASS_VERTEX = True
        self.CLOSURE_PASS_MEMBRANE = False

        # vertices demoted to curved-wall bend '0' by the curve pass; protected
        # from later passes (closure) and reported for debugging
        self._curve_bends = set()
        # Retain curved-wall bends across rebuilds and viewpoints. Current
        # occupancy/arc geometry can revoke an incorrectly identified bend.
        self._persistent_curve_bends = set()
        self._confirmed_curve_quadrants = set()
        self._scan_curve_votes = {}
        self._native_curve_samples = False
        self.last_curve_fit_debug = {}
        self._confirmed_straight_walls = set()
        self._horizontal_surface_hits = np.zeros_like(self.arc_grid)
        self._vertical_surface_hits = np.zeros_like(self.arc_grid)

        # when True, build_report_matrix records a copy of the matrix after each
        # stage (raster / denoise / curve / closure / odd-guard) so a capture can
        # dump exactly what every pass added or removed for offline debugging
        self._record_stages = False
        self._stage_snapshots = {}
        self._matrix_meta = {}
        # raw scan + pose of the most recent integrated frame (for raw dumps)
        self._last_scan = None
        self._last_pose = None

        if self.SHOW_HIGH_RES_MAP or self.SHOW_MATRIX_MAP:
            plt.ion()
        self.fig_hi = self.ax_hi = self.img_hi = None
        self.fig_rep = self.ax_rep = self.img_rep = None
        self._map_hi_user_view = False
        self._map_rep_user_view = False

    # ------------------------------------------------------------------ #
    #  Main entry
    # ------------------------------------------------------------------ #
    def update(self, data, show=None):
        lidar = data["lidar"]
        if lidar is None or len(lidar) == 0:
            return

        # Mapping uses motion-compensated GPS averaging when legal world
        # velocity is available. The short median remains the fallback for old
        # streams: its noise and movement lag can accumulate thick occupied
        # bands, which is why it cannot qualify the fine physical layer.
        pos = data["position"]
        x = pos.get("map_x", pos["x"])
        y = self.GPS_Y_SIGN * pos.get("map_y", pos["y"])
        # Keep display rounding out of the geometric projection. At 400 mm,
        # half a degree alone shifts a return by approximately 3.5 mm.
        yaw = data["orientation"].get("yaw_raw", data["orientation"]["yaw"])

        if self.origin_x is None:
            # The robot starts at the start-tile centre, so anchor the grid
            # exactly here (relative 0,0 = tile centre, edges at +/-60). Do NOT
            # round to the 120 mm grid: that can offset the whole map by half a
            # tile and put real edge-walls onto mapper centre lines.
            self.origin_x = x
            self.origin_y = y
            # Boot references (rules: robot starts at a 120 mm main-tile CENTRE and
            # aligned to a cardinal). Save the exact GPS the grid is anchored to and
            # the IMU yaw at that cardinal, plus how far that reading sits from a
            # true 0/90/180/270 (the constant IMU offset), all available for reuse.
            self._start_yaw = yaw
            self._gps_ref_mm = (x, y)
            off = yaw % 90.0
            self._yaw_offset_deg = off if off <= 45.0 else off - 90.0

        rx = x - self.origin_x
        ry = y - self.origin_y

        self._mark_visited(rx, ry)
        self.visited_tiles.add(self._tile_of(rx, ry))

        # Use the GPS device's velocity measurement, not the derivative of its
        # noisy, rounded position. At 16 ms even 2 mm of position noise becomes
        # 125 mm/s and incorrectly blocks both mapping and floor observations.
        # Keep the old calculation only for historical captures without speed.
        now = data["time"]
        speed = 0.0
        if self._prev_ft is not None:
            dt = now - self._prev_ft
            if dt > 1e-6:
                speed = math.hypot(x - self._prev_fx, y - self._prev_fy) / dt
        self._prev_fx, self._prev_fy, self._prev_ft = x, y, now
        measured_speed = pos.get("speed_mmps")
        self.last_speed_source = "position-difference"
        if measured_speed is not None and math.isfinite(measured_speed) and measured_speed >= 0:
            speed = float(measured_speed)
            self.last_speed_source = "gps-velocity"
        self.last_speed = speed
        self.last_settled = speed <= self.MAP_SETTLE_SPEED_MMPS

        # Direct DOWNWARD colour-sensor + distance/cliff reading -> special tile
        # on the fixed 120 mm grid. Runs every frame (the median filter and the
        # cliff-confirmation counters need every sample) but only COMMITS from a
        # trustworthy pose (see the speed/turn gate inside), so a reading is never
        # dropped on the wrong grid cell while the robot is turning / driving fast.
        self.fuse_color_sensor_tile(rx, ry, yaw, data)
        self._track_room_crossing(self._tile_of(rx, ry), now)

        # Integrate one dense scan whenever the pose has meaningfully changed,
        # but NOT while driving fast: at speed the low-lag GPS trails the true
        # pose and the scan smears the walls along the path. Skipping fast frames
        # keeps the live map stable; the correct scan is painted the moment the
        # robot slows, and free-carving still cleans up anything stale.
        # Side-camera floor detection (coloured / swamp / checkpoint tiles to the
        # left & right) runs on its OWN speed gate, independent of wall painting.
        # Wall integration must skip fast frames (they smear), but the floor path
        # samples a single frame's tile footprint and is fine at driving speed -
        # gating it behind the 90 mm/s wall limit meant it never ran once keyboard
        # driving went past ~90 mm/s, so side specials stopped being detected.
        if self.FLOOR_SIDE_CAM_FUSE and speed <= self.FLOOR_MAX_SPEED_MMPS:
            self.fuse_floor_tiles(rx, ry, yaw, data)

        if self._moved_enough(x, y, yaw) and speed <= self.MAP_MAX_SPEED_MMPS:
            self.last_x, self.last_y, self.last_yaw = x, y, yaw
            self._integrate(rx, ry, yaw, data)
            # Fuse the side cameras with the same fresh, settled scan+pose so a
            # column's colour and its lidar bearing refer to the same instant.
            self.fuse_cameras(rx, ry, yaw, data)
            # keep the freshest integrated scan + pose for raw debug dumps
            lid = data.get("lidar")
            if self.DEBUG_MAPPING_DUMP and lid is not None and len(lid):
                self._last_scan = np.asarray(lid, dtype=np.float32).copy()
                self._last_pose = {
                    "rx": rx, "ry": ry, "yaw": yaw,
                    "roll": float(data["orientation"].get("roll", 0.0)),
                    "pitch": float(data["orientation"].get("pitch", 0.0)),
                    "lidar_hres": int(data.get("lidar_hres", 0)),
                    "lidar_fov": float(data.get("lidar_fov", 2.0 * math.pi)),
                    "lidar_vfov": float(data.get("lidar_vfov", self.LIDAR_VERTICAL_FOV)),
                    "time": now,
                }

        show_hi = self.SHOW_HIGH_RES_MAP if show is None else show
        if show_hi and now - self.last_draw >= self.DRAW_PERIOD_S:
            self.last_draw = now
            self.draw_high_resolution_map()

    def _moved_enough(self, x, y, yaw):
        if self.last_x is None:
            return True
        if math.hypot(x - self.last_x, y - self.last_y) >= self.MOVE_EPS_MM:
            return True
        return abs((yaw - self.last_yaw + 180) % 360 - 180) >= self.YAW_EPS_DEG

    def _track_room_crossing(self, tile, now):
        """Change area only after leaving a confirmed passage through a new side.

        Seeing a colour ahead or entering and backing out is not a crossing.
        Area 4 export uses only tiles whose area is established by this history.
        """
        connections = {"b": (1, 2), "y": (1, 3), "g": (1, 4),
                       "p": (2, 3), "o": (2, 4), "r": (3, 4)}
        previous = self._room_last_tile
        if tile != previous:
            self._room_entry_tile = previous
        if max(abs(tile[0]-previous[0]), abs(tile[1]-previous[1])) > 1:
            self._current_room = self._room_tiles.get(tile)
            self._room_pending_passage = None
        pending = self._room_pending_passage
        if pending is not None and tile != pending[0]:
            passage, token, entry, room = pending
            incoming = (entry[0]-passage[0], entry[1]-passage[1])
            outgoing = (tile[0]-passage[0], tile[1]-passage[1])
            pair = connections[token]
            if outgoing != incoming:
                if (sum(abs(v) for v in incoming) == 1
                        and sum(abs(v) for v in outgoing) == 1 and room in pair):
                    self._current_room = pair[1] if room == pair[0] else pair[0]
                    if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: transition history
                        self._room_transitions.append({"time": now, "tile": list(passage),
                                                       "from": room, "to": self._current_room})
                else:
                    self._current_room = self._room_tiles.get(tile)
            self._room_pending_passage = None
        token = self._committed_floor_tiles.get(tile)
        # Edge-only colour commits can still be relocated by better reads. They
        # must not trigger an irreversible room change while being refined.
        distance = self._cs_commit_dist.get(tile)
        if self._current_room is None:
            self._current_room = self._room_tiles.get(tile)
        if token in connections:
            # A passage is between areas; even an edge-only read must not turn
            # its coloured tile into ordinary Area 4 floor in the export.
            self._area4_tiles.discard(tile)
            self._room_tiles.pop(tile, None)
            if (distance is not None and distance <= self.CS_COMMIT_GOOD_DIST_MM
                    and self._room_pending_passage is None and self._room_entry_tile is not None):
                self._room_pending_passage = (tile, token, self._room_entry_tile, self._current_room)
        # Passage tracking is provisional. Only the observed room topology
        # below may assign floor regions or expand the submission mask.
        self._room_last_tile = tile

    # ------------------------------------------------------------------ #
    #  LiDAR integration (vectorised)
    # ------------------------------------------------------------------ #
    def _lidar_projection(self, yaw, data, indices):
        """Project the actual rounded depth texels used by Webots R2023b.

        Its point-cloud API uses ideal angles, but the renderer fetches rounded
        cube-map pixels and rectifies axial depth with an ideal cosine. Recover
        that pixel's actual vector before applying the unchanged mount and IMU.
        This calibration is for the robot's fixed, full-circle, low-VFOV lidar.
        """
        hres = int(data.get('lidar_hres', len(data['lidar'])))
        layers = len(data['lidar'])//hres
        layer = min(self.FRONT_INDEX//hres, layers-1)
        fov = float(data.get('lidar_fov', 2*math.pi))
        vfov = float(data.get('lidar_vfov', self.LIDAR_VERTICAL_FOV))
        key = (hres, layers, fov, vfov, layer)
        if getattr(self, '_lidar_sample_key', None) != key:
            height = math.ceil(vfov*hres/fov+1) if layers > 1 else 1
            render_vfov = height*fov/hres
            if not (fov > 3*math.pi/2 and render_vfov < 1.230959417):
                raise ValueError('LiDAR calibration requires full-circle, low vertical FOV')
            width = math.ceil(2/math.tan(fov/hres))
            rows = min(2048, max(1, int(1.27*width*height/hres)*4))
            width = min(2048, max(1, width))
            row = math.floor(layer*(height-1)/max(layers-1, 1))
            phi = (.5-(row+.5)/height)*render_vfov
            theta = (.5-(np.arange(hres)+.5)/hres)*fov
            ideal = np.column_stack((np.cos(theta)*math.cos(phi),
                np.sin(theta)*math.cos(phi), np.full(hres, math.sin(phi))))
            axis = np.argmax(abs(ideal), axis=1)
            major = ideal[np.arange(hres), axis]
            face = np.where(axis == 0, np.where(major > 0, 0, 2), np.where(major > 0, 3, 1))
            unit = ideal/abs(major[:, None])
            x = np.where(face == 0, unit[:, 1], np.where(face == 2, -unit[:, 1],
                np.where(face == 1, unit[:, 0], -unit[:, 0])))
            y = unit[:, 2]*(math.pi/2)/render_vfov
            ix = np.floor(.5*(1-x)*(width-1)+.5)
            iy = np.floor(.5*(1-y)*(rows-1)+.5)
            sx = 1-2*(ix+.5)/width
            sz = (1-2*(iy+.5)/rows)*height/hres*fov/(math.pi/2)
            local = np.column_stack((
                np.where(face == 0, 1, np.where(face == 2, -1, np.where(face == 1, sx, -sx))),
                np.where(face == 0, sx, np.where(face == 2, -sx, np.where(face == 1, -1, 1))), sz))
            self._lidar_sample_vectors = local*abs(major[:, None])
            self._lidar_sample_key = key
        local = self._lidar_sample_vectors[np.asarray(indices)]
        orientation = data.get('orientation', {})
        # NUE convention, as used by this world and the aligned IMU mounting.
        attitude = scipy_Rotation.from_euler('YZX', [math.radians(yaw),
            orientation.get('pitch', 0.0), orientation.get('roll', 0.0)]).as_matrix()
        rays = local @ self.LIDAR_MOUNT_ROTATION.T @ attitude.T
        offset = attitude @ self.LIDAR_MOUNT_MM
        return offset[[0,2]]*np.array([1,-1]), rays[:,[0,2]]*np.array([1,-1])

    def _physical_lidar_projection(self, yaw, data, indices):
        """Device-relative projection for explicitly GPS-referenced float poses.

        The canonical10mm semantic path retains its existing archive contract.
        A current GPS-device position needs the GPS-to-LiDAR lever, rather than
        adding the complete robot-root-to-LiDAR mount a second time.
        """
        offset, rays = self._lidar_projection(yaw, data, indices)
        orientation = data.get('orientation', {})
        attitude = scipy_Rotation.from_euler('YZX', [math.radians(yaw),
            orientation.get('pitch', 0.0), orientation.get('roll', 0.0)]).as_matrix()
        gps_offset = attitude @ np.array([0.0, 18.5, 0.0])
        return offset-gps_offset[[0,2]]*np.array([1,-1]), rays

    def _integrate(self, rx, ry, yaw, data):
        lidar = np.asarray(data["lidar"], dtype=np.float32)
        total = lidar.size
        hres = int(data.get("lidar_hres", total))
        fov = float(data.get("lidar_fov", 2.0 * math.pi))
        if hres <= 0 or total < hres:
            return

        layer = min(self.FRONT_INDEX // hres, total // hres - 1)
        start = layer * hres
        scan = lidar[start:start + hres]
        if scan.size < 8:
            return

        front = max(0, min(self.FRONT_INDEX - start, scan.size - 1))

        idx = np.arange(0, scan.size, self.BEAM_STEP)
        dist = scan[idx].astype(np.float32)
        # Test the original return before clipping open/far rays. A valid wall
        # at 380 mm is still within the 400 mm mapping range; the old 0.92
        # cutoff silently discarded it. Clipped rays must not invent a wall
        # exactly at the range limit.
        hit = (np.isfinite(dist) & (dist >= self.MIN_RANGE_MM)
               & (dist <= self.MAX_RANGE_MM))
        # NaN, negative and below-minimum returns cannot prove clear space.
        clear_ray = (dist >= self.MIN_RANGE_MM) & ~np.isnan(dist)
        dist[~np.isfinite(dist)] = self.MAX_RANGE_MM
        dist = np.clip(dist, 0.0, float(self.MAX_RANGE_MM))

        if (data.get('position', {}).get('map_pose_ready', False)
                and self._physical_evidence is None):
            self._physical_coarse_guard.update(map(tuple, np.argwhere(self.occupied_geometry())))
            self._physical_evidence = PhysicalEvidenceGrid(self, data=data)
        body_rx, body_ry = rx, ry
        offset, rays = self._lidar_projection(yaw, data, idx)
        rx, ry = rx+offset[0], ry+offset[1]
        ux, uy = rays.T
        # Curvature is invariant to a scan's common GPS translation. Use the
        # original depth precision to distinguish a partial arc from a line;
        # rounded millimetres can create artificial curvature on straight walls.
        original = data.get('lidar_raw_m')
        if self.MATRIX_ARC_CURVES and original is not None and len(original) == total:
            self._record_scan_curves(rx, ry, rays,
                np.asarray(original, dtype=float)[start+idx]*1000)
        if original is not None and len(original) == total:
            self._record_scan_surfaces(rx, ry, rays,
                np.asarray(original, dtype=float)[start+idx]*1000)

        # A sample along one ray does not prove its entire 10 mm cell empty.
        # Require seeing beyond every bearing covering the cell, including
        # the bracketing rays. Nearer returns/depth edges veto clearing.
        reach = np.maximum(dist - self.FREE_GAP_MM, 0.0)
        # Traverse every observed cell; a fixed number of points leaves radial
        # gaps that can reject a fully scanned navigation lane. One beam casts
        # one vote per cell, independent of sampling density.
        sampler = getattr(self, "_compiled_free_ray_cells", None)
        if sampler is not None:
            votes = sampler(self.n, self.c, self.GRID_MM, (rx, ry), rays, reach*clear_ray)
            self._clear_visible_cells(rx, ry, rays, reach*clear_ray, (), (), cell_votes=votes)
        else:
            # Portable class-only/offline compatibility. The self-contained
            # runtime bundle installs the exact native sampler before sensing.
            ts = np.linspace(0.0, 1.0, self.FREE_SAMPLES).reshape(-1, 1)
            self._clear_visible_cells(rx, ry, rays, reach*clear_ray,
                (rx + ts*reach[clear_ray]*ux[clear_ray]).ravel(),
                (ry + ts*reach[clear_ray]*uy[clear_ray]).ravel())

        # Only close returns become occupied wall hits.
        if np.any(hit):
            hx = rx + ux[hit] * dist[hit]
            hy = ry + uy[hit] * dist[hit]
            self._add(hx, hy, self.L_OCC)
            self.wall_hits += int(np.count_nonzero(hit))
            # Scan-time curve evidence (float precision, before rasterising):
            # a hit that is off BOTH nearest 60 mm grid lines by more than the
            # band lies on a curved wall's arc, never on a straight wall or a
            # sharp corner (those sit on the grid lines). Accumulate it so the
            # matrix curve pass can force the arc's bend vertex to '0'.
            if self.MATRIX_ARC_CURVES:
                # Position alone cannot distinguish a rounded wall from a
                # straight wall shifted by GPS/yaw error. Check the local
                # measured surface before allowing it to vote for a curve.
                # Use ~10 mm chords; reject discontinuities, sharp turns and
                # grazing views. Unreliable hits still update raw occupancy.
                points = np.column_stack((rx+ux*dist, ry+uy*dist))
                span = np.maximum(1, np.ceil(self.GRID_MM / np.maximum(dist, self.MIN_RANGE_MM)
                    / (fov/(scan.size-1)*self.BEAM_STEP)).astype(int))
                before = (np.arange(len(idx))-span) % len(idx)
                after = (np.arange(len(idx))+span) % len(idx)
                left, right = points-points[before], points[after]-points
                ll, lr = np.linalg.norm(left, axis=1), np.linalg.norm(right, axis=1)
                lu = left/np.maximum(ll[:, None], 1e-6)
                ru = right/np.maximum(lr[:, None], 1e-6)
                tangent = lu+ru
                norm = np.linalg.norm(tangent, axis=1)
                margin = math.sin(math.pi/8)
                surface = (hit & hit[before] & hit[after]
                    & (ll <= self.QUARTER_MM/2) & (lr <= self.QUARTER_MM/2)
                    & (np.sum(lu*ru, axis=1) > math.cos(math.pi/4))
                    & (abs(tangent[:, 0]*uy-tangent[:, 1]*ux) > margin*norm))
                quality = surface & (np.minimum(abs(tangent[:, 0]), abs(tangent[:, 1])) > margin*norm)
                q = float(self.QUARTER_MM)
                dvx = np.abs(hx - q * np.round(hx / q))
                dvy = np.abs(hy - q * np.round(hy / q))
                reliable = quality[hit]
                arc = (dvx > self.ARC_BAND_MM) & (dvy > self.ARC_BAND_MM) & reliable
                convex = ((dvx > self.ARC_CONVEX_BAND_MM)
                          & (dvy > self.ARC_CONVEX_BAND_MM) & ~arc & reliable)
                horizontal = (surface[hit] & (abs(tangent[hit, 1]) < math.sin(math.pi/16)*norm[hit])
                    & (dvy <= self.ARC_RADIUS_TOL_MM))
                vertical = (surface[hit] & (abs(tangent[hit, 0]) < math.sin(math.pi/16)*norm[hit])
                    & (dvx <= self.ARC_RADIUS_TOL_MM))
                if original is not None and len(original)==total:
                    # Raw whole-span axis fits were recorded before rasterizing.
                    # A local chord crossing a corner is not independent wall proof.
                    horizontal[:]=False;vertical[:]=False
                for mask, evidence in ((arc, self.arc_grid), (convex, self.convex_arc_grid),
                        (horizontal, self._horizontal_surface_hits), (vertical, self._vertical_surface_hits)):
                    if np.any(mask):
                        ax = np.rint(hx[mask] / self.GRID_MM).astype(np.int32) + self.c
                        ay = np.rint(hy[mask] / self.GRID_MM).astype(np.int32) + self.c
                        m = (ax >= 0) & (ax < self.n) & (ay >= 0) & (ay < self.n)
                        np.add.at(evidence, (ay[m], ax[m]), 1)

        if self.DEBUG_MAPPING_DUMP:
            keep = hit
            if not np.any(keep):
                keep = dist >= self.MIN_RANGE_MM
            sample_idx = idx[keep]
            sample_dist = dist[keep]
            sample = min(self.DEBUG_MAX_ITEMS, int(sample_idx.size))
            self.last_projected_points = [
                (int(sample_idx[k]), int(round(float(sample_dist[k]))),
                 round(float(rx + ux[keep][k] * sample_dist[k]), 1),
                 round(float(ry + uy[keep][k] * sample_dist[k]), 1))
                for k in range(sample)
            ]
            front_ex = float(rx + ux[0] * dist[0])
            front_ey = float(ry + uy[0] * dist[0])
            self.last_lidar_debug = {
                "total": int(total),
                "hres": int(hres),
                "layers": int(total // hres),
                "fov": round(fov, 6),
                "layer_index": int(layer),
                "layer_start": int(start),
                "front_index": int(front),
                "accepted_points": int(np.count_nonzero(hit)),
                "open_rays": int(np.count_nonzero(~hit)),
                "robot_world": (round(float(rx-offset[0]), 1), round(float(ry-offset[1]), 1)),
                "lidar_world": (round(float(rx), 3), round(float(ry), 3)),
                "projection": "r2023b-depth-texel-NUE-v2",
                "front_world": (round(front_ex, 1), round(front_ey, 1)),
                "settled": True,
                "first_points": self.last_projected_points,
            }

        # Functional physical evidence, independent of DEBUG and SHOW_MAPS.
        if 'map_pose_ready' in data.get('position', {}) or self._physical_evidence is not None:
            physical_ready = (data.get('position', {}).get('map_pose_ready', False)
                and (self._physical_evidence is None or not self._physical_evidence.precise
                     or PhysicalEvidenceGrid._pose_is_precise(data)))
            if physical_ready:
                if self._physical_evidence is None:
                    self._physical_evidence = PhysicalEvidenceGrid(self, data=data)
                self._physical_evidence.integrate(self, body_rx, body_ry, yaw, data)
            elif np.any(hit):
                # Only this unqualified scan's measured occupied hit cells
                # need a coarse footprint. An invalid or occluded scan cannot
                # widen already precise remote material across the whole map.
                cells = np.rint(np.column_stack((hx, hy))/self.GRID_MM).astype(int)+self.c
                cells = cells[np.all((cells >= 0) & (cells < self.n), axis=1)]
                for gx, gy in cells:
                    if self.logodds[gy, gx] >= self.OCC_T:
                        self._physical_coarse_guard.add((int(gy), int(gx)))

    def _fit_scan_lines(self,points,indices,max_gap):
        output=[];stack=[np.asarray(indices)]
        while stack:
            ids=stack.pop()
            if len(ids)<3:continue
            p=points[ids];gaps=np.linalg.norm(np.diff(p,axis=0),axis=1)
            gap=int(np.argmax(gaps))
            if gaps[gap]>max_gap:
                stack.extend((ids[:gap+1],ids[gap+1:]));continue
            mean=p.mean(0);v=p-mean;_,axes=np.linalg.eigh(v.T@v)
            normal=axes[:,0];errors=abs(v@normal)
            if errors.max()>.5:
                # Standard split-and-merge uses the endpoint chord to choose the
                # split. TLS's farthest residual may be an endpoint; repeatedly
                # peeling it would discard a shorter but perfectly visible face.
                chord=p[-1]-p[0];length=np.linalg.norm(chord)
                split_errors=(abs((p-p[0])@np.array([-chord[1],chord[0]])/length)
                    if length>1e-12 else errors)
                split=int(np.argmax(split_errors[1:-1]))+1
                stack.extend((ids[:split+1],ids[split:]));continue
            if len(ids)>=5:
                direction=axes[:,1];projection=v@direction
                output.append({'indices':ids,'mean':mean,'normal':normal,
                    'length':float(np.ptp(projection)),'max_error':float(errors.max()),
                    'rms_error':float(np.sqrt(np.mean(errors**2)))})
        # Splitting can fragment one real face. Merge ordered neighbours only if
        # their complete original support still fits the same bounded line.
        order={int(i):j for j,i in enumerate(indices)}
        output.sort(key=lambda f:order[int(f['indices'][0])]);merged=[]
        for feature in output:
            if merged:
                previous=merged[-1];ids=np.array(sorted(set(map(int,previous['indices']))|set(map(int,feature['indices'])),key=order.get))
                locations=[order[int(i)] for i in ids]
                # Never bridge dropped samples or invalid beams.
                if max(np.diff(locations),default=0)<=1:
                    p=points[ids];mean=p.mean(0);v=p-mean;_,axes=np.linalg.eigh(v.T@v)
                    errors=abs(v@axes[:,0])
                    if (errors.max()<=.5 and np.max(np.linalg.norm(np.diff(p,axis=0),axis=1))<=max_gap):
                        merged[-1]={'indices':ids,'mean':mean,'normal':axes[:,0],
                            'length':float(np.ptp(v@axes[:,1])),'max_error':float(errors.max()),
                            'rms_error':float(np.sqrt(np.mean(errors**2)))}
                        continue
            merged.append(feature)
        return merged


    def _unexplained_scan_faces(self,points,valid,quarter,tolerance,max_gap):
        # Invalid/open beams cut the ordered sequence. Rotate past one such cut
        # so a valid surface crossing index511/0 remains contiguous.
        index=np.arange(len(points));bad=np.flatnonzero(~valid)
        if len(bad):index=np.roll(index,-int(bad[0]))
        groups=np.split(index,np.flatnonzero(~valid[index])+1)
        result=[]
        for group in groups:
            group=group[valid[group]]
            for feature in self._fit_scan_lines(points,group,max_gap):
                p=points[feature['indices']];v=p-p.mean(0)
                # A bounded-error axis model uses the midrange, not the mean.
                # A single corner/sign sample must not shift a feasible wall fit.
                axis_error=float(min(np.ptp(p[:,0]),np.ptp(p[:,1]))/2)
                if axis_error<=.5:
                    if feature['length']>=self.GRID_MM:
                        axis=1 if np.ptp(p[:,1])<np.ptp(p[:,0]) else 0
                        if abs(np.median(p[:,axis])-quarter*round(float(np.median(p[:,axis]))/quarter))<=tolerance:
                            self._scan_axis_wall_beams[axis].update(map(int,feature['indices']))
                    continue
                fit,_,rank,_=np.linalg.lstsq(np.column_stack((2*v,np.ones(len(p)))),np.sum(v*v,axis=1),rcond=None)
                radius=np.sqrt(max(0,fit[2]+fit[:2]@fit[:2]));centre=p.mean(0)+fit[:2]
                circle_error=float(np.max(abs(np.linalg.norm(v-fit[:2],axis=1)-radius)))
                if (rank==3 and abs(radius-quarter)<=tolerance
                        and np.all(abs(centre-quarter*np.rint(centre/quarter))<=tolerance)
                        and circle_error<=.5):continue
                result.append({**feature,'axis_max_error_mm':axis_error,
                    'circle_radius_mm':float(radius),'circle_max_error_mm':circle_error})
        return result


    def _record_scan_surfaces(self, rx, ry, rays, ranges):
        """Identify bounded non-wall faces before10mm component merging.

        Two perpendicular, bounded faces propose a compact four-face body. Every
        accountable ray through its conservative interior must agree. Placement
        stays pending if its endpoint/position envelope crosses a120mm tile edge.
        """
        self.last_obstacle_fit_debug=[]
        valid=np.isfinite(ranges)&(ranges>=self.MIN_RANGE_MM)&(ranges<=self.MAX_RANGE_MM)
        laser=np.array([rx,ry]);points=laser+rays*np.where(valid,ranges,0)[:,None]
        self._scan_axis_wall_beams={0:set(),1:set()}
        faces=self._unexplained_scan_faces(points,valid,self.QUARTER_MM,
            self.ARC_RADIUS_TOL_MM,self.QUARTER_MM/2)
        # A face rejected by both allowed wall primitives is material, but
        # does not establish an object's centre or size. Defer its wall role;
        # only the independent bounded-body proof below may identify an object.
        for face in faces if self.OBSTACLE_DETECT else ():
            # Legacy yaw is rounded to whole degrees. A long straight wall
            # can drift off an axis by that half-degree without being oblique.
            if 2*face['axis_max_error_mm'] <= (face['length']*math.sin(math.radians(.5))
                                             + 2*face['max_error']):
                continue
            ids=face['indices']
            cells=np.rint((laser+rays[ids]*np.rint(ranges[ids,None]))/self.GRID_MM).astype(int)+self.c
            self._pending_nonwall_cells.update((int(y),int(x)) for x,y in cells
                if 0<=x<self.n and 0<=y<self.n)
        for axis,beams in self._scan_axis_wall_beams.items():
            if not beams:continue
            ids=np.array(sorted(beams))
            cells=np.rint((laser+rays[ids]*np.rint(ranges[ids,None]))/self.GRID_MM).astype(int)+self.c
            inside=np.all((cells>=0)&(cells<self.n),axis=1)
            x,y=cells[inside].T
            evidence=self._vertical_surface_hits if axis==0 else self._horizontal_surface_hits
            np.add.at(evidence,(y,x),1)
        if not self.OBSTACLE_DETECT:
            return
        bounded=[];count=len(ranges)
        for face in faces:
            ids=face['indices'];normal=face['normal']
            offset=float(normal@(face['mean']-laser));ends=[]
            for edge,neighbour in ((ids[0],(ids[0]-1)%count),(ids[-1],(ids[-1]+1)%count)):
                dot=float(normal@rays[neighbour]);incidence=abs(dot)/np.linalg.norm(rays[neighbour])
                predicted=offset/dot if abs(dot)>1e-12 else np.inf
                error=.5/abs(dot) if abs(dot)>1e-12 else np.inf
                visible=(self.MIN_RANGE_MM<=predicted and predicted+error<=self.MAX_RANGE_MM
                    and incidence>math.sin(math.pi/8) and not np.isnan(ranges[neighbour])
                    and ranges[neighbour]>predicted+error)
                ends.append({'edge':int(edge),'neighbour':int(neighbour),
                    'prediction':float(predicted),'error':float(error),
                    'incidence':float(incidence),'visible':bool(visible)})
            if all(e['visible'] for e in ends):
                face['ends']=ends;bounded.append(face)

        def end_interval(face,axis,which):
            end=face['ends'][which]
            a=float(axis@points[end['edge']])
            b=float(axis@(laser+rays[end['neighbour']]*end['prediction']))
            return np.array([min(a,b)-.5,max(a,b)+.5])

        for a,b in combinations(bounded,2):
            # One resolved face beyond a20mm square's diagonal excludes a
            # token. Its perpendicular face need not be equally long.
            if max(a['length'],b['length'])<=20*np.sqrt(2):
                continue
            normal=np.array([a['normal'],b['normal']])
            uncertainty=math.atan(1/a['length'])+math.atan(1/b['length'])
            if abs(normal[0]@normal[1])>math.sin(uncertainty):
                continue
            offsets=np.array([normal[0]@a['mean'],normal[1]@b['mean']])
            ai=[end_interval(a,normal[1],k) for k in (0,1)]
            bi=[end_interval(b,normal[0],k) for k in (0,1)]
            shared_a=[k for k in (0,1) if ai[k][0]-.5<=offsets[1]<=ai[k][1]+.5]
            shared_b=[k for k in (0,1) if bi[k][0]-.5<=offsets[0]<=bi[k][1]+.5]
            if len(shared_a)!=1 or len(shared_b)!=1:
                continue
            far=[bi[1-shared_b[0]],ai[1-shared_a[0]]]
            core=np.array([[offsets[k]+.5,far[k][0]] if offsets[k]<far[k].mean()
                else [far[k][1],offsets[k]-.5] for k in (0,1)])
            if np.any(core[:,1]<=core[:,0]):
                continue
            origin=normal@laser;direction=normal@rays.T
            with np.errstate(divide='ignore',invalid='ignore'):
                first=(core[:,0,None]-origin[:,None])/direction
                last=(core[:,1,None]-origin[:,None])/direction
            entry=np.minimum(first,last);near=entry.max(0);leave=np.maximum(first,last).min(0)
            hit=(near>=self.MIN_RANGE_MM)&(near<=self.MAX_RANGE_MM)&(leave>=near)
            if not hit.any():
                continue
            axis=np.argmax(entry,axis=0);normal_dot=abs(direction[axis,np.arange(count)])
            accountable=(hit&(normal_dot/np.linalg.norm(rays,axis=1)>math.sin(math.pi/8))
                &~np.isnan(ranges)&(ranges>=self.MIN_RANGE_MM))
            contradictions=accountable&(ranges>near+.5/np.maximum(normal_dot,1e-12))
            centre=np.linalg.solve(normal,(offsets+np.array([v.mean() for v in far]))/2)
            corners=np.array([np.linalg.solve(normal,values) for values in product(*[
                [(offsets[k]+error+value)/2 for error in (-.5,.5) for value in far[k]]
                for k in (0,1)])])
            low=corners.min(0)-self.ARC_RADIUS_TOL_MM
            high=corners.max(0)+self.ARC_RADIUS_TOL_MM
            tile_low=np.floor(low/self.TILE_MM+.5).astype(int)
            tile_high=np.floor(high/self.TILE_MM+.5).astype(int)
            certain=bool(np.all(tile_low==tile_high))
            if self.DEBUG_MAPPING_DUMP:
                self.last_obstacle_fit_debug.append({'centre_mm':centre.tolist(),
                    'placement_envelope_mm':[low.tolist(),high.tolist()],
                    'support_beams':[a['indices'].tolist(),b['indices'].tolist()],
                    'end_visibility':[a['ends'],b['ends']],
                    'clear_ray_contradictions':np.flatnonzero(contradictions).tolist(),
                    'accountable_core_rays':np.flatnonzero(accountable).tolist(),
                    'tile':tile_low.tolist() if certain else None})
            if contradictions.any() or not certain:
                continue
            tile=tuple(map(int,tile_low));ids=np.unique(np.r_[a['indices'],b['indices']])
            # Match the actual integration's rounded range before voxel indexing.
            cells=np.rint((laser+rays[ids]*np.rint(ranges[ids,None]))/self.GRID_MM).astype(int)+self.c
            evidence=self._raw_obstacle_cells.setdefault(tile,set())
            evidence.update((int(y),int(x)) for x,y in cells if 0<=x<self.n and 0<=y<self.n)
            # The positive body also explains its older occupied samples,
            # including a face not visible in this particular accepted scan.
            limits=np.array([[min(offsets[k],far[k].mean()),max(offsets[k],far[k].mean())] for k in (0,1)])
            inverse=np.linalg.inv(normal)
            vertices=np.array([inverse@v for v in product(*limits)])
            margin=self.ARC_RADIUS_TOL_MM*np.sum(abs(inverse),axis=1)
            first=np.maximum(0,np.floor((vertices.min(0)-margin)/self.GRID_MM).astype(int)+self.c)
            last=np.minimum(self.n-1,np.ceil((vertices.max(0)+margin)/self.GRID_MM).astype(int)+self.c)
            x,y=np.meshgrid(np.arange(first[0],last[0]+1),np.arange(first[1],last[1]+1))
            world=np.column_stack(((x.ravel()-self.c)*self.GRID_MM,(y.ravel()-self.c)*self.GRID_MM))
            local=world@normal.T
            inside=np.all((local>=limits[:,0]-self.ARC_RADIUS_TOL_MM)&(local<=limits[:,1]+self.ARC_RADIUS_TOL_MM),axis=1)
            self._raw_obstacle_regions.setdefault(tile,set()).update(zip(map(int,y.ravel()[inside]),map(int,x.ravel()[inside])))


    def _pending_nonwall_mask(self):
        """Defer a wall role without turning it into confirmed object evidence."""
        mask=np.zeros_like(self.logodds,dtype=bool)
        self._pending_nonwall_cells={p for p in self._pending_nonwall_cells
                                    if self.logodds[p]>self.FREE_T}
        for gy,gx in self._pending_nonwall_cells:
            tile=self._tile_of((gx-self.c)*self.GRID_MM,(gy-self.c)*self.GRID_MM)
            if tile not in self._area4_tiles and self.logodds[gy,gx]>=self.OCC_T:
                mask[gy,gx]=True
        if mask.any():
            for ax,ay,bx,by in self._confirmed_straight_walls:
                steps=round(math.hypot(bx-ax,by-ay)/self.GRID_MM)
                xs=np.rint(np.linspace(ax,bx,steps+1)/self.GRID_MM).astype(int)+self.c
                ys=np.rint(np.linspace(ay,by,steps+1)/self.GRID_MM).astype(int)+self.c
                mask[ys,xs]=False
            mask &= ~((self._horizontal_surface_hits>0)|(self._vertical_surface_hits>0))
            if self._raw_curve_wall_cells:
                ys,xs=np.array(sorted(self._raw_curve_wall_cells)).T
                mask[ys,xs]=False
        return mask

    def _raw_supported_obstacles(self):
        """Separate positive object evidence from independently measured walls."""
        mask=np.zeros_like(self.logodds,dtype=bool);tiles=set()
        for tile,cells in list(self._raw_obstacle_cells.items()):
            ys,xs=np.array(sorted(cells)).T
            values=self.logodds[ys,xs]
            if np.all(values<=self.FREE_T):
                del self._raw_obstacle_cells[tile]
                self._raw_obstacle_regions.pop(tile,None)
                continue
            tiles.add(tile)
            region=self._raw_obstacle_regions.get(tile,cells)
            ys,xs=np.array(sorted(region)).T
            occupied=self.logodds[ys,xs]>=self.OCC_T
            mask[ys[occupied],xs[occupied]]=True
        # A voxel may contain both sides of a narrow gap. Its wall support wins
        # this semantic mask; its raw occupied material is never removed anywhere.
        mask &= ~((self._horizontal_surface_hits>0)|(self._vertical_surface_hits>0))
        if self._raw_curve_wall_cells:
            ys,xs=np.array(sorted(self._raw_curve_wall_cells)).T
            mask[ys,xs]=False
        return tiles,mask

    def _record_scan_curves(self, rx, ry, rays, ranges):
        """Validate observable arc fragments without pooling occluding surfaces.

        Start with the existing10mm half-window and halve it down to five raw
        samples. Every accepted interval independently fits the same grid-circle
        hypothesis and beats straight/sharp alternatives. Five samples provide
        two residual degrees of freedom for the three-parameter circle fit.
        """
        self._native_curve_samples = True
        valid = np.isfinite(ranges)&(ranges >= self.MIN_RANGE_MM)&(ranges <= self.MAX_RANGE_MM)
        ranges=np.where(valid,ranges,self.MAX_RANGE_MM)
        points = np.array([rx,ry])+rays*ranges[:,None]
        step=np.median(abs(np.diff(np.unwrap(np.arctan2(rays[:,1],rays[:,0])))))
        span=np.maximum(2,np.ceil(self.GRID_MM/np.maximum(ranges,self.MIN_RANGE_MM)/step).astype(int))
        q=self.QUARTER_MM;support={};details={};previous_span=None
        while True:
            before=(np.arange(len(points))-span)%len(points);after=(np.arange(len(points))+span)%len(points)
            a,b=points[before]-points,points[after]-points
            aa,bb=np.sum(a*a,axis=1),np.sum(b*b,axis=1)
            determinant=2*(a[:,0]*b[:,1]-a[:,1]*b[:,0])
            with np.errstate(divide='ignore',invalid='ignore'):
                relative=np.column_stack((aa*b[:,1]-bb*a[:,1],a[:,0]*bb-b[:,0]*aa))/determinant[:,None]
                centres=points+relative;radii=np.linalg.norm(relative,axis=1)
                normals=-relative/np.maximum(radii[:,None],1e-6)
            anchors=np.rint(np.nan_to_num(centres,nan=0,posinf=0,neginf=0)/q)
            normals=np.nan_to_num(normals,nan=0,posinf=0,neginf=0)
            signs=np.sign(points-centres)
            good=(valid&valid[before]&valid[after]&np.isfinite(radii)
                &(abs(radii-q)<=self.ARC_RADIUS_TOL_MM)
                &(np.sqrt(aa)<=q/2)&(np.sqrt(bb)<=q/2)
                &(abs(np.sum(normals*rays,axis=1))/np.linalg.norm(rays,axis=1)>math.sin(math.pi/8))
                &np.all(abs(centres-anchors*q)<=self.ARC_RADIUS_TOL_MM,axis=1)
                &np.all(np.sign(points[before]-centres)==signs,axis=1)
                &np.all(np.sign(points[after]-centres)==signs,axis=1)
                &np.all(signs!=0,axis=1))
            if previous_span is not None:
                good &= span!=previous_span
            previous_span=span.copy()
            for i in np.flatnonzero(good):
                sx,sy=(-signs[i]).astype(int);cx,cy=anchors[i].astype(int)
                model=(int(cx-sx),int(cy-sy),int(sx),int(sy))
                if self._scan_curve_votes.get(model,0)>=self.ARC_SOFT_HITS:
                    continue
                window=np.arange(i-span[i],i+span[i]+1)%len(points)
                if not np.all(valid[window]):
                    continue
                mean=np.mean(points[window],axis=0);p=points[window]-mean
                fit,_,rank,_=np.linalg.lstsq(np.column_stack((2*p,np.ones(len(p)))),np.sum(p*p,axis=1),rcond=None)
                if rank<3:
                    continue
                centre=mean+fit[:2];radius=math.sqrt(max(0.0,fit[2]+np.dot(fit[:2],fit[:2])))
                # The complete span must support THIS model, not some other
                # circle through a selected subset of its returns.
                if (abs(radius-q)>self.ARC_RADIUS_TOL_MM
                        or np.any(abs(centre-anchors[i]*q)>self.ARC_RADIUS_TOL_MM)
                        or np.any(np.sign(points[window]-centre)!=signs[i])):
                    continue
                circle_error=np.sum((np.linalg.norm(p-fit[:2],axis=1)-radius)**2)/(len(p)-3)
                line_sse=max(0.0,np.linalg.eigvalsh(p.T@p)[0])
                # A box can be rotated. Fit ordered orthogonal faces with one
                # free orientation, rather than assuming every sharp corner
                # follows the wall grid. This model has three parameters.
                splits=np.arange(1,len(p))
                sums=np.cumsum(p,axis=0)
                products=np.cumsum(p[:,:,None]*p[:,None,:],axis=0)
                first=products[splits-1]-sums[splits-1,:,None]*sums[splits-1,None,:]/splits[:,None,None]
                tail=sums[-1]-sums[splits-1]
                last=products[-1]-products[splits-1]-tail[:,:,None]*tail[:,None,:]/(len(p)-splits)[:,None,None]
                xx=first[:,1,1]+last[:,0,0];yy=first[:,0,0]+last[:,1,1]
                xy=last[:,0,1]-first[:,0,1]
                sharp_sse=max(0.0,float(np.min((xx+yy-np.hypot(xx-yy,2*xy))/2)))
                # Keep the simpler grid-aligned two-parameter alternative too.
                axis_sse=max(0.0,float(min(np.min(first[:,0,0]+last[:,1,1]),
                    np.min(first[:,1,1]+last[:,0,0]))))
                line_error=min(line_sse,axis_sse)/(len(p)-2)
                line_error=min(line_error,sharp_sse/(len(p)-3))
                if circle_error>=line_error:
                    continue
                support.setdefault(model,set()).update(map(int,window))
                if self.DEBUG_MAPPING_DUMP:
                    details.setdefault(model,[]).append({'beam':int(i),'half_span':int(span[i]),
                        'support_beams':list(map(int,window)),'radius_mm':radius,
                        'centre_mm':centre.tolist(),'circle_error_mm2':float(circle_error),
                        'straight_or_corner_error_mm2':float(line_error)})
            if np.all(span==2):
                break
            span=np.maximum(2,span//2)
        for model,beams in support.items():
            ids=np.array(sorted(beams))
            cells=np.rint((np.array([rx,ry])+rays[ids]*np.rint(ranges[ids,None]))/self.GRID_MM).astype(int)+self.c
            self._raw_curve_wall_cells.update((int(y),int(x)) for x,y in cells if 0<=x<self.n and 0<=y<self.n)
            self._scan_curve_votes[model]=min(self.ARC_SOFT_HITS,self._scan_curve_votes.get(model,0)+len(beams))
        if self.DEBUG_MAPPING_DUMP:
            self.last_curve_fit_debug={'groups':[{'model':list(model),
                'beams':sorted({d['beam'] for d in details[model]}),'support_beams':sorted(beams),
                'radii_mm':[d['radius_mm'] for d in details[model]],
                'centres_mm':[d['centre_mm'] for d in details[model]],
                'validated_spans':details[model],'scan_votes':self._scan_curve_votes[model]}
                for model,beams in support.items()]}


    def _clear_visible_cells(self, rx, ry, rays, reach, xs, ys, registration_mm=0.0, cell_votes=None):
        """Protect existing occupied evidence unless its whole cell is seen through.

        This is a 2D inverse sensor model, not a test of the last hit's normal:
        occlusion and grazing silhouettes change with the current viewpoint.
        Invalid/too-close returns have zero reach. The existing range margin
        is retained. A cell hidden even partly by a nearer return gets no miss.
        """
        if cell_votes is None:
            cx = np.rint(xs/self.GRID_MM).astype(int)+self.c
            cy = np.rint(ys/self.GRID_MM).astype(int)+self.c
            inside = (cx >= 0) & (cx < self.n) & (cy >= 0) & (cy < self.n)
            cells, counts = np.unique(cy[inside]*self.n+cx[inside], return_counts=True)
        else:
            cells, counts = cell_votes
        flat = self.logodds.ravel()
        guarded = flat[cells] > 0
        if cell_votes is not None:
            # New free-space knowledge needs a complete view, except cells
            # actually traversed by the chassis. Visits never erase positives.
            traversed = self.visited.ravel()[cells]
            guarded |= (flat[cells] > self.FREE_T) & ~traversed
        clear = ~guarded
        protected = cells[guarded]
        if self.DEBUG_MAPPING_DUMP:
            self.last_free_space_debug = {"candidate_cells": len(cells),
                "guard_scope": "positive evidence and unconfirmed untraversed cells" if cell_votes is not None else "positive evidence",
                "positive_cell_tests": []}
        if not len(protected):
            flat[cells] = np.maximum(-self.L_CLAMP, flat[cells]-self.L_FREE*counts)
            return
        dx = (protected % self.n-self.c)*self.GRID_MM-rx
        dy = (protected // self.n-self.c)*self.GRID_MM-ry
        half = self.GRID_MM/2
        x = dx[:, None]+np.array([-half, -half, half, half])
        y = dy[:, None]+np.array([-half, half, -half, half])
        centre = np.arctan2(dy, dx)
        delta = (np.arctan2(y, x)-centre[:, None]+math.pi) % (2*math.pi)-math.pi
        distance = np.hypot(x, y)
        if registration_mm and math.isfinite(registration_mm) and registration_mm > 0:
            # A common translation error moves the angular occlusion shadow,
            # not just the radial hit distance. Test the square's Minkowski
            # sum with the registration disk: corner tangent angles and the
            # furthest corner plus its radius bound that entire footprint.
            spread = np.arcsin(np.minimum(1.0, registration_mm/np.maximum(distance, 1e-12)))
            low = centre+(delta-spread).min(axis=1)
            high = centre+(delta+spread).max(axis=1)
        else:
            low, high = centre+delta.min(axis=1), centre+delta.max(axis=1)
        far = distance.max(axis=1)+registration_mm
        angles = np.arctan2(rays[:, 1], rays[:, 0])
        order = np.argsort(angles)
        # A gap outside the sensor FOV is unobserved, not a free sector.
        step = np.max(np.abs(np.diff(np.unwrap(angles))))
        gaps = np.diff(np.r_[angles[order], angles[order][0]+2*math.pi])
        blind = gaps > step+1e-9
        radial = reach[order]*np.linalg.norm(rays[order], axis=1)
        radial[blind | np.roll(blind, 1)] = 0
        angles = np.concatenate((angles[order]-2*math.pi, angles[order], angles[order]+2*math.pi))
        limits = np.tile(radial, 3)
        left = np.searchsorted(angles, low, side='right')-1
        right = np.searchsorted(angles, high, side='left')
        # Sparse range minima keep the whole angular-footprint test linear
        # in the number of candidate cells, rather than cell x beam pairs.
        minima = [limits]
        for k in range(1, int(math.log2(len(limits)))+1):
            shift = 1 << (k-1)
            previous = minima[-1]
            minima.append(np.minimum(previous[:-shift], previous[shift:]))
        power = np.floor(np.log2(right-left+1)).astype(int)
        observed = np.empty(len(protected))
        for k in np.unique(power):
            mask = power == k
            observed[mask] = np.minimum(minima[k][left[mask]],
                minima[k][right[mask]-(1 << int(k))+1])
        clear[guarded] = ((far < observed)
            & (np.hypot(dx, dy) > half*math.sqrt(2)+registration_mm))
        if self.DEBUG_MAPPING_DUMP:
            # DEBUG ONLY: every positive cell considered for clearing, without
            # truncation. The raw scan plus pose reconstructs its veto rays.
            tests = [
                [int(c % self.n), int(c // self.n), int(flat[c]), int(n),
                 float(f), float(limit), bool(accepted)]
                for c, n, f, limit, accepted in zip(protected, counts[guarded],
                    far, observed, clear[guarded])]
            self.last_free_space_debug["positive_cell_tests"] = [v for v in tests if v[2] > 0]
            if cell_votes is not None:
                self.last_free_space_debug["unconfirmed_cell_tests"] = [v for v in tests if v[2] <= 0]
        flat[cells[clear]] = np.maximum(-self.L_CLAMP, flat[cells[clear]]-self.L_FREE*counts[clear])

    def _add(self, xs, ys, delta):
        cx = np.rint(xs / self.GRID_MM).astype(np.int32) + self.c
        cy = np.rint(ys / self.GRID_MM).astype(np.int32) + self.c
        m = (cx >= 0) & (cx < self.n) & (cy >= 0) & (cy < self.n)
        np.add.at(self.logodds, (cy[m], cx[m]), delta)
        np.clip(self.logodds, -self.L_CLAMP, self.L_CLAMP, out=self.logodds)

    def _mark_visited(self, rx, ry):
        cx = int(round(rx / self.GRID_MM)) + self.c
        cy = int(round(ry / self.GRID_MM)) + self.c
        r = max(1, int(round(36 / self.GRID_MM)))
        x0, x1 = max(0, cx - r), min(self.n, cx + r + 1)
        y0, y1 = max(0, cy - r), min(self.n, cy + r + 1)
        # Only points underneath the chassis have actually been traversed.
        # The old square also marked diagonal points 56 mm from its centre.
        yy, xx = np.ogrid[y0:y1, x0:x1]
        self.visited[y0:y1, x0:x1] |= (
            ((xx-self.c)*self.GRID_MM-rx)**2
            + ((yy-self.c)*self.GRID_MM-ry)**2 <= 36.0**2)

    # ------------------------------------------------------------------ #
    #  Camera <-> lidar obstacle fusion
    # ------------------------------------------------------------------ #
    @staticmethod
    def _cam_band_hsv(buf, w, h, band):
        """Per-column (H[0..360], S[0..1]) medians in the horizon row band.

        `buf` is Webots BGRA bytes (row-major, h*w*4). Returns two length-w
        arrays or (None, None) on a malformed buffer. V (brightness) is thrown
        away by the caller, so a wall lit or in shadow reads the same.
        """
        if buf is None:
            return None, None
        try:
            arr = np.frombuffer(buf, dtype=np.uint8)
        except (TypeError, ValueError):
            return None, None
        if w <= 0 or h <= 0 or arr.size < w * h * 4:
            return None, None
        arr = arr[: w * h * 4].reshape(h, w, 4).astype(np.float32) / 255.0
        r0 = max(0, int(round(h * band[0])))
        r1 = min(h, max(r0 + 1, int(round(h * band[1]))))
        # Webots buffer is BGRA -> pick R,G,B for the band only
        b = arr[r0:r1, :, 0]
        g = arr[r0:r1, :, 1]
        r = arr[r0:r1, :, 2]
        mx = np.maximum(np.maximum(r, g), b)
        mn = np.minimum(np.minimum(r, g), b)
        df = mx - mn
        s = np.where(mx > 1e-6, df / (mx + 1e-9), 0.0)
        rc = (mx - r) / (df + 1e-9)
        gc = (mx - g) / (df + 1e-9)
        bc = (mx - b) / (df + 1e-9)
        hue = np.zeros_like(mx)
        m = df > 1e-6
        hue = np.where((mx == r) & m, bc - gc, hue)
        hue = np.where((mx == g) & m, 2.0 + rc - bc, hue)
        hue = np.where((mx == b) & m, 4.0 + gc - rc, hue)
        hue = (hue / 6.0) % 1.0
        colH = np.median(hue, axis=0) * 360.0
        colS = np.median(s, axis=0)
        return colH.astype(np.float32), colS.astype(np.float32)

    @classmethod
    def _cam_decode_rgb(cls, buf, w, h):
        """Webots BGRA buffer -> float RGB array (h, w, 3) in 0..1."""
        if buf is None:
            return None
        try:
            arr = np.frombuffer(buf, dtype=np.uint8)
        except (TypeError, ValueError):
            return None
        if w <= 0 or h <= 0 or arr.size < w * h * 4:
            return None
        bgra = arr[: w * h * 4].reshape(h, w, 4).astype(np.float32) / 255.0
        return bgra[:, :, (2, 1, 0)]

    @classmethod
    def _cam_fake_victim_white(cls, rgb, w, h, a0, a1, wall_v=None):
        """True when the obstacle column run also shows a victim white plaque."""
        if rgb is None:
            return False
        r0 = max(0, int(round(h * cls.CAM_VICTIM_WHITE_ROWS[0])))
        r1 = min(h, max(r0 + 1, int(round(h * cls.CAM_VICTIM_WHITE_ROWS[1]))))
        c0 = max(0, a0 - cls.CAM_VICTIM_WHITE_MARGIN)
        c1 = min(w - 1, a1 + cls.CAM_VICTIM_WHITE_MARGIN)
        patch = rgb[r0:r1, c0:c1 + 1]
        if patch.size == 0:
            return False
        mx = np.max(patch, axis=2)
        mn = np.min(patch, axis=2)
        s = np.where(mx > 1e-6, (mx - mn) / (mx + 1e-9), 0.0)
        abs_white = ((mx >= cls.CAM_VICTIM_WHITE_V_MIN)
                     & (s <= cls.CAM_VICTIM_WHITE_S_MAX))
        if float(abs_white.mean()) >= cls.CAM_VICTIM_WHITE_FRAC:
            return True
        if wall_v is not None:
            # Brighter than the wall AND genuinely near-white: the absolute floor
            # (WHITE_V_MIN) keeps the bright grey arena floor / a grey obstacle
            # body - both brighter than a dark teal wall - from counting as a
            # plaque here (they are not white, just light grey).
            rel_v_min = max(wall_v + cls.CAM_VICTIM_REL_DV, cls.CAM_VICTIM_WHITE_V_MIN)
            rel_white = ((mx >= rel_v_min)
                         & (s <= cls.CAM_VICTIM_WHITE_S_MAX))
            if float(rel_white.mean()) >= cls.CAM_VICTIM_WHITE_FRAC:
                return True
        return False

    def _seg_wall_flush(self, rng, a0, a1, w):
        """True when an obstacle-coloured segment is a decal / sign / 3D victim
        mounted FLUSH on the wall rather than a free-standing obstacle.

        The lidar range across the segment is compared to the wall line
        interpolated from the near columns immediately flanking it (both sides).
        A flush decal sits ON that line (protrusion ~0); a real obstacle juts
        toward the robot, so its range is nearer than the line by more than
        CAM_VICTIM_FLUSH_MM. Requires a near wall on BOTH sides - a sign is
        embedded in a wall - so an obstacle with open floor / far wall beside it
        (no finite flank) is never mistaken for a decal.
        """
        nf = self.CAM_FLUSH_FLANK_COLS
        rmax = self.CAM_RANGE_MAX_MM
        seg = [float(rng[c]) for c in range(a0, a1 + 1)
               if math.isfinite(rng[c]) and rng[c] <= rmax * 2.0]
        if not seg:
            return False
        segmed = float(np.median(seg))
        L = [(c, float(rng[c])) for c in range(max(0, a0 - nf), a0)
             if math.isfinite(rng[c]) and rng[c] <= rmax]
        R = [(c, float(rng[c])) for c in range(a1 + 1, min(w, a1 + 1 + nf))
             if math.isfinite(rng[c]) and rng[c] <= rmax]
        if not L or not R:
            return False                     # no continuous wall both sides => obstacle
        lc, lv = L[-1]
        rc, rv = R[0]
        mid = (a0 + a1) / 2.0
        interp = ((lv + (rv - lv) * ((mid - lc) / (rc - lc)))
                  if rc != lc else 0.5 * (lv + rv))
        protrusion = interp - segmed         # +ve => segment nearer than wall line
        return protrusion <= self.CAM_VICTIM_FLUSH_MM

    def _cam_clear_obstacle_tile(self, tile):
        """Drop a camera-committed obstacle tile and its vote accumulator."""
        t = tuple(tile)
        self._cam_obstacle_tiles.discard(t)
        self._obst_votes.pop(t, None)
        self._persistent_obstacle_tiles.discard(t)

    @staticmethod
    def _rgb_to_hsv(r, g, b):
        """Scalar RGB (0..1) -> (H deg 0..360, S 0..1, V 0..1)."""
        mx = max(r, g, b)
        mn = min(r, g, b)
        df = mx - mn
        if df < 1e-6:
            h = 0.0
        elif mx == r:
            h = (60.0 * ((g - b) / df)) % 360.0
        elif mx == g:
            h = 60.0 * ((b - r) / df) + 120.0
        else:
            h = 60.0 * ((r - g) / df) + 240.0
        s = 0.0 if mx < 1e-6 else df / mx
        return h % 360.0, s, mx

    @classmethod
    def classify_floor_rgb(cls, r, g, b):
        """Map a floor-patch colour to its RCJ map character, or None.

        Calibration-free floor classification. Order matters:
          1. near-black AND neutral (V very low, low S) -> hole '2'
             (a dark COLOURED wall is dark but chromatic -> never a hole)
          2. saturated (S high) + a hue bin     -> connection colour b/y/g/p/o/r
          3. warm hue + medium saturation       -> swamp '3'
          4. dark + low-sat + blue tint (B>R)   -> checkpoint '4'
          5. otherwise (plain gray, shadow,
             cyan wall, unknown)                -> None
        Saturation is the key separator: connection colours are vivid (>=0.62),
        swamp is a duller tan (~0.55), checkpoint/floor are near-grey. Checkpoint
        is told from grey floor/shadow only by its consistent blue tint, so a
        neutral shadow can never become a '4'.

        r,g,b are 0..1. Returns one of 'b','y','g','p','o','r','2','3','4' or None.
        """
        h, s, v = cls._rgb_to_hsv(r, g, b)
        if v < cls.FLOOR_HOLE_V_MAX and s <= cls.FLOOR_HOLE_SAT_MAX:
            return "2"                       # very dark AND neutral => hole (a black
                                             # tile); a dark COLOURED wall is not
        if s >= cls.FLOOR_CONN_SAT_MIN and v >= cls.FLOOR_MIN_V:
            for char, lo, hi in cls.FLOOR_HUE_BINS:
                if lo <= h < hi:
                    return char
            return None                      # cyan/magenta gap (e.g. wall) => reject
        if (cls.FLOOR_SWAMP_SAT_MIN <= s < cls.FLOOR_CONN_SAT_MIN
                and v >= cls.FLOOR_MIN_V
                and cls.FLOOR_SWAMP_HUE[0] <= h < cls.FLOOR_SWAMP_HUE[1]):
            return "3"                       # tan, medium saturation => swamp
        if (s < cls.FLOOR_CP_SAT_MAX
                and cls.FLOOR_CP_V[0] <= v <= cls.FLOOR_CP_V[1]
                and (b - r) >= cls.FLOOR_CP_BLUE_MIN):
            return "4"                       # dark bluish grey => checkpoint
        return None

    @classmethod
    def classify_color_sensor(cls, r, g, b):
        """Classify a DOWNWARD colour-sensor reading into one of the 9 special
        floor tiles, or None for plain floor / shadow.

        r,g,b are 0..255 (the raw colour sensor pixel). Thresholds are calibrated
        from sensor calibration captures (see the CS_* constants). The
        plain floor reads as a BRIGHT neutral grey (V ~0.95), so a special tile
        must be vivid, warm, or a DARK-BLUISH grey (the checkpoint). HOLES are NOT
        detected here - the distance/cliff sensor finds them - so a neutral dark
        reading returns None (this removes the false holes/checkpoints on shadows).
        Order matters:
          1. vivid (high S) + a hue bin           -> connection colour b/y/g/p/o/r
          2. warm hue + medium S                  -> swamp '3'
          3. darker-than-floor grey with a BLUE tint (B>R) -> checkpoint '4'
          4. otherwise (bright floor, neutral dark hole) -> None
        Returns 'b','y','g','p','o','r','3','4' or None.
        """
        rn, gn, bn = r / 255.0, g / 255.0, b / 255.0
        h, s, v = cls._rgb_to_hsv(rn, gn, bn)
        if s >= cls.CS_CONN_SAT_MIN and v >= cls.CS_CONN_V_MIN:
            for char, lo, hi in cls.CS_HUE_BINS:
                if lo <= h < hi:
                    return char                          # vivid connection colour
            return None                                  # cyan/magenta gap => reject
        if (cls.CS_SWAMP_SAT[0] <= s < cls.CS_SWAMP_SAT[1]
                and cls.CS_SWAMP_HUE[0] <= h < cls.CS_SWAMP_HUE[1]):
            return "3"                                   # warm/tan => swamp
        if (s < cls.CS_CP_SAT_MAX and v >= cls.CS_CP_V_MIN
                and (bn - rn) >= cls.CS_CP_BLUE_MIN):
            return "4"                                   # blue-tinted grey => checkpoint
        return None                                      # bright floor / neutral dark / etc.

    @classmethod
    def _is_bright_floor(cls, r, g, b):
        """Plain arena floor as seen by the downward colour sensor (~243/255)."""
        rn, gn, bn = r / 255.0, g / 255.0, b / 255.0
        _, s, v = cls._rgb_to_hsv(rn, gn, bn)
        return v >= 0.85 and s < 0.12

    @classmethod
    def _is_pit_color(cls, r, g, b):
        """Near-black neutral pit colour (~[48,48,48]) - not checkpoint bluish."""
        rn, gn, bn = r / 255.0, g / 255.0, b / 255.0
        _, s, v = cls._rgb_to_hsv(rn, gn, bn)
        return v < 0.35 and s < 0.25 and (bn - rn) < cls.CS_CP_BLUE_MIN

    def _log_floor_commit(self, t, kind, ch, tile, dist, rx, ry, sx, sy, yaw,
                          cur_tile, off_x, off_y, center_r, run, yaw_rate,
                          blocked=None, replaced=None, extra=None):
        """Append one special-tile commit event to the rolling diagnostic log.

        Captures the exact pose + projected geometry at the instant a tile was
        stamped (or blocked), so a later capture can show WHY a tile landed on a
        given grid cell instead of only the pose at the 'S' press.
        """
        if not self.DEBUG_MAPPING_DUMP:
            return
        rec = {
            "t": round(float(t), 2),
            "kind": kind,
            "class": ch,
            "committed": blocked is None,
            "blocked": blocked,
            "tile": list(tile) if tile is not None else None,
            "replaced": replaced,
            "commit_dist_mm": (round(float(dist), 1) if dist is not None else None),
            "robot_mm": [round(float(rx), 1), round(float(ry), 1)],
            "proj_mm": [round(float(sx), 1), round(float(sy), 1)],
            "yaw_deg": round(float(yaw), 1),
            "cur_tile": list(cur_tile) if cur_tile is not None else None,
            "off_mm": [round(float(off_x), 1), round(float(off_y), 1)],
            "center_dist_mm": round(float(center_r), 1),
            "run": int(run),
            "speed_mmps": round(float(self.last_speed), 1),
            "yaw_rate_dps": round(float(yaw_rate), 1),
        }
        if extra:
            rec.update(extra)
        # Collapse ALL frames that share an outcome (same kind/tile/committed/
        # blocked) into ONE entry, keeping the MOST-centred frame's geometry and
        # the earliest time seen. A robot parked on one tile for thousands of
        # frames therefore leaves a single entry and never evicts the specials
        # committed earlier (interleaved colour+hole commits used to do exactly
        # that and flood the log).
        key = (rec["kind"], rec["class"], tuple(rec["tile"]) if rec["tile"] else None,
               rec["committed"], rec["blocked"])
        prev = self._floor_commit_log.get(key)
        if prev is not None:
            rec["seq"] = prev["seq"]
            rec["first_t"] = prev.get("first_t", prev["t"])
            rec["count"] = prev.get("count", 1) + 1
            rec["run"] = max(prev["run"], rec["run"])
            if rec["center_dist_mm"] > prev["center_dist_mm"]:
                # keep the better-centred geometry but the updated bookkeeping
                keep = dict(prev)
                keep["count"] = rec["count"]
                keep["run"] = rec["run"]
                keep["t"] = rec["t"]
                self._floor_commit_log[key] = keep
                return
            self._floor_commit_log[key] = rec
            return
        rec["first_t"] = rec["t"]
        rec["count"] = 1
        rec["seq"] = self._floor_commit_seq
        self._floor_commit_seq += 1
        self._floor_commit_log[key] = rec
        # bound distinct outcomes (specials + a few false/blocked) generously
        if len(self._floor_commit_log) > 200:
            oldest = min(self._floor_commit_log, key=lambda k: self._floor_commit_log[k]["seq"])
            del self._floor_commit_log[oldest]




    def _cs_quality_ok(self, run, dist, speed):
        """Whether a colour run has enough evidence to commit at distance *dist*."""
        if dist is None or run < self.CS_CONFIRM:
            return False
        if dist <= self.CS_COMMIT_GOOD_DIST_MM:
            return True
        if dist <= self.CS_COMMIT_EDGE_DIST_MM:
            return (run >= self.CS_CONFIRM_EDGE
                    and speed <= self.CS_MAX_SPEED_MMPS)
        return False

    def _cs_lock_commit(self, tile, run, dist):
        """Remember a high-confidence commit so edge-bleed cannot overwrite it."""
        if (run >= self.CS_LOCK_RUN
                or (dist is not None and dist <= self.CS_LOCK_DIST_MM)):
            self._cs_locked_tiles.add(tile)

    def fuse_color_sensor_tile(self, rx, ry, yaw, data):
        """Place special floor tiles on the fixed 120 mm grid from the DOWNWARD
            sensors + GPS + IMU. Two sensors are combined:

              * COLOUR sensor -> the 8 non-hole tiles (6 connection colours, swamp,
                checkpoint). Its floor footprint is COLOR_SENSOR_FWD_MM ahead of the
                robot centre, so its reading belongs to centre + offset*heading.
              * DISTANCE / cliff sensor -> the black HOLE. Over a pit the beam finds
                no floor and the reading jumps above the learnt flat-floor level, so
                the hole is detected from a safe distance instead of by driving the
                colour sensor into it.

            Reads are median-low-passed, must agree for N frames, must be well inside
            one tile, and require a reliable pose and footprint motion margin.
            The farther-ahead cliff projection uses its own motion margin. Committing to
            _committed_floor_tiles shows the tile on BOTH maps (high-res + matrix).

            rx, ry are the robot-centre position relative to the start tile (mm);
            yaw is the IMU yaw (deg). Works at any orientation.
            """
        if not (self.COLOR_SENSOR_FUSE and self.FLOOR_FUSE):
            return
        yaw = float(data.get('orientation', {}).get('yaw_raw', yaw))
        col = data.get('color') or {}
        try:
            if isinstance(col, (list, tuple)) and len(col) >= 3:
                (r, g, b) = (int(col[0]), int(col[1]), int(col[2]))
            else:
                r = int(col['red'])
                g = int(col['green'])
                b = int(col['blue'])
        except (KeyError, TypeError, ValueError):
            return
        bang = math.radians(self.YAW_OFFSET_DEG + self.YAW_SIGN * yaw)
        sx = rx + self.COLOR_SENSOR_FWD_MM * math.sin(bang)
        sy = ry + self.COLOR_SENSOR_FWD_MM * math.cos(bang)
        sensor_tile = (int(round(sx / self.TILE_MM)), int(round(sy / self.TILE_MM)))
        if sensor_tile != self._cs_last_tile:
            self._cs_rgb_hist.clear()
            self._cs_confirm_char = None
            self._cs_last_tile = sensor_tile
        self._cs_rgb_hist.append((r, g, b))
        arr = np.asarray(self._cs_rgb_hist, dtype=np.float32)
        (rr, gg, bb) = (float(np.median(arr[:, i])) for i in range(3))
        ch = self.classify_color_sensor(rr, gg, bb)
        ch_raw = self.classify_color_sensor(r, g, b)
        tile = sensor_tile
        off_x = sx - tile[0] * self.TILE_MM
        off_y = sy - tile[1] * self.TILE_MM
        center_r = math.hypot(off_x, off_y)
        centered = abs(off_x) <= self.CS_CENTER_MARGIN_MM and abs(off_y) <= self.CS_CENTER_MARGIN_MM and (center_r <= self.CS_CENTER_R_MM)
        stamp_tile = tile
        t = data.get('time', 0.0)
        yaw_rate = 0.0
        if self._cs_prev_t is not None:
            dt = t - self._cs_prev_t
            if dt > 1e-06:
                dyaw = abs((yaw - self._cs_prev_yaw + 180.0) % 360.0 - 180.0)
                yaw_rate = dyaw / dt
        (self._cs_prev_yaw, self._cs_prev_t) = (yaw, t)
        speed = self.last_speed
        stable = yaw_rate <= self.CS_MAX_YAW_RATE_DPS and speed <= self.CS_MAX_SPEED_MMPS
        sample_s = float(data.get('sensor_period_s', 0.016))
        motion_margin = 2.0 + 2.0 * sample_s * (speed + self.COLOR_SENSOR_FWD_MM * math.radians(yaw_rate))
        tile_margin = self.TILE_MM / 2 - max(abs(off_x), abs(off_y))
        color_stable = speed <= self.CS_MAX_SPEED_MMPS and (stable or (0 < sample_s <= 0.032 and tile_margin > motion_margin))
        color_surface_occluded = False
        scan = data.get('lidar')
        if scan is not None and len(scan) > self.FRONT_INDEX:
            hres = int(data.get('lidar_hres', len(scan)))
            start = self.FRONT_INDEX // hres * hres
            front = self.FRONT_INDEX - start
            color_surface_occluded = any((self.MIN_RANGE_MM <= float(scan[start + i]) <= self.COLOR_SENSOR_FWD_MM for i in (front, (front - 1) % hres)))
        nonpassage = ch is None and ch_raw is None and self._is_bright_floor(r, g, b) and self._is_bright_floor(rr, gg, bb) and (center_r <= self.CS_COMMIT_GOOD_DIST_MM) and color_stable and (not color_surface_occluded)
        if nonpassage:
            self._nonpassage_read_run = self._nonpassage_read_run + 1 if self._nonpassage_read_tile == tile else 1
            self._nonpassage_read_tile = tile
            if self._nonpassage_read_run >= self.CS_CONFIRM:
                self._nonpassage_floor_tiles.add(tile)
        else:
            (self._nonpassage_read_tile, self._nonpassage_read_run) = (None, 0)
        if ch is None or ch_raw != ch or (not centered) or (not color_stable) or color_surface_occluded:
            self._cs_confirm_char = None
            self._cs_run_len = 0
            self._cs_run_best_tile = None
            self._cs_run_best_dist = None
        elif ch == self._cs_confirm_char:
            self._cs_run_len += 1
            if center_r < self._cs_run_best_dist:
                self._cs_run_best_dist = center_r
                self._cs_run_best_tile = tile
        else:
            self._cs_confirm_char = ch
            self._cs_run_len = 1
            self._cs_run_best_dist = center_r
            self._cs_run_best_tile = tile
        committed = None
        commit_source = None
        commit_tile = self._cs_run_best_tile
        commit_dist = self._cs_run_best_dist
        quality_ok = self._cs_quality_ok(self._cs_run_len, commit_dist, speed)
        if ch is not None and ch_raw == ch and quality_ok and centered and color_stable and (not color_surface_occluded) and (commit_tile is not None) and (commit_tile != self.start_tile):
            blocked = None
            replaced = None
            if self._committed_floor_tiles.get(commit_tile) == ch:
                commit_dist = min(commit_dist, self._cs_commit_dist.get(commit_tile, commit_dist))
            self._committed_floor_tiles[commit_tile] = ch
            if ch in self.CS_CONNECTION_CHARS:
                self._nonpassage_floor_tiles.discard(commit_tile)
            self._cs_commit_dist[commit_tile] = commit_dist
            self._provisional_holes.discard(commit_tile)
            self._cs_lock_commit(commit_tile, self._cs_run_len, commit_dist)
            vc = self._floor_votes.setdefault(commit_tile, {})
            vc[ch] = max(vc.get(ch, 0), self.FLOOR_CONFIRM) + 1
            committed = [list(commit_tile), ch]
            commit_source = 'color'
            self._log_floor_commit(t, 'color', ch, commit_tile, commit_dist, rx, ry, sx, sy, yaw, sensor_tile, off_x, off_y, center_r, self._cs_run_len, yaw_rate, blocked=blocked if False else None, replaced=replaced)
        if committed is None and (ch_raw is not None or ch is not None):
            reason = 'surface-before-floor' if color_surface_occluded else 'median-disagrees' if ch_raw != ch else 'tile-edge' if not centered else 'column' if False else 'pose' if not color_stable else 'confirmation' if not quality_ok else None
            if reason:
                self._log_floor_commit(t, 'color-rejected', ch_raw or ch, tile, center_r, rx, ry, sx, sy, yaw, sensor_tile, off_x, off_y, center_r, self._cs_run_len, yaw_rate, blocked=reason, extra={'rgb_raw': [r, g, b], 'rgb_median': [rr, gg, bb], 'tile_margin_mm': tile_margin, 'motion_margin_mm': motion_margin})
        dist_cm = data.get('distance')
        cliff = False
        strong = False
        wall_occluded = False
        flat_max_cm = None
        hole_tile = None
        if self.HOLE_FROM_DISTANCE and dist_cm is not None:
            dist_cm = float(dist_cm)
            if self._dist_floor_cm is None:
                if self.DIST_FLOOR_MIN_CM <= dist_cm <= self.DIST_FLOOR_MAX_CM:
                    self._dist_floor_cm = dist_cm
                floor_cm = self._dist_floor_cm
            else:
                floor_cm = self._dist_floor_cm
            cliff = floor_cm is not None and dist_cm >= floor_cm + self.DIST_CLIFF_DELTA_CM
            bright = self._is_bright_floor(rr, gg, bb)
            pit = self._is_pit_color(rr, gg, bb)
            orientation = data.get('orientation') or {}
            tilt = abs(orientation.get('roll', 0.0)) + abs(orientation.get('pitch', 0.0))
            down_angle = math.asin(self.DIST_SENSOR_DOWN) - tilt
            if down_angle > 0:
                mount_cm = math.hypot(self.DIST_SENSOR_HEIGHT_CM, 3.7)
                height_cm = self.DIST_SENSOR_HEIGHT_CM + 2 * mount_cm * math.sin(tilt / 2)
                flat_max_cm = height_cm / math.sin(down_angle) / self.DIST_IR_MIN_REFLECTION
                strong = dist_cm >= flat_max_cm + self.DIST_CLIFF_DELTA_CM and (floor_cm is None or dist_cm >= floor_cm + self.DIST_CLIFF_STRONG_CM)
                cliff = cliff or strong
            hx = rx + self.DIST_SENSOR_FWD_MM * math.sin(bang)
            hy = ry + self.DIST_SENSOR_FWD_MM * math.cos(bang)
            beam_tile = (int(round(hx / self.TILE_MM)), int(round(hy / self.TILE_MM)))
            ir_margin = self.TILE_MM / 2 - max(abs(hx - beam_tile[0] * self.TILE_MM), abs(hy - beam_tile[1] * self.TILE_MM))
            ir_motion = 2.0 + 2.0 * sample_s * (speed + self.DIST_SENSOR_FWD_MM * math.radians(yaw_rate))
            distance_stable = speed <= self.CS_MAX_SPEED_MMPS and (stable or (0 < sample_s <= 0.032 and ir_margin > ir_motion))
            known_floor = beam_tile == self.start_tile or self._committed_floor_tiles.get(beam_tile) in ('3', '4', 'b', 'y', 'g', 'p', 'o', 'r') or (beam_tile == sensor_tile and ch is not None)
            scan = data.get('lidar')
            if scan is not None and len(scan) > self.FRONT_INDEX:
                front_mm = float(scan[self.FRONT_INDEX])
                wall_occluded = self.MIN_RANGE_MM <= front_mm <= self.DIST_SENSOR_FWD_MM
            hole_ok = cliff and (not known_floor) and (not wall_occluded) and (pit and (not bright) or (strong and beam_tile != sensor_tile))
            if floor_cm is not None and (not cliff) and (not pit):
                if self.DIST_FLOOR_MIN_CM <= dist_cm <= self.DIST_FLOOR_MAX_CM:
                    self._dist_floor_cm += self.DIST_FLOOR_EMA * (dist_cm - self._dist_floor_cm)
            if hole_ok and distance_stable:
                hx = rx + self.DIST_SENSOR_FWD_MM * math.sin(bang)
                hy = ry + self.DIST_SENSOR_FWD_MM * math.cos(bang)
                ht = (int(round(hx / self.TILE_MM)), int(round(hy / self.TILE_MM)))
                if ht == self._cs_cliff_tile:
                    self._cs_cliff_run += 1
                else:
                    self._cs_cliff_tile = ht
                    self._cs_cliff_run = 1
            else:
                self._cs_cliff_tile = None
                self._cs_cliff_run = 0
            if hole_ok and self._cs_cliff_run >= self.CS_CLIFF_CONFIRM and distance_stable:
                hole_proj = self._cs_cliff_tile
                hole_tile = hole_proj
                hx = rx + self.DIST_SENSOR_FWD_MM * math.sin(bang)
                hy = ry + self.DIST_SENSOR_FWD_MM * math.cos(bang)
                hox = hx - hole_tile[0] * self.TILE_MM
                hoy = hy - hole_tile[1] * self.TILE_MM
                hole_dist = math.hypot(hox, hoy)
                hole_centered = abs(hox) <= self.CS_CENTER_MARGIN_MM and abs(hoy) <= self.CS_CENTER_MARGIN_MM and (hole_dist <= self.CS_CENTER_R_MM)
                hole_blocked = None
                hole_commit = False
                if hole_tile != self.start_tile and hole_centered:
                    hole_commit = True
                if hole_commit:
                    confirmed = self._committed_floor_tiles.get(hole_tile) == '2' and hole_tile not in self._provisional_holes
                    self._committed_floor_tiles[hole_tile] = '2'
                    self._cs_commit_dist[hole_tile] = min(hole_dist, self._cs_commit_dist.get(hole_tile, hole_dist))
                    vc = self._floor_votes.setdefault(hole_tile, {})
                    vc['2'] = max(vc.get('2', 0), self.FLOOR_CONFIRM) + 1
                    committed = [list(hole_tile), '2']
                    commit_source = 'cliff+color' if pit else 'cliff'
                    if pit:
                        self._provisional_holes.discard(hole_tile)
                        self._cs_lock_commit(hole_tile, self._cs_cliff_run, hole_dist)
                    elif not confirmed:
                        self._provisional_holes.add(hole_tile)
                elif hole_tile is not None and hole_tile != self.start_tile:
                    if not hole_centered:
                        hole_blocked = hole_blocked or 'center-gate'
                    elif not stable:
                        hole_blocked = hole_blocked or 'unstable'
                    else:
                        hole_blocked = hole_blocked or 'gate'
                self._log_floor_commit(t, 'hole', '2', hole_tile, hole_dist, rx, ry, hx, hy, yaw, hole_proj, hox, hoy, hole_dist, self._cs_cliff_run, yaw_rate, blocked=None if hole_commit else hole_blocked, extra={'dist_cm': dist_cm, 'floor_cm': floor_cm, 'fwd_mm': self.DIST_SENSOR_FWD_MM, 'pit_color': pit, 'strong': strong, 'hole_proj': list(hole_proj) if hole_proj else None, 'hole_snapped': False})
            if floor_cm is not None and self._provisional_holes and stable:
                fx = rx + self.DIST_SENSOR_FWD_MM * math.sin(bang)
                fy = ry + self.DIST_SENSOR_FWD_MM * math.cos(bang)
                fwd_ht = (int(round(fx / self.TILE_MM)), int(round(fy / self.TILE_MM)))
                veto = None
                reason = None
                forward_centered = math.hypot(fx - fwd_ht[0] * self.TILE_MM, fy - fwd_ht[1] * self.TILE_MM) <= self.CS_COMMIT_GOOD_DIST_MM
                if fwd_ht in self._provisional_holes and (not cliff) and (not wall_occluded) and forward_centered:
                    (veto, reason) = (fwd_ht, 'normal-floor-ahead')
                elif tile in self._provisional_holes and center_r <= self.CS_COMMIT_GOOD_DIST_MM and (ch_raw == ch) and (self._is_bright_floor(rr, gg, bb) or (ch is not None and ch != '2')):
                    (veto, reason) = (tile, 'colour-shows-floor')
                if veto is not None and self._committed_floor_tiles.get(veto) == '2':
                    del self._committed_floor_tiles[veto]
                    self._floor_votes.pop(veto, None)
                    self._provisional_holes.discard(veto)
                    self._cs_commit_dist.pop(veto, None)
                    if self._cs_cliff_tile == veto:
                        self._cs_cliff_tile = None
                        self._cs_cliff_run = 0
                    self._log_floor_commit(t, 'hole', '2', veto, 0.0, rx, ry, fx, fy, yaw, fwd_ht, 0.0, 0.0, 0.0, 0, yaw_rate, blocked='vetoed', extra={'dist_cm': dist_cm, 'floor_cm': floor_cm, 'reason': reason})
        if self.DEBUG_MAPPING_DUMP:
            self.last_color_sensor_fusion = {'rgb_median': [int(rr), int(gg), int(bb)], 'rgb_raw': [r, g, b], 'class': ch, 'class_raw': ch_raw, 'run': self._cs_run_len, 'tile_run': self._cs_run_len, 'confirm_char': self._cs_confirm_char, 'best_tile': list(self._cs_run_best_tile) if self._cs_run_best_tile is not None else None, 'best_dist_mm': round(self._cs_run_best_dist, 1) if self._cs_run_best_dist is not None else None, 'sensor_world_mm': [round(sx, 1), round(sy, 1)], 'tile': list(tile), 'stamp_tile': list(stamp_tile), 'column_snapped': False, 'tile_offset_mm': [round(off_x, 1), round(off_y, 1)], 'center_dist_mm': round(center_r, 1), 'centered': centered, 'column_ok': True, 'stable': color_stable, 'distance_pose_stable': stable, 'distance_footprint_stable': distance_stable if self.HOLE_FROM_DISTANCE and dist_cm is not None else False, 'tile_margin_mm': round(tile_margin, 2), 'motion_margin_mm': round(motion_margin, 2), 'quality_ok': quality_ok, 'color_surface_occluded': color_surface_occluded, 'nonpassage_floor_observed': tile in self._nonpassage_floor_tiles, 'speed_mmps': round(speed, 1), 'yaw_rate_dps': round(yaw_rate, 1), 'distance_cm': dist_cm, 'floor_dist_cm': round(self._dist_floor_cm, 1) if self._dist_floor_cm is not None else None, 'cliff': cliff, 'strong_cliff': strong, 'flat_ir_max_cm': flat_max_cm, 'wall_occluded': wall_occluded, 'pit_color': self._is_pit_color(rr, gg, bb), 'bright_floor': self._is_bright_floor(rr, gg, bb), 'hole_tile': list(hole_tile) if hole_tile else None, 'hole_proj': list(self._cs_cliff_tile) if self._cs_cliff_tile is not None else None, 'hole_snapped': False if hole_tile is not None else False, 'commit_source': commit_source, 'committed': committed, 'provisional_holes': sorted((list(ph) for ph in self._provisional_holes)), 'locked_tiles': sorted((list(lt) for lt in self._cs_locked_tiles))}

    def _obst_segments(self, mask, w):
        """Contiguous obstacle runs, gap-merged and noise/edge filtered.

        Mirrors the validated offline pass: merge runs split by <= CAM_GAP,
        drop runs narrower than CAM_MIN_SEG, and drop short runs touching the
        image edge (projection is least trustworthy there).
        """
        segs, s = [], None
        for c in range(w):
            if mask[c] and s is None:
                s = c
            elif not mask[c] and s is not None:
                segs.append([s, c - 1])
                s = None
        if s is not None:
            segs.append([s, w - 1])
        merged = []
        for a, b in segs:
            if merged and a - merged[-1][1] - 1 <= self.CAM_GAP:
                merged[-1][1] = b
            else:
                merged.append([a, b])
        out = []
        for a, b in merged:
            wdt = b - a + 1
            if wdt < self.CAM_MIN_SEG:
                continue
            if (a < self.CAM_EDGE or b > w - 1 - self.CAM_EDGE) and wdt < 6:
                continue
            out.append((a, b))
        return out

    def fuse_cameras(self, rx, ry, yaw, data):
        """Classify the side surfaces the lidar sees using the side cameras.

        The lidar says WHERE a surface is and, via its bearing, WHICH image
        column looks at it; the camera says WHETHER that surface is the wall
        material or something else. Obstacle = any near surface whose colour
        deviates from the learned wall reference (never the obstacle's own
        colour). A tile is only committed to 'x' after CAM_CONFIRM distinct
        frames agree, so no single-frame glitch can plant a false obstacle.
        """
        if not self.CAM_FUSE:
            return
        cams = data.get("cameras")
        if not cams:
            return
        w = int(data.get("cam_w", 0))
        h = int(data.get("cam_h", 0))
        if w <= 0 or h <= 0:
            return
        cam_fov = float(data.get("cam_fov", self.CAM_FOV_RAD))

        lidar = np.asarray(data["lidar"], dtype=np.float32)
        total = lidar.size
        hres = int(data.get("lidar_hres", total))
        lfov = float(data.get("lidar_fov", 2.0 * math.pi))
        if hres <= 0 or total < hres:
            return
        nlay = total // hres
        layer = min(self.FRONT_INDEX // hres, nlay - 1)
        start = layer * hres
        scan = lidar[start:start + hres]
        if scan.size < 8:
            return
        front = max(0, min(self.FRONT_INDEX - start, scan.size - 1))
        # The lidar layer just below the reference (layers run top->bottom), used
        # to confirm a surface is vertical (same range in both) and reject flat
        # coloured floor tiles. Absent on a single-layer lidar -> check skipped.
        down_scan = (lidar[(layer + 1) * hres:(layer + 2) * hres]
                     if layer + 1 < nlay else None)
        rad_per_idx = lfov / scan.size            # world angle per lidar index
        half_idx = (cam_fov / 2.0) / rad_per_idx  # camera half-width in indices

        # camera optical-centre local index: front is 0, +90 deg (robot right)
        # is +hres/4 in the same clockwise convention _integrate uses.
        centres = {"right": front + hres / 4.0, "left": front + 3.0 * hres / 4.0}
        base_ang = math.radians(self.YAW_OFFSET_DEG + self.YAW_SIGN * yaw)
        q = float(self.QUARTER_MM)              # 60 mm (half a tile)
        T = float(self.TILE_MM)                 # 120 mm tile pitch
        band = float(self.OBST_LINE_BAND_MM)    # tile-boundary reject band

        cols = np.arange(w, dtype=np.float32)
        per_side = {}
        for side, buf in (("left", cams.get("left")), ("right", cams.get("right"))):
            colH, colS = self._cam_band_hsv(buf, w, h, self.CAM_BAND_FRAC)
            if colH is None:
                continue
            idxf = centres[side] + (cols - w / 2.0) / (w / 2.0) * half_idx
            li = np.mod(np.rint(idxf).astype(np.int64), scan.size)
            rng = scan[li]
            near = (np.isfinite(rng)
                    & (rng >= self.CAM_RANGE_MIN_MM)
                    & (rng <= self.CAM_RANGE_MAX_MM))
            # Verticality: the layer below must see the SAME surface at the same
            # range. This is what tells a standing wall/obstacle apart from a
            # flat coloured floor tile (which the down beam hits nearer, or not
            # at all), so a floor colour change can never become an obstacle.
            if down_scan is not None:
                drng = down_scan[li]
                near &= np.isfinite(drng) & (np.abs(drng - rng) <= self.CAM_VERT_TOL_MM)
            per_side[side] = (colH, colS, idxf, rng, near)
            # feed the global wall reference from chromatic near columns (walls
            # dominate the run, so their look defines the reference).
            wm = near & (colS > self.CAM_S_FLOOR)
            if np.any(wm):
                a = np.deg2rad(colH[wm])
                self._wallref_cx += float(np.sum(colS[wm] * np.cos(a)))
                self._wallref_cy += float(np.sum(colS[wm] * np.sin(a)))
                self._wallref_n += int(np.count_nonzero(wm))

        if not per_side:
            return
        if self._wallref_n < self.CAM_WALLREF_MIN:
            if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: diagnostic payload
                self.last_cam_fusion = {"ready": False, "wallref_n": self._wallref_n}
            return

        wx = self._wallref_cx / self._wallref_n
        wy = self._wallref_cy / self._wallref_n
        Sw = math.hypot(wx, wy)
        thr = self.CAM_CHROMA_FRAC * Sw

        frame_tiles = set()
        victim_vetoed = []
        dbg_sides = {}
        for side, (colH, colS, idxf, rng, near) in per_side.items():
            buf = cams.get(side)
            rgb = self._cam_decode_rgb(buf, w, h)
            a = np.deg2rad(colH)
            cx = colS * np.cos(a)
            cy = colS * np.sin(a)
            dist = np.hypot(cx - wx, cy - wy)
            is_obst = near & (dist > thr)
            segs = self._obst_segments(is_obst, w)
            wall_v = None
            if rgb is not None:
                r0 = max(0, int(round(h * self.CAM_VICTIM_WHITE_ROWS[0])))
                r1 = min(h, max(r0 + 1, int(round(h * self.CAM_VICTIM_WHITE_ROWS[1]))))
                wall_cols = near & (~is_obst)
                if np.any(wall_cols):
                    mx_band = np.max(rgb[r0:r1], axis=2)
                    wall_v = float(np.median(mx_band[:, wall_cols]))
            side_tiles = set()
            vetoed_segs = []
            for (a0, a1) in segs:
                if (self._cam_fake_victim_white(rgb, w, h, a0, a1, wall_v=wall_v)
                        or self._seg_wall_flush(rng, a0, a1, w)):
                    vetoed_segs.append([a0, a1])
                    continue
                # ONE vote per obstacle segment, at its dominant (mode) tile.
                # Projecting every column scatters votes across neighbours when
                # the surface is viewed at an angle, so a wide/merged run could
                # otherwise plant a stray vote that repeats and false-commits.
                cnt = {}
                best_r = {}
                for c in range(a0, a1 + 1):
                    if not near[c]:
                        continue
                    r = float(rng[c])
                    ang = base_ang + (idxf[c] - front) * rad_per_idx
                    wxp = rx + math.sin(ang) * r
                    wyp = ry + math.cos(ang) * r
                    # An obstacle sits INSIDE a tile and never reaches a 120 mm
                    # tile boundary; every wall lies ON a tile-boundary line.
                    # Signs / tokens only ever live on walls, so a colour
                    # anomaly whose lidar hit lands on a tile boundary is a sign
                    # on a wall, not an obstacle: drop it and let the wall raster
                    # keep the wall. (Tile centres coincide with 60 mm lines, so
                    # gating on 60 mm lines would wrongly reject an obstacle's
                    # own centre - we gate on the 120 mm tile boundary instead.)
                    mx = (wxp - q) % T
                    my = (wyp - q) % T
                    if (min(mx, T - mx) <= band) or (min(my, T - my) <= band):
                        continue
                    t = (int(round(wxp / self.TILE_MM)),
                         int(round(wyp / self.TILE_MM)))
                    cnt[t] = cnt.get(t, 0) + 1
                    if t not in best_r or r < best_r[t]:
                        best_r[t] = r
                if not cnt:
                    continue
                # most columns wins; ties -> the nearest (most reliable) surface
                t = min(cnt, key=lambda k: (-cnt[k], best_r[k]))
                frame_tiles.add(t)
                if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: per-camera attribution
                    side_tiles.add(t)
            for (a0, a1) in vetoed_segs:
                cnt = {}
                best_r = {}
                for c in range(a0, a1 + 1):
                    if not near[c]:
                        continue
                    r = float(rng[c])
                    ang = base_ang + (idxf[c] - front) * rad_per_idx
                    wxp = rx + math.sin(ang) * r
                    wyp = ry + math.cos(ang) * r
                    mx = (wxp - q) % T
                    my = (wyp - q) % T
                    if (min(mx, T - mx) <= band) or (min(my, T - my) <= band):
                        continue
                    t = (int(round(wxp / self.TILE_MM)),
                         int(round(wyp / self.TILE_MM)))
                    cnt[t] = cnt.get(t, 0) + 1
                    if t not in best_r or r < best_r[t]:
                        best_r[t] = r
                if cnt:
                    t = min(cnt, key=lambda k: (-cnt[k], best_r[k]))
                    self._cam_clear_obstacle_tile(t)
                    if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: diagnostic history
                        victim_vetoed.append([side, list(t), [a0, a1]])
            # Single-column chromatic hits with a white victim plaque (too narrow
            # to form CAM_MIN_SEG) still must not keep a committed obstacle.
            for c in range(w):
                if not (near[c] and is_obst[c]):
                    continue
                if not self._cam_fake_victim_white(rgb, w, h, c, c, wall_v=wall_v):
                    continue
                r = float(rng[c])
                ang = base_ang + (idxf[c] - front) * rad_per_idx
                wxp = rx + math.sin(ang) * r
                wyp = ry + math.cos(ang) * r
                mx = (wxp - q) % T
                my = (wyp - q) % T
                if (min(mx, T - mx) <= band) or (min(my, T - my) <= band):
                    continue
                t = (int(round(wxp / self.TILE_MM)),
                     int(round(wyp / self.TILE_MM)))
                if t in self._cam_obstacle_tiles:
                    self._cam_clear_obstacle_tile(t)
                    if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: diagnostic history
                        victim_vetoed.append([side, list(t), [c, c]])
            if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: diagnostic payload
                dbg_sides[side] = {
                    "segments": segs,
                    "victim_vetoed": vetoed_segs,
                    "tiles": sorted(side_tiles),
                }

        newly = []
        for t in frame_tiles:
            self._obst_votes[t] = self._obst_votes.get(t, 0) + 1
            if (self._obst_votes[t] >= self.CAM_CONFIRM
                    and t not in self._cam_obstacle_tiles):
                self._cam_obstacle_tiles.add(t)
                if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: diagnostic history
                    newly.append(t)

        if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: diagnostic payload
            self.last_cam_fusion = {
                "ready": True,
                "wallref_n": int(self._wallref_n),
                "Hw": round(math.degrees(math.atan2(wy, wx)) % 360, 1),
                "Sw": round(Sw, 3),
                "thr": round(thr, 3),
                "sides": dbg_sides,
                "frame_tiles": sorted(frame_tiles),
                "victim_vetoed": victim_vetoed,
                "committed": sorted(self._cam_obstacle_tiles),
                "newly": sorted(newly),
            }

    def _grid_yaw(self, yaw):
        """Snap the heading to the nearest grid-aligned cardinal when it is close.

        The robot boots aligned to a cardinal and the tile grid is axis-parallel,
        so start_yaw + k*90 are the true grid headings. When the measured yaw is
        within FLOOR_YAW_SNAP_DEG of one of those, the robot is meant to be driving
        along the grid and the small deviation is IMU noise/bias - snap it out so
        the side tile geometry is exact. Genuine diagonal poses (far from a
        cardinal) are left untouched.
        """
        start = getattr(self, "_start_yaw", None)
        if start is None:
            return yaw
        dev = (yaw - start) % 90.0
        if dev > 45.0:
            dev -= 90.0
        return yaw - dev if abs(dev) <= self.FLOOR_YAW_SNAP_DEG else yaw

    def _floor_tile_core_clear(self, tx, ty):
        """Coarse plausibility check for side-camera-only floor guesses.

        Half-walls may cross a special tile's core. Occupancy here cannot
        disprove a floor type independently confirmed by the downward sensors.
        """
        gx = int(round(tx * self.TILE_MM / self.GRID_MM)) + self.c
        gy = int(round(ty * self.TILE_MM / self.GRID_MM)) + self.c
        half = int(round(self.FLOOR_CORE_HALF_MM / self.GRID_MM))
        if not (0 <= gy - half and gy + half < self.n
                and 0 <= gx - half and gx + half < self.n):
            return True
        win = self.logodds[gy - half:gy + half + 1, gx - half:gx + half + 1]
        return int((win >= self.OCC_T).sum()) < self.FLOOR_CORE_OCC_MAX

    def mapped_floor_tiles(self):
        """The same accepted floor labels for all three map representations."""
        return {tile: token for tile, token in self._committed_floor_tiles.items()
                if self.FLOOR_FUSE and tile != self.start_tile
                and tile not in self.obstacle_tiles
                and (tile in self._cs_commit_dist
                     or self._floor_tile_core_clear(*tile))}

    @classmethod
    def _floor_proj(cls, side, f, s, w):
        """Project robot-frame ground point (forward f, lateral s mm) to image
        (col,row) for the given side camera (left reuses the right homography
        mirrored). Returns None if behind the camera."""
        H = cls.FLOOR_H
        ss = s if side == "right" else -s
        den = H[2][0] * f + H[2][1] * ss + H[2][2]
        if abs(den) < 1e-9:
            return None
        col = (H[0][0] * f + H[0][1] * ss + H[0][2]) / den
        row = (H[1][0] * f + H[1][1] * ss + H[1][2]) / den
        if side != "right":
            col = (w - 1) - col
        return col, row

    def fuse_floor_tiles(self, rx, ry, yaw, data):
        """Detect coloured / special FLOOR tiles in ANY of the 6 side-neighbour
        positions (front-right, right, back-right, front-left, left, back-left)
        at ANY robot yaw, fusing the side cameras + lidar + GPS + IMU.

        Method (robust, precision-first):
          1. From GPS + IMU yaw, enumerate the robot's OWN tile and its 8
             neighbours (the own tile matters when the robot sits off-centre, so
             the tile it stands on extends far enough to a side to be seen).
          2. Project each candidate tile's real WORLD footprint into the image
             with the fixed ground homography and sample its interior.
          3. Per sample, keep only the trustworthy ones: far enough to the side
             (FLOOR_LAT_MIN_MM), bearing inside the side view (FLOOR_AZ_MAX_RAD),
             on lidar-confirmed floor (a wall must be nearer than the sample to
             occlude it). Classify the survivors.
          4. Require the tile to be strongly + mostly one colour AND its CENTRE to
             read that colour (so a neighbour bleeding onto an edge is rejected),
             then commit only after FLOOR_CONFIRM agreeing frames.

        Sampling the footprints of the KNOWN tiles (rather than localising blobs)
        plus the per-sample trust gates is what keeps false positives out: we
        never look anywhere but a real candidate tile, plain floor yields no
        colour, walls / tokens / obstacles are removed by the lidar gate, and
        under-robot / oblique samples that would land on the wrong tile are
        dropped. The left camera reuses the right homography mirrored (symmetric
        +/-200 mm mount).
        """
        if not (self.FLOOR_FUSE and self.FLOOR_SIDE_CAM_FUSE):
            return
        cams = data.get("cameras")
        if not cams:
            return
        w = int(data.get("cam_w", 0))
        h = int(data.get("cam_h", 0))
        if w <= 0 or h <= 0:
            return

        cam_fov = float(data.get("cam_fov", self.CAM_FOV_RAD))
        lidar = np.asarray(data["lidar"], dtype=np.float32)
        total = lidar.size
        hres = int(data.get("lidar_hres", total))
        if hres <= 0 or total < hres:
            return
        nlay = total // hres
        lfov = float(data.get("lidar_fov", 2.0 * math.pi))
        layer = min(self.FRONT_INDEX // hres, nlay - 1)
        ref = lidar[layer * hres:(layer + 1) * hres]
        if ref.size < 8:
            return
        # layer just below the reference confirms a surface is vertical (both see
        # the same range) so a coloured floor tile is never mistaken for a wall.
        dwn = (lidar[(layer + 1) * hres:(layer + 2) * hres]
               if layer + 1 < nlay else None)
        front = max(0, min(self.FRONT_INDEX - layer * hres, ref.size - 1))
        rad_per_idx = lfov / ref.size
        half_idx = (cam_fov / 2.0) / rad_per_idx if rad_per_idx else 0.0
        centres = {"right": front + hres / 4.0, "left": front + 3.0 * hres / 4.0}

        # snap the heading to the grid when near-aligned (rules: start is cardinal,
        # tiles are axis-parallel) so the side tile geometry is exact.
        b = math.radians(self.YAW_OFFSET_DEG + self.YAW_SIGN * self._grid_yaw(yaw))
        fwd = (math.sin(b), math.cos(b))
        rgt = (math.cos(b), -math.sin(b))
        T = float(self.TILE_MM)
        rtx, rty = int(round(rx / T)), int(round(ry / T))
        row0 = self.FLOOR_ROW0
        inset = self.FLOOR_INSET_MM
        grid = [x / (self.FLOOR_KGRID - 1.0) * 2.0 * inset - inset
                for x in range(self.FLOOR_KGRID)]

        # decode both frames + per-column lidar floor gate once
        # per-column lidar surface range: distance to a near VERTICAL surface
        # (wall / obstacle) in that camera bearing, or inf when the bearing is
        # clear floor. A surface only occludes a floor sample nearer/at it.
        rgb_of = {}
        surf = {}
        raw_side = {}
        for side, buf in (("left", cams.get("left")), ("right", cams.get("right"))):
            if buf is None:
                continue
            try:
                arr = np.frombuffer(buf, dtype=np.uint8)
            except (TypeError, ValueError):
                continue
            if arr.size < w * h * 4:
                continue
            rgb_of[side] = arr[:w * h * 4].reshape(h, w, 4)[:, :, 2::-1].astype(np.float32) / 255.0
            srng = [float("inf")] * w
            rrng = [float("inf")] * w
            for c in range(w):
                idxf = centres[side] + (c - w / 2.0) / (w / 2.0) * half_idx
                li = int(round(idxf)) % ref.size
                rng = float(ref[li])
                if math.isfinite(rng):
                    rrng[c] = rng
                vertical = (math.isfinite(rng)
                            and self.CAM_RANGE_MIN_MM <= rng <= self.CAM_RANGE_MAX_MM)
                if vertical and dwn is not None:
                    dr = float(dwn[li])
                    vertical = (math.isfinite(dr)
                                and abs(dr - rng) <= self.CAM_VERT_TOL_MM)
                if vertical:
                    srng[c] = rng
            surf[side] = srng
            raw_side[side] = rrng

        occ = self.FLOOR_OCC_MARGIN_MM
        frame_hits = {}
        dbg_sides = {"left": [], "right": []}

        # ---- SIDE-AIM path: mark the 120 mm tile the side vector AIMS at ----
        # For each side camera, cast the robot's side vector (perpendicular to the
        # heading, from GPS + IMU) and pick the tile whose CENTRE that vector points
        # at: perpendicular offset |f0| small (the centre sits on the side line) and
        # the NEAREST such tile inside the camera's reliable lateral band. Sample
        # that tile's visible interior and accept the colour only if it dominates
        # AND the tile-centre pixel itself reads it (kills an adjacent tile bleeding
        # onto an edge). Because |f0| must be small, only a RIGHT/LEFT tile ever
        # qualifies - never a forward/diagonal one - yet the tile is still found
        # when the robot is half on it or driving diagonally (both missed before).
        for sgn, side in ((1.0, "right"), (-1.0, "left")):
            if side not in rgb_of:
                continue
            rgb = rgb_of[side]
            srng = surf[side]
            # nearest tile whose centre the side vector aims at
            aim = None
            for dx in (-2, -1, 0, 1, 2):
                for dy in (-2, -1, 0, 1, 2):
                    tx, ty = rtx + dx, rty + dy
                    cx, cy = tx * T, ty * T
                    f0 = (cx - rx) * fwd[0] + (cy - ry) * fwd[1]
                    s0 = (cx - rx) * rgt[0] + (cy - ry) * rgt[1]
                    lat = s0 * sgn
                    if abs(f0) >= self.FLOOR_AIM_F_MAX:
                        continue                    # vector does not aim at this centre
                    if not (self.FLOOR_AIM_S_MIN <= lat <= self.FLOOR_AIM_S_MAX):
                        continue
                    key = (lat, abs(f0))             # nearest aimed tile wins
                    if aim is None or key < aim[0]:
                        aim = (key, tx, ty, f0, s0)
            if aim is None:
                continue
            _, tx, ty, f0, s0 = aim
            cx, cy = tx * T, ty * T
            votes = {}
            rows_of = {}                            # winning-colour vertical extent
            npx = 0
            for a in grid:                          # sample the tile interior (world)
                for bb in grid:
                    wx, wy = cx + a, cy + bb
                    f = (wx - rx) * fwd[0] + (wy - ry) * fwd[1]
                    s = (wx - rx) * rgt[0] + (wy - ry) * rgt[1]
                    if s * sgn < self.FLOOR_AIM_LAT_FLOOR:
                        continue                    # too far under the robot to read
                    if abs(math.atan2(f, abs(s))) > self.FLOOR_AIM_AZ_MAX:
                        continue
                    pr = self._floor_proj(side, f, s, w)
                    if pr is None:
                        continue
                    ci, ri = int(round(pr[0])), int(round(pr[1]))
                    if not (0 <= ci < w and row0 <= ri < h):
                        continue
                    # occlusion: skip only if a wall/obstacle sits at or nearer
                    # than this sample's ground range (a wall BEYOND is fine).
                    if srng[ci] <= math.hypot(f, s) + occ:
                        continue
                    npx += 1
                    p = rgb[ri, ci]
                    ch = self.classify_floor_rgb(float(p[0]), float(p[1]), float(p[2]))
                    if ch is not None:
                        votes[ch] = votes.get(ch, 0) + 1
                        rows_of.setdefault(ch, set()).add(ri)
            nclass = sum(votes.values())
            if (npx < self.FLOOR_MIN_PIX or nclass < self.FLOOR_MIN_CLASS
                    or nclass < self.FLOOR_COVER_FRAC * npx):
                continue
            best = max(votes, key=votes.get)
            if votes[best] < self.FLOOR_VOTE_FRAC * nclass:
                continue
            if len(rows_of.get(best, ())) < self.FLOOR_MIN_ROWS:
                continue
            # the tile CENTRE pixel must itself read the winning colour - a
            # neighbour tile whose colour merely bleeds onto this tile's edge does
            # not colour its centre, so this rejects the bleed / plain-tile case.
            prc = self._floor_proj(side, f0, s0, w)
            if prc is None:
                continue
            cci, cri = int(round(prc[0])), int(round(prc[1]))
            if not (0 <= cci < w and row0 <= cri < h):
                continue
            wall_mm = self._side_wall_range(raw_side[side], cci)
            if wall_mm < self.FLOOR_SIDE_WALL_MIN_MM:
                if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: diagnostic payload
                    dbg_sides[side].append({
                        "blocked": "side-wall-too-close",
                        "tile": [tx, ty],
                        "wall_mm": round(float(wall_mm), 1),
                        "min_required_mm": self.FLOOR_SIDE_WALL_MIN_MM,
                    })
                continue
            pc = rgb[cri, cci]
            if self.classify_floor_rgb(float(pc[0]), float(pc[1]), float(pc[2])) != best:
                continue
            frame_hits.setdefault((tx, ty), best)
            if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: diagnostic payload
                dbg_sides[side].append({"ch": best, "tile": [tx, ty],
                                        "f": round(f0), "s": round(s0),
                                        "wall_mm": (round(float(wall_mm), 1)
                                                    if math.isfinite(wall_mm) else None),
                                        "npx": npx, "nclass": nclass})

        # robot viewpoint bin (for the multi-view gate) - the tile stays fixed on
        # the grid while the robot moves, so distinct bins = distinct vantage points.
        pose_bin = (int(round(rx / self.FLOOR_VIEW_BIN_MM)),
                    int(round(ry / self.FLOOR_VIEW_BIN_MM)))
        newly = []
        for t, ch in frame_hits.items():
            # The robot physically stands on its own tile - it cannot be on a hole
            # (it would have fallen in), so a hole attributed to the current tile is
            # impossible: never let it vote.
            if ch == "2" and t == (rtx, rty):
                continue
            # Holes belong to the downward distance/cliff sensor only; side-camera
            # '2' reads (far dark walls / shadows) were the false-positive source.
            if ch == "2" and not self.FLOOR_SIDE_CAM_HOLES:
                continue
            vc = self._floor_votes.setdefault(t, {})
            vc[ch] = vc.get(ch, 0) + 1
            vw = self._floor_views.setdefault(t, {}).setdefault(ch, set())
            vw.add(pose_bin)
            # commit only when a single colour has enough agreeing frames, is seen
            # from >= FLOOR_MIN_VIEWS DISTINCT robot viewpoints (a one-spot misread
            # never clears this), clearly dominates every vote for the tile, and the
            # tile core is clear of wall/obstacle occupancy (lidar overrules the
            # camera - a colour sitting on a wall, e.g. a dark wall read as a hole,
            # is discarded).
            if (vc[ch] >= self.FLOOR_CONFIRM
                    and len(vw) >= self.FLOOR_MIN_VIEWS
                    and vc[ch] >= 0.7 * sum(vc.values())
                    and self._committed_floor_tiles.get(t) != ch
                    and self._floor_tile_core_clear(*t)):
                self._committed_floor_tiles[t] = ch
                if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: diagnostic history
                    newly.append((t, ch))

        if self.DEBUG_MAPPING_DUMP:  # DEBUG ONLY: diagnostic payload
            self.last_floor_fusion = {
                "sides": dbg_sides,
                "frame_hits": {f"{k[0]},{k[1]}": v for k, v in frame_hits.items()},
                "committed": {f"{k[0]},{k[1]}": v
                              for k, v in self._committed_floor_tiles.items()},
                "newly": [[list(t), c] for t, c in newly],
            }

    def _side_wall_range(self, ranges, center_col):
        if not ranges:
            return float("inf")
        c0 = max(0, int(center_col) - self.FLOOR_SIDE_WALL_COLW)
        c1 = min(len(ranges), int(center_col) + self.FLOOR_SIDE_WALL_COLW + 1)
        vals = [float(v) for v in ranges[c0:c1] if math.isfinite(float(v))]
        return min(vals) if vals else float("inf")

    # ------------------------------------------------------------------ #
    #  Tile / wall logic
    # ------------------------------------------------------------------ #
    def _tile_of(self, rx, ry):
        return int(round(rx / self.TILE_MM)), int(round(ry / self.TILE_MM))

    def floor_neighbor_tiles(self, rx, ry, yaw):
        """Robot-frame neighbour tiles for the side-camera floor search.

        Returns the robot's own tile, its intra-tile offset (how far the robot
        centre sits from its tile centre - usually NOT zero), and, for each
        robot-relative position (right/left, and front/back one row), the world
        tile index + world centre in mm. The right camera looks at the R/RF/RB
        set, the left camera at L/LF/LB; RR/LL are the +2 stretch targets.

        Map frame: +y=North, +x=East. forward=(sin b,cos b), right=(cos b,-sin
        b) with b = radians(YAW_OFFSET_DEG + YAW_SIGN*yaw), matching _integrate
        and fuse_cameras exactly.
        """
        T = float(self.TILE_MM)
        b = math.radians(self.YAW_OFFSET_DEG + self.YAW_SIGN * yaw)
        fwd = (math.sin(b), math.cos(b))
        rgt = (math.cos(b), -math.sin(b))
        rt = (int(round(rx / T)), int(round(ry / T)))
        cx, cy = rt[0] * T, rt[1] * T
        offset = (round(rx - cx, 1), round(ry - cy, 1))
        combos = {
            "R": (1, 0), "RF": (1, 1), "RB": (1, -1),
            "L": (-1, 0), "LF": (-1, 1), "LB": (-1, -1),
            "RR": (2, 0), "LL": (-2, 0),
            "F": (0, 1), "B": (0, -1),
        }
        out = {}
        for label, (dr, dfwd) in combos.items():
            wx = cx + rgt[0] * dr * T + fwd[0] * dfwd * T
            wy = cy + rgt[1] * dr * T + fwd[1] * dfwd * T
            out[label] = {
                "tile": [int(round(wx / T)), int(round(wy / T))],
                "world_mm": [round(wx, 1), round(wy, 1)],
            }
        return {
            "robot_tile": [rt[0], rt[1]],
            "robot_world_mm": [round(rx, 1), round(ry, 1)],
            "intra_tile_offset_mm": list(offset),
            "forward_unit": [round(fwd[0], 4), round(fwd[1], 4)],
            "right_unit": [round(rgt[0], 4), round(rgt[1], 4)],
            "neighbors": out,
        }

    def _detect_walls(self):
        """Confirm wall segments from the occupancy grid.

        A point lying on a wall also crosses the perpendicular grid-lines, so
        per-point voting is ambiguous. Instead we require *coverage*: a real
        wall segment is backed by many occupied cells spanning that tile edge,
        whereas a mere corner crossing contributes only one or two.
        """
        t, h = self.TILE_MM, self.QUARTER_MM
        occupied=self.logodds>=self.OCC_T
        if self._obstacle_mask is not None:
            occupied &= ~self._obstacle_mask
        ys, xs = np.where(occupied)
        if xs.size == 0:
            return set(), set()

        wx = (xs - self.c) * self.GRID_MM
        wy = (ys - self.c) * self.GRID_MM

        fys, fxs = np.where(self.logodds <= self.FREE_T)
        fwx = (fxs - self.c) * self.GRID_MM
        fwy = (fys - self.c) * self.GRID_MM

        def edges(coordinate, along, free_coordinate, free_along):
            edge = np.rint((coordinate-h)/t)
            use = abs(coordinate-(t*edge+h)) <= self.EDGE_TOL_MM
            samples = np.column_stack((edge[use], np.rint(along[use]/t))).astype(np.int64)
            keys, labels = np.unique(samples, axis=0, return_inverse=True)
            if not len(keys):
                return set()
            cells = np.unique(np.column_stack((labels, np.rint(along[use]/self.GRID_MM).astype(np.int64))), axis=0)
            cover = np.bincount(cells[:,0], minlength=len(keys))
            low = np.full(len(keys), np.inf); high = np.full(len(keys), -np.inf)
            np.minimum.at(low, labels, along[use]); np.maximum.at(high, labels, along[use])
            free_edge = np.rint((free_coordinate-h)/t)
            clear = abs(free_coordinate-(t*free_edge+h)) <= self.FREE_EDGE_TOL_MM
            free_keys, count = np.unique(np.column_stack((free_edge[clear], np.rint(free_along[clear]/t))).astype(np.int64), axis=0, return_counts=True)
            free = dict(zip(map(tuple, free_keys), count))
            accepted = (cover >= self.WALL_MIN_COVER) & (high-low >= self.WALL_MIN_SPAN_MM)
            return {tuple(map(int,key)) for key, votes, ok in zip(keys,cover,accepted)
                    if ok and votes > free.get(tuple(key),0)}
        vertical = edges(wx, wy, fwx, fwy)
        horizontal = {(x,y) for y,x in edges(wy, wx, fwy, fwx)}
        return vertical, horizontal

    def _detect_obstacles(self):
        """Detect LiDAR objects without treating connected walls as objects.

        Independently bounded raw face pairs can identify a compact body even
        when its10mm occupancy cells join a nearby wall. Raw surface evidence
        retains the body's identity and a separate occupied-cell semantic mask.
        The component detector below remains the fallback for other isolated
        bodies: compact, off-grid, surrounded by observed free space, and not
        explained by a permitted straight, sharp, or curved wall.

        Returns (tiles, mask). Neither path removes occupied material from the
        physical map or navigation. The caller excludes positively identified
        object cells from wall fitting and marks the object's tile with'x'.
        """
        n = self.n
        occ = self.logodds >= self.OCC_T
        tiles = set()
        mask = np.zeros((n, n), dtype=bool)
        if not occ.any():
            self._persistent_obstacle_cells.clear()
            self._persistent_obstacle_tiles.clear()
            self._raw_obstacle_cells.clear()
            self._raw_obstacle_regions.clear()
            return tiles, mask

        grid = self.GRID_MM
        q = float(self.QUARTER_MM)
        band = self.OBST_LINE_BAND_MM
        # Raw circle support exists before report-raster confirmation. Do not
        # classify its measured component as a solid object and then remove
        # the very samples needed to confirm the curved wall.
        curve_models = self._confirmed_curve_quadrants | {
            model for model,votes in self._scan_curve_votes.items()
            if self._native_curve_samples and votes >= self.ARC_SOFT_HITS}

        seen = np.zeros((n, n), dtype=bool)
        ys, xs = np.where(occ)
        max_cells = self.OBST_MAX_CELLS

        for sy, sx in zip(ys.tolist(), xs.tolist()):
            if seen[sy, sx]:
                continue
            # 8-connected component in the RAW occupancy. A free-standing object
            # is its own small component; the arena walls form one large linked
            # component that trips the size cap below (overflow => skip).
            comp = []
            stack = [(sy, sx)]
            seen[sy, sx] = True
            overflow = False
            while stack:
                cy, cx = stack.pop()
                comp.append((cy, cx))
                if len(comp) > max_cells:
                    overflow = True            # too large for the component fallback
                for dy in (-1, 0, 1):
                    for dx in (-1, 0, 1):
                        if dy == 0 and dx == 0:
                            continue
                        ny, nx = cy + dy, cx + dx
                        if (0 <= ny < n and 0 <= nx < n
                                and occ[ny, nx] and not seen[ny, nx]):
                            seen[ny, nx] = True
                            stack.append((ny, nx))
            if overflow or len(comp) < self.OBST_MIN_CELLS:
                continue

            cys = [p[0] for p in comp]
            cxs = [p[1] for p in comp]
            miny, maxy = min(cys), max(cys)
            minx, maxx = min(cxs), max(cxs)
            h_mm = (maxy - miny + 1) * grid
            w_mm = (maxx - minx + 1) * grid
            if max(h_mm, w_mm) > self.OBST_MAX_MM:
                continue                        # spans more than a tile => wall
            if min(h_mm, w_mm) < self.OBST_MIN_SIDE_MM:
                continue                        # a 1-cell sliver => wall fragment

            # SOLID: an obstacle is a filled box; a stray disconnected wall arc is
            # a thin curve that barely fills its bounding box.
            fill = len(comp) / float((maxy - miny + 1) * (maxx - minx + 1))
            if fill < self.OBST_MIN_FILL:
                continue

            # SECONDARY guard: a disconnected straight wall fragment still lies ON
            # the 60 mm grid lines; an obstacle sits off them. Require a real
            # fraction of cells to be off BOTH 60 mm lines.
            wxs = (np.array(cxs) - self.c) * grid
            wys = (np.array(cys) - self.c) * grid
            # A thin, complete 60 mm grid-wall segment is still a wall when
            # pose/voxel spread gives it several occupied rows. Repeated views
            # must not promote that smear to an isolated object.
            grid_wall = False
            for along, across in ((wxs, wys), (wys, wxs)):
                axis = q*round(float(np.median(across))/q)
                on_axis = abs(across-axis) <= grid
                if (np.ptp(across)+grid <= 2*self.ARC_RADIUS_TOL_MM+grid
                        and np.count_nonzero(on_axis) >= 4
                        and np.ptp(along[on_axis]) >= q-grid
                        and abs(along.min()-q*round(float(along.min())/q)) <= grid
                        and abs(along.max()-q*round(float(along.max())/q)) <= grid):
                    grid_wall = True
                    break
            # Losing occupied samples cannot turn an accepted object into a
            # wall. A new wall-shaped extension may contradict that identity;
            # a subset of its previous body is only a weaker observation.
            if grid_wall and not all(p in self._persistent_obstacle_cells for p in comp):
                continue
            # A connected component containing an already confirmed arc is a
            # wall network, even when two adjacent curves make it compact and
            # apparently solid. Associate actual candidate-hit cells with the
            # confirmed quadrant; do not rely on the bounding box alone.
            curve_component = False
            evidence = self.arc_grid[cys, cxs]+self.convex_arc_grid[cys, cxs]
            for bx, by, ax, ay in curve_models:
                px, py = ax*(wxs-bx*q), ay*(wys-by*q)
                fit = ((px >= grid) & (py >= grid) & (px <= q) & (py <= q)
                    & (evidence > 0)
                    & (abs(np.hypot(q-px, q-py)-q) <= self.ARC_RADIUS_TOL_MM))
                if (np.count_nonzero(fit) >= 3
                        and np.any(px[fit] < py[fit]) and np.any(px[fit] > py[fit])):
                    curve_component = True
                    break
            if curve_component:
                continue
            # Seeing both faces of a curved wall fills more of its bounding
            # box than either face alone. Do not let that extra evidence turn
            # a measured 60 mm quarter-circle into a free-standing obstacle.
            # Require the whole component to fit the arc, both endpoint spans,
            # and the same scan-time arc evidence used by wall conversion.
            is_arc = False
            if min(h_mm, w_mm) >= q:
                for bx in {q*round(float(wxs.min())/q), q*round(float(wxs.max())/q)}:
                    for by in {q*round(float(wys.min())/q), q*round(float(wys.max())/q)}:
                        for ax, ay in ((1, 1), (-1, 1), (1, -1), (-1, -1)):
                            px, py = ax*(wxs-bx), ay*(wys-by)
                            if (px.min() < -grid or py.min() < -grid
                                    or px.max() > q+grid or py.max() > q+grid
                                    or np.ptp(px) < q-grid or np.ptp(py) < q-grid):
                                continue
                            residual = abs(np.hypot(q-px, q-py)-q)
                            if (np.all(residual <= self.ARC_RADIUS_TOL_MM)
                                    and self._arc_evidence(bx, by, ay > 0, ay < 0,
                                                          ax < 0, ax > 0) >= self.ARC_SOFT_HITS):
                                is_arc = True
                                break
                        if is_arc:
                            break
                    if is_arc:
                        break
            if is_arc:
                continue
            off_x = np.abs(wxs - q * np.round(wxs / q)) > band
            off_y = np.abs(wys - q * np.round(wys / q)) > band
            previously_confirmed = any(p in self._persistent_obstacle_cells for p in comp)
            if (not previously_confirmed
                    and float(np.mean(off_x & off_y)) < self.OBST_MIN_OFFGRID):
                continue                        # aligned to the 60 mm grid => wall

            # A fragment with unknown space at an endpoint is not yet proven
            # free-standing. Require a complete observed-free perimeter before
            # removing it from the wall map and reporting an obstacle. Otherwise
            # retain its measured geometry until another view resolves it.
            if minx == 0 or miny == 0 or maxx == n-1 or maxy == n-1:
                continue
            perimeter = np.concatenate((self.logodds[miny-1, minx-1:maxx+2],
                self.logodds[maxy+1, minx-1:maxx+2],
                self.logodds[miny:maxy+1, minx-1],
                self.logodds[miny:maxy+1, maxx+1]))
            if not previously_confirmed and not np.all(perimeter <= self.FREE_T):
                continue

            cyc = sum(cys) / len(comp)
            cxc = sum(cxs) / len(comp)
            tx = int(round(((cxc - self.c) * grid) / self.TILE_MM))
            ty = int(round(((cyc - self.c) * grid) / self.TILE_MM))
            tiles.add((tx, ty))
            for cy, cx in comp:
                mask[cy, cx] = True
        # Re-evaluate every build. A partial wall can initially look like an
        # isolated object; later scans connecting it to the wall must revoke 'x'.
        raw_tiles,raw_mask=self._raw_supported_obstacles()
        tiles |= raw_tiles
        mask |= raw_mask
        self._persistent_obstacle_tiles = tiles.copy()
        self._persistent_obstacle_cells = set(zip(*np.where(mask)))
        return tiles, mask

    def _obstacle_mask_from_tiles(self, tiles):
        """High-res mask of just the OFF-GRID BODY of each committed obstacle
        tile - never the walls in or around it.

        An obstacle sits inside the tile, off the 60 mm grid; a wall (whether it
        is the tile's own boundary, or a wall the obstacle stands in front of /
        beside / in the corner of) lies ON a 60 mm line. So we only pull cells
        that are off BOTH 60 mm lines out of the wall population; every on-grid
        wall cell is kept and still rasterises to '1'. This is what lets an
        obstacle be marked 'x' WITHOUT erasing the walls behind or beside it.
        """
        if not tiles:
            return None
        n = self.n
        mask = np.zeros((n, n), dtype=bool)
        occ = self.logodds >= self.OCC_T
        T = float(self.TILE_MM)
        q = float(self.QUARTER_MM)
        grid = float(self.GRID_MM)
        band = float(self.OBST_LINE_BAND_MM)
        half_int = q - float(self.OBST_TILE_TOUCH_MM)
        for (tx, ty) in tiles:
            cxw = tx * T
            cyw = ty * T
            lo_x = max(0, int(math.floor((cxw - half_int) / grid)) + self.c)
            hi_x = min(n - 1, int(math.ceil((cxw + half_int) / grid)) + self.c)
            lo_y = max(0, int(math.floor((cyw - half_int) / grid)) + self.c)
            hi_y = min(n - 1, int(math.ceil((cyw + half_int) / grid)) + self.c)
            if lo_x > hi_x or lo_y > hi_y:
                continue
            for cy in range(lo_y, hi_y + 1):
                wy = (cy - self.c) * grid
                on_y = abs(wy - q * round(wy / q)) <= band
                for cx in range(lo_x, hi_x + 1):
                    if not occ[cy, cx]:
                        continue
                    wx = (cx - self.c) * grid
                    on_x = abs(wx - q * round(wx / q)) <= band
                    if on_x or on_y:
                        continue            # on a 60 mm line => wall, keep it
                    mask[cy, cx] = True     # off-grid body => pull out of walls
        return mask

    def _estimate_grid_offset(self, occ_wx, occ_wy):
        """Estimate the global GPS-drift offset (dx, dy) in mm, matrix only.

        Idea (areas 1-3 are tile-based): every straight wall lies on a 60 mm
        sub-grid line, so for an axis-aligned wall cell the signed distance to
        its nearest 60 mm line *is* the GPS drift along that axis plus a little
        lidar noise. Each wall cell votes only on the axis it is closest to a
        line on (a vertical-wall cell has a small x-residual -> votes dx; a
        horizontal-wall cell votes dy), so straight walls of both orientations
        each correct their own axis.

        Robustness: arbitrary-shaped obstacles and curves (allowed even in
        areas 1-3) sit mid-tile or scatter their residuals, so they fall
        outside the narrow vote band or are rejected by the MEDIAN. The result
        is hard-bounded to less than half the 60 mm spacing, so a bad estimate
        can never move a wall onto the wrong line. When the map is already
        aligned the median is ~0 and nothing changes.
        """
        q = float(self.QUARTER_MM)
        band = float(self.MATRIX_GRID_LOCK_BAND_MM)
        if occ_wx.size == 0:
            return 0.0, 0.0

        res_x = occ_wx - q * np.round(occ_wx / q)
        res_y = occ_wy - q * np.round(occ_wy / q)
        ax, ay = np.abs(res_x), np.abs(res_y)

        # route each wall cell to the axis on which it hugs a line
        vx = res_x[(ax <= band) & (ax <= ay)]
        vy = res_y[(ay <= band) & (ay < ax)]

        lim = float(self.MATRIX_GRID_LOCK_MAX_MM)
        def offset(votes):
            if votes.size < self.MATRIX_GRID_LOCK_MIN_VOTES:
                return 0.0
            return max(-lim, min(lim, float(np.median(votes))))
        return offset(vx), offset(vy)

    # ------------------------------------------------------------------ #
    #  Competition matrix (Erebus format)
    # ------------------------------------------------------------------ #
    def _room_body_clear(self):
        """Exact EDT threshold, reused while occupied cells/geometry stay equal."""
        occupied = self.logodds >= self.OCC_T
        grid, radius = self.GRID_MM, NavigationMapper.ROBOT_RADIUS_MM
        key = (occupied.shape, grid, radius, np.packbits(occupied).tobytes())
        cached = getattr(self, '_room_body_clear_cache', None)
        if cached is not None and cached[0] == key:
            return cached[1]
        if not occupied.any() or radius <= 0 or radius/grid > min(occupied.shape):
            # Preserve SciPy's special all-free finite-array boundary semantics.
            clear = scipy_ndimage.distance_transform_edt(~occupied)*grid >= radius
        else:
            extent = math.ceil(radius/grid)
            y, x = np.mgrid[-extent:extent+1, -extent:extent+1]
            footprint = np.sqrt(x*x+y*y)*grid < radius
            # A cell fails the original >=radius test iff an occupied cell is
            # strictly inside this disk. Outside-array space adds no obstacle.
            clear = scipy_ndimage.binary_erosion(~occupied, footprint, border_value=1)
        clear.flags.writeable = False
        self._room_body_clear_cache = key, clear
        return clear

    def _resolve_observed_rooms(self):
        """Identify rooms from traversable observed floor and passage colours.

        A missed/edge-only passage must not carry the previous room into the
        next one. Weak passages separate regions but do not assign a room;
        other reliable passages can resolve both sides later. No world labels
        or arbitrary flood-fill seeds enter this calculation.
        """
        coords = (np.arange(self.n)-self.c)*self.GRID_MM
        tiles = np.floor((coords+self.TILE_MM/2)/self.TILE_MM).astype(int)
        traversable = ((self.logodds <= self.FREE_T) | self.visited) & self._room_body_clear()
        connections = {"b": (1, 2), "y": (1, 3), "g": (1, 4),
                       "p": (2, 3), "o": (2, 4), "r": (3, 4)}
        passages = {t: connections[ch] for t, ch in self._committed_floor_tiles.items()
                    if ch in connections}
        floor = traversable.copy()
        # LiDAR sees across a pit, but that free beam is not traversable floor.
        # A confirmed hole must separate room regions just as it does routing.
        barriers = set(passages) | {tile for tile, token in self._committed_floor_tiles.items()
                                   if token == "2"}
        for tx, ty in barriers:
            floor[np.ix_(tiles == ty, tiles == tx)] = False
        labels, count = scipy_ndimage.label(floor)
        rooms = [set((1, 2, 3, 4)) for _ in range(count+1)]
        rooms[0] = set()
        start = int(labels[self.c, self.c])
        if start:
            rooms[start] = {1}
        signature = tuple(sorted((tile, self._committed_floor_tiles[tile]) for tile in passages))
        established = (dict(self._room_tiles) if signature == self._room_model_signature
                       else {self.start_tile: 1})
        if signature == self._room_model_signature:
            # A later ambiguous free-space component cannot erase an area
            # already established by reliable passages and observed floor.
            established.update((tile, 4) for tile in self._area4_tiles if tile not in passages)
        for (tx, ty), room in established.items():
            for component in np.unique(labels[np.ix_(tiles == ty, tiles == tx)]):
                if component:
                    rooms[component].intersection_update((room,))
        different = []
        export_passage_links = defaultdict(list)
        for (tx, ty), pair in passages.items():
            if self._cs_commit_dist.get((tx, ty), float("inf")) > self.CS_COMMIT_GOOD_DIST_MM:
                continue
            ys, xs = np.flatnonzero(tiles == ty), np.flatnonzero(tiles == tx)
            if not ys.size or not xs.size:
                continue
            y0, y1, x0, x1 = ys[0], ys[-1]+1, xs[0], xs[-1]+1
            if min(x0, y0) == 0 or max(x1, y1) >= self.n:
                continue
            local, _ = scipy_ndimage.label(traversable[y0:y1, x0:x1])
            borders = defaultdict(set)
            for inside, outside in ((local[:, 0], labels[y0:y1, x0-1]),
                                    (local[:, -1], labels[y0:y1, x1]),
                                    (local[0], labels[y0-1, x0:x1]),
                                    (local[-1], labels[y1, x0:x1])):
                for component, neighbor in zip(inside, outside):
                    if component and neighbor:
                        borders[int(component)].add(int(neighbor))
            for neighbors in borders.values():
                for neighbor in neighbors:
                    rooms[neighbor].intersection_update(pair)
                if len(neighbors) == 2:
                    different.append(tuple(neighbors))
                    export_passage_links[(tx, ty)].append(tuple(neighbors))
        # A component can still contain multiple areas when its separator is
        # unseen or a native tile straddles connected floor regions. Conflicting
        # constraints make that component unknown; an empty domain must never
        # erase reliable evidence in every adjoining room.
        domains = [known.copy() for known in rooms]
        conflicted = {label for label in range(1, count+1) if not domains[label]}
        while True:
            rooms = [set((1, 2, 3, 4)) if label in conflicted else known.copy()
                     for label, known in enumerate(domains)]
            new_conflicts = set()
            changed = True
            while changed and not new_conflicts:
                changed = False
                for a, b in different:
                    if a in conflicted or b in conflicted:
                        continue
                    for left, right in ((a, b), (b, a)):
                        possible = {room for room in rooms[left] if rooms[right]-{room}}
                        if not possible:
                            new_conflicts.update((a, b))
                            break
                        if possible != rooms[left]:
                            rooms[left] = possible
                            changed = True
                    if new_conflicts:
                        break
            if not new_conflicts:
                break
            conflicted.update(new_conflicts)
        resolved = established
        for tx, ty in self.visited_tiles-passages.keys():
            inside_x = (tiles == tx) & (abs(coords-tx*self.TILE_MM) < self.QUARTER_MM)
            inside_y = (tiles == ty) & (abs(coords-ty*self.TILE_MM) < self.QUARTER_MM)
            components = set(np.unique(labels[np.ix_(inside_y, inside_x)]))-{0}
            certain = set().union(*(rooms[c] for c in components if len(rooms[c]) == 1))
            if len(certain) == 1:
                room = next(iter(certain))
                if all(room in rooms[c] for c in components):
                    resolved[(tx, ty)] = room
        # Export can identify a directly observed special floor before the
        # chassis enters it. Keep that evidence separate from geometry regions.
        # A long free component alone cannot prove an unseen doorway's side.
        export_positive = {tile for tile, token in self._committed_floor_tiles.items()
                           if token in ("3", "4")}
        export_nonpassage = ((self._nonpassage_floor_tiles | export_positive)
            - passages.keys() - {tile for tile, token in self._committed_floor_tiles.items()
                                 if token == "2"})
        def local_link(a, b, observed):
            xs = np.flatnonzero((tiles == a[0]) | (tiles == b[0]))
            ys = np.flatnonzero((tiles == a[1]) | (tiles == b[1]))
            if not xs.size or not ys.size:
                return False
            local, _ = scipy_ndimage.label(observed[np.ix_(ys, xs)])
            def interior(tile):
                return ((tiles[ys, None] == tile[1]) & (tiles[None, xs] == tile[0])
                    & (abs(coords[ys, None]-tile[1]*self.TILE_MM) < self.QUARTER_MM)
                    & (abs(coords[None, xs]-tile[0]*self.TILE_MM) < self.QUARTER_MM))
            return bool((set(np.unique(local[interior(a)]))-{0})
                        & (set(np.unique(local[interior(b)]))-{0}))
        self._competition_floor_area4 = set()
        for tile in export_positive-self.visited_tiles:
            tx, ty = tile
            inside_x = (tiles == tx) & (abs(coords-tx*self.TILE_MM) < self.QUARTER_MM)
            inside_y = (tiles == ty) & (abs(coords-ty*self.TILE_MM) < self.QUARTER_MM)
            components = set(np.unique(labels[np.ix_(inside_y, inside_x)]))-{0}
            if not components or any(rooms[c] != {4} for c in components):
                continue
            for neighbor in ((tx-1,ty), (tx+1,ty), (tx,ty-1), (tx,ty+1)):
                if (resolved.get(neighbor) == 4 and neighbor in export_nonpassage
                        and local_link(tile, neighbor, floor)):
                    self._competition_floor_area4.add(tile)
                    break
                pair = passages.get(neighbor, ())
                if (4 not in pair or self._cs_commit_dist.get(neighbor, float("inf"))
                        > self.CS_COMMIT_GOOD_DIST_MM):
                    continue
                other = next(room for room in pair if room != 4)
                side_resolved = any((a in components and rooms[b] == {other})
                    or (b in components and rooms[a] == {other})
                    for a, b in export_passage_links[neighbor])
                if side_resolved and local_link(tile, neighbor, traversable):
                    self._competition_floor_area4.add(tile)
                    break
        self._room_tiles = resolved
        self._room_model_signature = signature
        tile = self._room_entry_tile if self._room_last_tile in passages else self._room_last_tile
        self._current_room = resolved.get(tile)

    def _update_area4_tiles(self):
        """Extend known Area 4 through observed free space, stopping at passages.

        Robot visitation alone misses tiles occupied by an obstacle. LiDAR can
        establish their exposed floor. A component touching known areas other
        than 4 is ambiguous and must not expand the Area 4 mask.
        """
        self._competition_border_area4 = set()
        self._competition_border_context = None
        self._resolve_observed_rooms()
        if 4 not in self._room_tiles.values():
            self._area4_tiles = set()
            return
        coords = (np.arange(self.n)-self.c)*self.GRID_MM
        tiles = np.floor((coords+self.TILE_MM/2)/self.TILE_MM).astype(int)
        interior = abs(coords-tiles*self.TILE_MM) <= self.TILE_MM/2-self.GRID_MM
        free = self.logodds <= self.FREE_T
        passages = {tile for tile, token in self._committed_floor_tiles.items()
                    if token in self.CS_CONNECTION_CHARS}
        for tx, ty in passages:
            free &= ~((tiles[:, None] == ty) & (tiles[None, :] == tx))
        labels, count = scipy_ndimage.label(free)
        rooms = [set() for _ in range(count+1)]
        confirmed = set()
        for (tx, ty), room in self._room_tiles.items():
            if (tx, ty) in passages:
                continue
            if room == 4:
                confirmed.add((tx, ty))
            crop = labels[np.ix_((tiles == ty) & interior, (tiles == tx) & interior)]
            for label in np.unique(crop):
                if label:
                    rooms[label].add(room)
        area4 = [label for label, known in enumerate(rooms) if known == {4}]
        ys, xs = np.nonzero(np.isin(labels, area4) & interior[:, None] & interior[None, :])
        inferred = set(zip(map(int, tiles[xs]), map(int, tiles[ys])))
        # An unseen passage can still join a free component to another area.
        # Interpolate observed floor inside the extent established by visits;
        # never extrapolate that area's limits through an unclassified doorway.
        if confirmed:
            x0, y0 = np.min(list(confirmed), axis=0)
            x1, y1 = np.max(list(confirmed), axis=0)
            inferred = {(x, y) for x, y in inferred if x0 <= x <= x1 and y0 <= y <= y1}
        self._area4_tiles = confirmed | inferred
        self._competition_border_context = (labels, area4, rooms, tiles, self.logodds >= self.OCC_T, self.visited.copy())
        self._competition_border_area4 = None

    def build_report_matrix(self, show=None):
        """Rasterise the occupancy grid into the Erebus matrix.

        Each 120 mm tile is 4 matrix cells (30 mm each). Walls live on the
        60 mm sub-grid lines (tile edges at 0/120 mm and quarter mid-lines at
        60 mm). Every occupied grid cell is snapped to its nearest wall line
        and marked, so straight walls, half-walls and curved walls are all
        produced the same way the Erebus MapAnswer generator does.
        """
        previous_walls = ({(self._matrix_meta['x0_mm']+c*self.TILE_MM//4,
                            self._matrix_meta['y1_mm']-r*self.TILE_MM//4)
            for r, row in enumerate(self.report_matrix) for c, value in enumerate(row)
            if value == '1'} if self._matrix_meta else set())
        # Keep straight-wall lists for the JSON / debug payloads.
        self._update_area4_tiles()

        t, q = self.TILE_MM, self.QUARTER_MM
        cell = self.TILE_MM // 4                      # 30 mm matrix cell

        # Obstacles: keep their shape in the high-res map (for navigation) and
        # pull only their OFF-GRID body cells out of the wall population, so an
        # obstacle cannot stamp a spurious wall, yet every on-grid wall behind /
        # beside / in the corner of it still rasterises to '1'. Their tiles get
        # a whole-tile 'x' block (and any interior wall cleared) at the very end.
        self.obstacle_tiles = set()
        self._obstacle_mask = None
        if self.CAM_FUSE:
            # The camera is the obstacle authority: only surfaces it has
            # confirmed (colour deviates from the wall, multi-frame) become 'x'.
            # An obstacle NEVER carries a token/sign - those live only on walls -
            # and a sign sits on a flat, grid-aligned wall, not an off-grid tile
            # interior, so it can never reach this committed set.
            self.obstacle_tiles = set(self._cam_obstacle_tiles)
            self._obstacle_mask = self._obstacle_mask_from_tiles(self.obstacle_tiles)
        elif self.OBSTACLE_DETECT:
            self.obstacle_tiles, self._obstacle_mask = self._detect_obstacles()
        self._confirmed_obstacle_mask=self._obstacle_mask
        if self.OBSTACLE_DETECT:
            # Provisional non-wall material must not enter the confirmed-body
            # history used by the obstacle classifier on the next update.
            pending=self._pending_nonwall_mask()
            self._obstacle_mask=(pending if self._obstacle_mask is None
                                 else self._obstacle_mask | pending)
        self._v_walls, self._h_walls = self._detect_walls()

        ys, xs = np.where(self.logodds >= self.OCC_T)
        if self._obstacle_mask is not None and xs.size:
            keep = ~self._obstacle_mask[ys, xs]
            ys, xs = ys[keep], xs[keep]
        occ_wx = (xs - self.c) * self.GRID_MM
        occ_wy = (ys - self.c) * self.GRID_MM

        # Remove accumulated GPS drift by locking the wall population onto the
        # 60 mm sub-grid (matrix only; the high-res grid above is untouched).
        dx, dy = (0.0, 0.0)
        if self.MATRIX_GRID_LOCK:
            dx, dy = self._estimate_grid_offset(occ_wx, occ_wy)
            if dx or dy:
                occ_wx = occ_wx - dx
                occ_wy = occ_wy - dy
        self.last_grid_offset = (dx, dy)

        tiles = set(self.visited_tiles)
        tiles.add(self.start_tile)
        for ox, oy in zip(occ_wx, occ_wy):
            tiles.add((int(round(ox / t)), int(round(oy / t))))
        tiles |= self.obstacle_tiles                  # keep obstacle tiles in view
        for x0w, y0w, x1w, y1w in self._confirmed_straight_walls:
            tiles.add(self._tile_of(x0w, y0w))
            tiles.add(self._tile_of(x1w, y1w))
        tiles |= self._area4_tiles
        if self.FLOOR_FUSE and self._committed_floor_tiles:
            tiles |= set(self._committed_floor_tiles)  # keep coloured floor tiles in view

        min_tx = min(p[0] for p in tiles)
        max_tx = max(p[0] for p in tiles)
        min_ty = min(p[1] for p in tiles)
        max_ty = max(p[1] for p in tiles)

        rows = 4 * (max_ty - min_ty + 1) + 1
        cols = 4 * (max_tx - min_tx + 1) + 1
        matrix = [["0"] * cols for _ in range(rows)]

        x0 = t * min_tx - q                           # world x at column 0
        y1 = t * max_ty + q                           # world y at row 0 (North)

        # remember the matrix<->world mapping for offline raw-data debugging
        self._matrix_meta = {
            "x0_mm": x0, "y1_mm": y1, "cell_mm": cell,
            "min_tx": min_tx, "max_tx": max_tx,
            "min_ty": min_ty, "max_ty": max_ty,
            "rows": rows, "cols": cols,
        }

        counts = defaultdict(int)
        for ox, oy in zip(occ_wx, occ_wy):
            xv = q * round(ox / q)
            yh = q * round(oy / q)
            dv = abs(ox - xv)
            dh = abs(oy - yh)
            if dv <= dh:
                if dv > self.MATRIX_SNAP_MM:
                    continue
                c = int(round((xv - x0) / cell))
                r = int(round((y1 - oy) / cell))
            else:
                if dh > self.MATRIX_SNAP_MM:
                    continue
                r = int(round((y1 - yh) / cell))
                c = int(round((ox - x0) / cell))
            if 0 <= r < rows and 0 <= c < cols:
                counts[(r, c)] += 1

        # Candidate wall cells, then a denoise pass. Real walls/curves are large
        # connected structures; small disconnected blobs are lidar speckle. Drop
        # any 8-connected component smaller than MATRIX_MIN_COMPONENT unless it
        # carries overwhelming evidence (a strongly-hit cell).
        # Losing a second distinct voxel is not evidence that the remaining
        # measured wall vanished. Keep an already accepted cell while it has
        # occupied support; reliable free observations must remove that support
        # before the cell can disappear. Later curve/object passes still apply.
        retained = {rc for rc, n in counts.items() if n > 0
                    and (x0+rc[1]*cell, y1-rc[0]*cell) in previous_walls}
        cand = {rc for rc, n in counts.items() if n >= self.MATRIX_CELL_MIN_HITS
                or rc in retained}
        seen = set()
        for seed in cand:
            if seed in seen:
                continue
            stack, comp = [seed], []
            seen.add(seed)
            while stack:
                r, c = stack.pop()
                comp.append((r, c))
                # 8-connected so diagonal curve staircases stay one component
                for nb in ((r - 1, c), (r + 1, c), (r, c - 1), (r, c + 1),
                           (r - 1, c - 1), (r - 1, c + 1),
                           (r + 1, c - 1), (r + 1, c + 1)):
                    if nb in cand and nb not in seen:
                        seen.add(nb)
                        stack.append(nb)
            strong = any(rc in retained or counts[rc] >= self.MATRIX_CELL_STRONG_HITS
                         or self._has_direct_wall_support(*rc, x0, y1, cell)
                         for rc in comp)
            if len(comp) >= self.MATRIX_MIN_COMPONENT or strong:
                for (r, c) in comp:
                    if 0 <= r < rows and 0 <= c < cols:
                        matrix[r][c] = "1"

        sr = 4 * (max_ty - self.start_tile[1])        # start-tile quarter centres
        sc = 4 * (self.start_tile[0] - min_tx)

        def _stage(name):
            if self._record_stages:
                self._stage_snapshots[name] = [row[:] for row in matrix]
        self._stage_snapshots = {}
        _stage("raster")

        if self.MATRIX_DENOISE:
            self._denoise_matrix(matrix, rows, cols, x0, y1, cell, retained)
            _stage("denoise")

        if self.MATRIX_SNAP_GRID:
            self._snap_walls_to_grid(matrix, rows, cols, x0, y1, cell, retained)
            _stage("gridsnap")

        if self.MATRIX_FIX_CURVES:
            self._apply_curve_vertices(matrix, rows, cols, x0, y1, cell)
            _stage("curve")

        # Complete internal walls the matrix dropped but the high-res clearly
        # shows (high-res backed, so only REAL walls) - every frame, so 'F' is
        # needed only for the never-scanned outer border.
        if self.MATRIX_AUTO_RESCUE:
            self._rescue_dropped_walls(matrix, rows, cols, x0, y1, cell)
            _stage("auto_rescue")

        # Border / wall closure - ONLY when the user has toggled it with 'F'.
        if self.closure_enabled:
            self._apply_closure(matrix, rows, cols, x0, y1, cell)
            _stage("closure")

        # Hard Erebus invariant: walls only ever live on the 60 mm edge/vertex
        # lines (at least one even index). An (odd, odd) cell is a tile
        # quarter-cell interior that can only hold a tile type (0/5/victim),
        # never a wall - so clear any wall that landed there (raster noise or an
        # over-eager closure rescue).
        for r in range(1, rows, 2):
            for c in range(1, cols, 2):
                if matrix[r][c] == "1":
                    matrix[r][c] = "0"
        _stage("odd_guard")

        # Final template cleanup on the settled wall layout: strip any remaining
        # double-wall smear / short fragment now that every earlier pass (grid
        # snap, curves, closure, odd/isolated guards) has run. This is the state
        # validated to remove exactly the invalid cells and keep every real
        # straight (>=3) and curve. Matrix only; the high-res map stays raw.
        if self.MATRIX_DENOISE:
            self._denoise_matrix(matrix, rows, cols, x0, y1, cell, retained)
            _stage("final_denoise")


        # Coloured / special FLOOR tiles (side-camera colour fusion). Stamp the
        # committed char on the tile's 4 interior quarter cells - the same cells
        # MapAnswer uses for a tile type. Done before start '5' and obstacle 'x'
        # so those win if they ever share a tile (they should not).
        if self.FLOOR_FUSE:
            for (tx, ty), ch in self.mapped_floor_tiles().items():
                br = 4 * (max_ty - ty)
                bc = 4 * (tx - min_tx)
                for dr in (1, 3):
                    for dc in (1, 3):
                        rr, cc = br + dr, bc + dc
                        if 0 <= rr < rows and 0 <= cc < cols:
                            matrix[rr][cc] = ch

        for dr in (1, 3):
            for dc in (1, 3):
                if 0 <= sr + dr < rows and 0 <= sc + dc < cols:
                    matrix[sr + dr][sc + dc] = "5"

        # Obstacles: mark the whole 120 mm tile with 'x' (its 4 interior quarter
        # cells), matching MapAnswer. Done last so 'x' wins over a tile type.
        for (tx, ty) in self.obstacle_tiles:
            br = 4 * (max_ty - ty)
            bc = 4 * (tx - min_tx)
            # Only the detected body was excluded before wall rasterisation.
            # An obstacle and a half-wall can share a 120 mm tile; keep the wall.
            for dr in (1, 3):
                for dc in (1, 3):
                    if 0 <= br + dr < rows and 0 <= bc + dc < cols:
                        matrix[br + dr][bc + dc] = "x"

        # Wall-template isolated-cell guard (matrix only) - run LAST, on the fully
        # stamped layout. Every real Erebus wall is a >=3-cell straight or a curve,
        # so a wall cell always touches another wall cell in one of the 8 directions
        # (orthogonally for straights, diagonally at a curve bend). A '1' with NO
        # '1' neighbour cannot belong to any valid template - it is raster speckle,
        # an obstacle body grazing a tile edge, or a boundary cell orphaned when the
        # obstacle interior beneath it was cleared. Drop only those. 8-connectivity
        # keeps curve staircases. Removing an isolated cell can never orphan another,
        # so one pass suffices. The high-res map keeps the raw shape for planning.
        if self.MATRIX_DROP_ISOLATED_WALLS:
            eight = [(dr, dc) for dr in (-1, 0, 1) for dc in (-1, 0, 1)
                     if (dr, dc) != (0, 0)]
            drop = []
            for r in range(rows):
                for c in range(cols):
                    if matrix[r][c] != "1":
                        continue
                    if not any(0 <= r + dr < rows and 0 <= c + dc < cols
                               and matrix[r + dr][c + dc] == "1"
                               for dr, dc in eight):
                        if ((r, c) not in retained
                                and not self._has_direct_wall_support(r, c, x0, y1, cell)):
                            drop.append((r, c))
            for (r, c) in drop:
                matrix[r][c] = "0"
            _stage("wall_template")

        self._apply_straight_wall_models(matrix, x0, y1, cell)
        _stage("confirmed_straight_walls")
        if self.MATRIX_FIX_CURVES:
            self._remove_curve_raster_aliases(matrix, x0, y1, cell)
            _stage("curve_surface_template")

        # Keep all physical geometry internally. Only the submission copy masks
        # Area 4; navigation and internal debugging use this unmasked matrix.
        self.report_matrix = matrix
        if self._record_stages:
            self._stage_snapshots["competition_export"] = self.competition_matrix()

        if self.DEBUG_MAPPING_DUMP:
            self._update_debug_snapshot()

        show_rep = self.SHOW_MATRIX_MAP if show is None else show
        if show_rep:
            self.draw_competition_matrix_map()

        return matrix

    def _remove_curve_raster_aliases(self, matrix, x0, y1, cell):
        """A measured quarter wall does not prove its opposite edge midpoints.

        Rounding a curved return to10mm, then independently to60mm lines and
        30mm cells, can create a branch at a shared endpoint. Keep such a branch
        only when an independent axis plane, straight model or another curve
        supports it. Physical occupancy and navigation material are untouched.
        """
        self.last_curve_raster_debug=[]
        if not self._native_curve_samples:
            return
        models={model for model in self._confirmed_curve_quadrants
                if model[:2] in self._persistent_curve_bends
                and self._scan_curve_votes.get(model,0)>=self.ARC_SOFT_HITS}
        arms={(bx*60+sx*step,by*60) for bx,by,sx,sy in self._confirmed_curve_quadrants for step in (30,60)}
        arms|={(bx*60,by*60+sy*step) for bx,by,sx,sy in self._confirmed_curve_quadrants for step in (30,60)}
        dx,dy=self.last_grid_offset
        for bx,by,sx,sy in sorted(models):
            if self._tile_of(bx*60+sx*30,by*60+sy*30) in self._area4_tiles:
                continue
            candidates=[(bx*60+sx*60,by*60+sy*30,self._vertical_surface_hits),
                        (bx*60+sx*30,by*60+sy*60,self._horizontal_surface_hits)]
            for wx,wy,signature in candidates:
                r,c=round((y1-wy)/cell),round((wx-x0)/cell)
                if not (0<=r<len(matrix) and 0<=c<len(matrix[r]) and matrix[r][c]=='1'):
                    continue
                gx=round((wx+dx)/self.GRID_MM)+self.c
                gy=round((wy+dy)/self.GRID_MM)+self.c
                measured_axis=(0<=gx<self.n and 0<=gy<self.n and
                    np.any(signature[max(0,gy-1):gy+2,max(0,gx-1):gx+2]>0))
                known_straight=any((ax==ex==wx and min(ay,ey)<=wy<=max(ay,ey))
                    or (ay==ey==wy and min(ax,ex)<=wx<=max(ax,ex))
                    for ax,ay,ex,ey in self._confirmed_straight_walls)
                supported=(wx,wy) in arms or measured_axis or known_straight
                if not supported:
                    matrix[r][c]='0'
                if self.DEBUG_MAPPING_DUMP:
                    self.last_curve_raster_debug.append({'curve':list((bx,by,sx,sy)),
                        'cell_mm':[wx,wy],'axis_surface_support':bool(measured_axis),
                        'straight_model_support':known_straight,'other_curve_support':(wx,wy) in arms,
                        'action':'retain' if supported else 'remove_raster_alias'})

    def _apply_straight_wall_models(self, matrix, x0, y1, cell):
        """Keep a measured 60 mm wall through a later inconclusive view.

        Interior longitudinal bins require raw occupancy. An exact endpoint of
        an independently confirmed straight may support a shared end bin that
        its physical cap and GPS/grid phase leave unobserved. Unknown readings
        cannot revoke it; an observed
        free strip or a newly confirmed object can. Area 4 keeps raw geometry.
        """
        occ = self.logodds >= self.OCC_T
        if self._obstacle_mask is not None:
            occ &= ~self._obstacle_mask
        scale = self.QUARTER_MM//self.GRID_MM
        anchors = set()
        def supported(wall):
            ax, ay, bx, by = wall
            if self._tile_of((ax+bx)/2, (ay+by)/2) in self._area4_tiles:
                return False
            gx, gy = round(ax/self.GRID_MM)+self.c, round(ay/self.GRID_MM)+self.c
            view = (np.s_[gy:gy+scale+1, gx-1:gx+2] if ax == bx
                    else np.s_[gy-1:gy+2, gx:gx+scale+1])
            return not (np.all(self.logodds[view] <= self.FREE_T)
                or (self._confirmed_obstacle_mask is not None
                    and self._confirmed_obstacle_mask[view].any()))
        # Snapshot only existing, still-supported straight ENDPOINTS. Neither
        # a curve bend nor a new candidate in this pass can bootstrap a wall.
        for wall in self._confirmed_straight_walls:
            if supported(wall):
                anchors.update((wall[:2], wall[2:]))
        xs = np.arange(round(x0/self.GRID_MM)+self.c,
            round((x0+(len(matrix[0])-1)*cell)/self.GRID_MM)+self.c+1, scale)
        ys = np.arange(round((y1-(len(matrix)-1)*cell)/self.GRID_MM)+self.c,
            round(y1/self.GRID_MM)+self.c+1, scale)
        xs, ys = xs[(xs > 0) & (xs < self.n-1)], ys[(ys > 0) & (ys < self.n-1)]
        for transpose, across, along in ((False, xs, ys), (True, ys, xs)):
            grid = occ.T if transpose else occ
            signature = (self._horizontal_surface_hits.T if transpose else self._vertical_surface_hits) > 0
            if self._obstacle_mask is not None:
                signature &= ~(self._obstacle_mask.T if transpose else self._obstacle_mask)
            by_line = defaultdict(set)
            for ax, ay in anchors:
                gx, gy = round(ax/self.GRID_MM)+self.c, round(ay/self.GRID_MM)+self.c
                line, position = (gy, gx) if transpose else (gx, gy)
                by_line[line].add(position)
            starts = along[along+scale < self.n-1]
            samples = starts[:, None]+np.arange(scale+1)
            for line in across:
                profile = grid[:, line-1:line+2].any(axis=1)
                normals = signature[:, line-1:line+2].any(axis=1)
                support = profile[samples]
                for endpoint in by_line.get(line, ()):
                    # Preserve the body-role veto even at a shared model end.
                    masked = self._obstacle_mask.T if transpose and self._obstacle_mask is not None else self._obstacle_mask
                    if masked is not None and masked[endpoint, line-1:line+2].any():
                        continue
                    support[:, 0] |= starts == endpoint
                    support[:, -1] |= starts+scale == endpoint
                measured = np.all(support, axis=1) & np.all(normals[samples[:, 2:5]], axis=1)
                for begin in starts[measured]:
                    a, b = ((line-self.c)*self.GRID_MM, (begin-self.c)*self.GRID_MM)
                    wall = ((b, a, b+self.QUARTER_MM, a) if transpose
                            else (a, b, a, b+self.QUARTER_MM))
                    self._confirmed_straight_walls.add(tuple(map(int,wall)))
        keep = set()
        for wall in self._confirmed_straight_walls:
            if not supported(wall):
                continue
            ax, ay, bx, by = wall
            keep.add(wall)
            for x, y in zip(np.linspace(ax, bx, 3), np.linspace(ay, by, 3)):
                r, c = round((y1-y)/cell), round((x-x0)/cell)
                if 0 <= r < len(matrix) and 0 <= c < len(matrix[r]):
                    matrix[r][c] = '1'
        self._confirmed_straight_walls = keep

    def _has_direct_wall_support(self, r, c, x0, y1, cell):
        """A partial straight wall can be real before its full template is seen."""
        if r % 2 and c % 2:
            return False
        dx, dy = self.last_grid_offset
        window = self._cell_grid_window(r, c, x0+dx, y1+dy, cell, 2)
        if window is None or window.shape != (5, 5) or window[2, 2] < self.OCC_T:
            return False
        occupied = window >= self.OCC_T
        horizontal = int(np.count_nonzero(occupied[1:4, :].any(axis=0)))
        vertical = int(np.count_nonzero(occupied[:, 1:4].any(axis=1)))
        along, across = (horizontal, vertical) if r % 2 == 0 else (vertical, horizontal)
        if r % 2 == c % 2 == 0:
            along, across = max(horizontal, vertical), min(horizontal, vertical)
        # A grazing partial wall can occupy three rows and three columns after
        # 10 mm quantisation. Keep that supported tie; two-cell blobs stay weak.
        return along >= 2 and (along > across or along == across >= 3)

    def _straight_wall_mask(self, models=None):
        """Cache the exact original model raster; batch integer axis segments."""
        models = frozenset(self._confirmed_straight_walls if models is None else models)
        key = models, self.GRID_MM, self.logodds.shape, self.c
        cached = getattr(self, '_straight_wall_mask_cache', None)
        if cached is not None and cached[0] == key:
            return cached[1]
        mask = np.zeros_like(self.logodds, dtype=bool)
        walls = np.asarray(list(models), float).reshape(-1, 4)
        scaled = walls/self.GRID_MM
        integral = np.rint(scaled)
        if np.array_equal(scaled, integral) and np.all((walls[:,0] == walls[:,2]) | (walls[:,1] == walls[:,3])):
            cells = integral.astype(np.int64)
            delta = cells[:,2:]-cells[:,:2]
            counts = np.max(abs(delta), axis=1)+1
            labels = np.repeat(np.arange(len(cells)), counts)
            local = np.arange(int(counts.sum()))-np.repeat(np.cumsum(np.r_[0, counts])[:-1], counts)
            xy = cells[labels,:2]+local[:,None]*np.sign(delta[labels])
            mask[xy[:,1]+self.c, xy[:,0]+self.c] = True
        else:
            for ax, ay, bx, by in models:
                steps = round(math.hypot(bx-ax, by-ay)/self.GRID_MM)
                x = np.rint(np.linspace(ax,bx,steps+1)/self.GRID_MM).astype(int)+self.c
                y = np.rint(np.linspace(ay,by,steps+1)/self.GRID_MM).astype(int)+self.c
                mask[y,x] = True
        mask.flags.writeable = False
        self._straight_wall_mask_cache = key, mask
        return mask

    def occupied_geometry(self):
        """Measured occupancy plus confirmed straight-wall material."""
        return (self.logodds >= self.OCC_T) | self._straight_wall_mask()

    def _physical_subdivide(self, mask, grid):
        """Exactly tile the original occupied squares; never shrink a pit/wall."""
        y, x = np.nonzero(mask)
        centres = np.column_stack(((x-self.c)*self.GRID_MM, (y-self.c)*self.GRID_MM))
        offset = np.arange(-self.GRID_MM/2+grid/2, self.GRID_MM/2, grid)
        ox, oy = np.meshgrid(offset, offset)
        sub = np.column_stack((ox.ravel(), oy.ravel()))
        return (centres[:, None, :]+sub).reshape(-1, 2)

    def physical_material(self):
        """Return measured material centres and their square width in mm."""
        evidence = self._physical_evidence
        if evidence is None:
            y, x = np.nonzero(self.occupied_geometry())
            return np.column_stack(((x-self.c)*self.GRID_MM, (y-self.c)*self.GRID_MM)), self.GRID_MM
        self._physical_coarse_guard = {p for p in self._physical_coarse_guard
                                      if self.logodds[p] > self.FREE_T}
        cache_key = (id(evidence), evidence.version, frozenset(self._physical_coarse_guard),
                     frozenset(self._confirmed_straight_walls))
        if evidence.precise and getattr(self, '_physical_material_cache_key', None) == cache_key:
            return self._physical_material_cache
        fine = evidence.occupied_centres()
        coarse = np.zeros_like(self.logodds, dtype=bool)
        for gy, gx in self._physical_coarse_guard:
            coarse[gy,gx] = True
        if evidence.precise:
            inferred = evidence.template_material(self)
        else:
            coarse |= self._straight_wall_mask()
            inferred = np.empty((0,2))
        points = np.vstack([fine, self._physical_subdivide(coarse,evidence.GRID_MM), inferred])
        if evidence.precise:
            points.flags.writeable = False
            self._physical_material_cache = (points,evidence.GRID_MM)
            self._physical_material_cache_key = cache_key
        return points,evidence.GRID_MM

    def _denoise_matrix(self, matrix, rows, cols, x0, y1, cell, retained=()):
        """Remove the two recurring matrix artefacts.

        1. Double-wall *smear*: a real Erebus wall sits on a single 60 mm line,
           so two wall cells can never be 30 mm (one matrix cell) apart. Any
           cell sitting directly beside a solid (>= 3 long) wall that does not
           itself continue away from that wall is smear -> drop it.
        2. Stray *fragments*: tiny isolated blobs (8-connected component of
           fewer than 3 cells) are lidar speckle -> drop them. Connectivity is
           used (not run length) so a curve's thin diagonal cells are kept.
        """
        def wall(r, c):
            return 0 <= r < rows and 0 <= c < cols and matrix[r][c] == "1"

        SOLID = 3

        # Phase A - raw smear candidates from the ORIGINAL matrix. A cell beside a
        # solid wall that does not itself continue away from it is smear.
        raw_v = set()   # horizontal smear stuck to a solid VERTICAL wall
        for c in range(cols):
            r = 0
            while r < rows:
                if wall(r, c):
                    r2 = r
                    while r2 < rows and wall(r2, c):
                        r2 += 1
                    if r2 - r >= SOLID:
                        for rr in range(r, r2):
                            for dc in (-1, 1):
                                ca = c + dc
                                if wall(rr, ca) and not wall(rr, ca + dc):
                                    raw_v.add((rr, ca))
                    r = r2
                else:
                    r += 1
        raw_h = set()   # vertical smear stuck to a solid HORIZONTAL wall
        for r in range(rows):
            c = 0
            while c < cols:
                if wall(r, c):
                    c2 = c
                    while c2 < cols and wall(r, c2):
                        c2 += 1
                    if c2 - c >= SOLID:
                        for cc in range(c, c2):
                            for dr in (-1, 1):
                                ra = r + dr
                                if wall(ra, cc) and not wall(ra + dr, cc):
                                    raw_h.add((ra, cc))
                    c = c2
                else:
                    c += 1

        # Phase B - a smear cell must never itself count as part of a solid wall
        # that authorises deleting the perpendicular wall it is stuck to. When
        # scanning for solid horizontal walls we ignore cells that are vertical
        # walls' horizontal smear (raw_v), and vice-versa. This stops a couple of
        # speckle cells beside a real vertical wall from masquerading as a solid
        # horizontal wall and deleting that wall's end cell.
        def wall_h(r, c):          # wall for horizontal-run detection
            return wall(r, c) and (r, c) not in raw_v

        def wall_v(r, c):          # wall for vertical-run detection
            return wall(r, c) and (r, c) not in raw_h

        # A candidate about to be cleared as smear is actually the END CELL of a
        # real perpendicular wall arm when it belongs to a short run (SOLID..
        # ARM_MAX) of genuine (non-opposite-smear) walls running perpendicular to
        # the source wall. Protecting those keeps L/T-junction and curve arms that
        # sit one cell off a crossing wall. The upper cap means a long straight
        # border spike (whose "run" is the whole perimeter) is still treated as
        # smear and removed, so this can never enlarge the arena outline.
        ARM_MAX = 4

        def hrun_real(r, c):       # candidate + non-raw_v walls, horizontally
            n = 1
            cc = c - 1
            while wall(r, cc) and (r, cc) not in raw_v:
                n += 1; cc -= 1
            cc = c + 1
            while wall(r, cc) and (r, cc) not in raw_v:
                n += 1; cc += 1
            return n

        def vrun_real(r, c):       # candidate + non-raw_h walls, vertically
            n = 1
            rr = r - 1
            while wall(rr, c) and (rr, c) not in raw_h:
                n += 1; rr -= 1
            rr = r + 1
            while wall(rr, c) and (rr, c) not in raw_h:
                n += 1; rr += 1
            return n

        clear = set()
        # solid horizontal walls -> clear vertical smear above/below
        for r in range(rows):
            c = 0
            while c < cols:
                if wall_h(r, c):
                    c2 = c
                    while c2 < cols and wall_h(r, c2):
                        c2 += 1
                    if c2 - c >= SOLID:
                        for cc in range(c, c2):
                            for dr in (-1, 1):
                                ra = r + dr
                                if wall(ra, cc) and not wall(ra + dr, cc):
                                    far = ra + dr
                                    if wall(far, cc - 1) or wall(far, cc + 1):
                                        continue  # diagonal curve/corner connector
                                    if SOLID <= vrun_real(ra, cc) <= ARM_MAX:
                                        continue      # real perpendicular arm
                                    clear.add((ra, cc))
                    c = c2
                else:
                    c += 1
        # solid vertical walls -> clear horizontal smear left/right
        for c in range(cols):
            r = 0
            while r < rows:
                if wall_v(r, c):
                    r2 = r
                    while r2 < rows and wall_v(r2, c):
                        r2 += 1
                    if r2 - r >= SOLID:
                        for rr in range(r, r2):
                            for dc in (-1, 1):
                                ca = c + dc
                                if wall(rr, ca) and not wall(rr, ca + dc):
                                    far = ca + dc
                                    if wall(rr - 1, far) or wall(rr + 1, far):
                                        continue  # diagonal curve/corner connector
                                    if SOLID <= hrun_real(rr, ca) <= ARM_MAX:
                                        continue      # real perpendicular arm
                                    clear.add((rr, ca))
                    r = r2
                else:
                    r += 1
        for (r, c) in clear:
            if ((r, c) not in retained
                    and not self._has_direct_wall_support(r, c, x0, y1, cell)):
                matrix[r][c] = "0"

        # drop only *isolated* speckle left after the smear pass. We use
        # 8-connected component size (not run length) so a curve's thin
        # diagonal cells survive as long as they hang together with the rest
        # of the curve - only genuinely tiny blobs are removed.
        walls = {(r, c) for r in range(rows) for c in range(cols)
                 if matrix[r][c] == "1"}
        seen = set()
        for s in walls:
            if s in seen:
                continue
            stack, comp = [s], []
            seen.add(s)
            while stack:
                r, c = stack.pop()
                comp.append((r, c))
                for nb in ((r - 1, c), (r + 1, c), (r, c - 1), (r, c + 1),
                           (r - 1, c - 1), (r - 1, c + 1),
                           (r + 1, c - 1), (r + 1, c + 1)):
                    if nb in walls and nb not in seen:
                        seen.add(nb)
                        stack.append(nb)
            if len(comp) < 3:
                for (r, c) in comp:
                    if ((r, c) not in retained
                            and not self._has_direct_wall_support(r, c, x0, y1, cell)):
                        matrix[r][c] = "0"

    def _snap_walls_to_grid(self, matrix, rows, cols, x0, y1, cell, retained=()):
        """Snap walls to the 60 mm grid by deleting perpendicular thickness bleed.

        A physical wall is ~1.5 matrix cells thick in the high-res grid, so its
        occupancy bleeds one cell to the side. When that bleed happens to sit
        near a perpendicular 60 mm line the point-snap raster marks a spurious
        EDGE cell there - a stub sticking out of the real wall (e.g. a lone
        horizontal cell hanging off a vertical wall). In every Erebus map
        (MapAnswer.generateAnswer) a wall is stamped as a full tile-edge segment,
        never a single perpendicular stub, so these are always noise.

        An edge cell lies on one 60 mm line; a real wall there runs ALONG that
        line, a bleed stub's occupancy runs ACROSS it (it is really the flank of
        the perpendicular wall). Using the live high-res occupancy we measure how
        far the occupancy extends along vs across the cell's own line and drop the
        cell when it is dominated by the perpendicular direction, or when it has
        almost no occupancy along its own line and is not part of a solid matrix
        run (a barely-there parallel-bleed cell). Vertices (even,even) are left to
        the curve pass; only edge cells are snapped. Matrix-structural runs of
        length >= SOLID are always kept, so real walls can never be thinned away.
        """
        dx, dy = self.last_grid_offset
        occ = self.logodds >= self.OCC_T
        if self._obstacle_mask is not None:
            occ &= ~self._obstacle_mask
        W = self.SNAP_WIN_CELLS
        n = self.n

        def extents(r, c):
            wx = x0 + c * cell + dx
            wy = y1 - r * cell + dy
            gx = int(round(wx / self.GRID_MM)) + self.c
            gy = int(round(wy / self.GRID_MM)) + self.c
            if not (W <= gx < n - W and W <= gy < n - W):
                return None
            hstrip = occ[gy - 1:gy + 2, gx - W:gx + W + 1]
            vstrip = occ[gy - W:gy + W + 1, gx - 1:gx + 2]
            h_ext = int(np.count_nonzero(hstrip.any(axis=0)))
            v_ext = int(np.count_nonzero(vstrip.any(axis=1)))
            return h_ext, v_ext

        def wall(r, c):
            return 0 <= r < rows and 0 <= c < cols and matrix[r][c] == "1"

        def hrun(r, c):
            n_ = 1; cc = c - 1
            while wall(r, cc): n_ += 1; cc -= 1
            cc = c + 1
            while wall(r, cc): n_ += 1; cc += 1
            return n_

        def vrun(r, c):
            n_ = 1; rr = r - 1
            while wall(rr, c): n_ += 1; rr -= 1
            rr = r + 1
            while wall(rr, c): n_ += 1; rr += 1
            return n_

        perp = self.SNAP_PERP_MIN
        weak = self.SNAP_WEAK_MIN
        drop = []
        for r in range(rows):
            for c in range(cols):
                if matrix[r][c] != "1" or (r, c) in retained:
                    continue
                h_edge = (r % 2 == 0 and c % 2 == 1)
                v_edge = (r % 2 == 1 and c % 2 == 0)
                if not (h_edge or v_edge):
                    continue                       # vertices handled by curve pass
                ex = extents(r, c)
                if ex is None:
                    continue
                h_ext, v_ext = ex
                if h_edge:
                    along, across, run = h_ext, v_ext, hrun(r, c)
                else:
                    along, across, run = v_ext, h_ext, vrun(r, c)
                if run >= self.MATRIX_MIN_COMPONENT:
                    continue                       # part of a solid wall run -> keep
                if along >= 3 and along > across:
                    continue                       # measured partial wall, not cross-line bleed
                if across - along >= perp:
                    drop.append((r, c))            # occupancy runs across the line
                elif along < weak:
                    drop.append((r, c))            # almost nothing along its own line
        for r, c in drop:
            matrix[r][c] = "0"

    def _apply_curve_vertices(self, matrix, rows, cols, x0, y1, cell):
        """Detect supported curve bends in the internal structured map.

        Empty bends remain registered for navigation. Occupied endpoints and
        measured straight walls from other features survive internally;
        competition_matrix applies scoring conventions to a separate copy.

        Row 0 is North (y1); world y at matrix row r is y1 - r*cell.
        """
        # Unknown or occluded cells do not contradict a measured curve. A
        # complete seen-free quadrant does: release its model and scan history
        # so later raw observations can reopen a genuinely clear passage.
        cleared=set();scale=self.QUARTER_MM//self.GRID_MM
        for model in self._confirmed_curve_quadrants | set(self._scan_curve_votes):
            bx,by,sx,sy=model;gx,gy=bx*scale+self.c,by*scale+self.c
            xa,xb=sorted((gx,gx+sx*scale));ya,yb=sorted((gy,gy+sy*scale))
            if (0<=xa<=xb<self.n and 0<=ya<=yb<self.n
                    and np.all(self.logodds[ya:yb+1,xa:xb+1]<=self.FREE_T)):
                self._confirmed_curve_quadrants.discard(model)
                self._scan_curve_votes.pop(model,None);cleared.add((bx,by))
        for vertex in cleared:
            if not any(m[:2]==vertex for m in self._confirmed_curve_quadrants):
                self._persistent_curve_bends.discard(vertex)
        rad = 1
        # Remember every vertex we demote to a curve bend so later passes
        # (closure) can never resurrect it back into a sharp '1'.
        self._curve_bends = set()
        # Raw curvature plus both endpoints and a measured wall connection
        # establishes the model before lossy rasterisation. A dropped coarse
        # arm must not prevent that independent proof from being consulted.
        dx, dy = self.last_grid_offset
        raw_models = []
        for (bx, by, sx, sy), count in list(self._scan_curve_votes.items()):
            if count < self.ARC_SOFT_HITS:
                continue
            tile = self._tile_of((bx+sx/2)*self.QUARTER_MM,
                                 (by+sy/2)*self.QUARTER_MM)
            if tile in self._area4_tiles:
                continue  # free-form Area4 retains measured geometry
            wx, wy = bx*self.QUARTER_MM-dx, by*self.QUARTER_MM-dy
            if self._arc_evidence(wx, wy, sy>0, sy<0, sx<0, sx>0,
                                  confirm=True) < self.ARC_SOFT_HITS:
                continue
            raw_models.append((bx, by, sx, sy, wx, wy))
        # The current observed wall extent is a conservative guard for the
        # coarse corner classifier. A crop edge alone does not establish the
        # true arena boundary or prove that a geometric feature is sharp.
        min_r = min_c = 10 ** 9
        max_r = max_c = -1
        for rr in range(rows):
            for cc in range(cols):
                if matrix[rr][cc] == "1":
                    if rr < min_r: min_r = rr
                    if rr > max_r: max_r = rr
                    if cc < min_c: min_c = cc
                    if cc > max_c: max_c = cc
        for r in range(0, rows, 2):
            for c in range(0, cols, 2):
                wx = x0 + c * cell
                wy = y1 - r * cell
                key = self._vertex_key(wx, wy)
                up = r > 0 and matrix[r - 1][c] == "1"
                dn = r < rows - 1 and matrix[r + 1][c] == "1"
                lf = c > 0 and matrix[r][c - 1] == "1"
                rt = c < cols - 1 and matrix[r][c + 1] == "1"
                if (matrix[r][c] == "0" and (up or dn) and (lf or rt)
                        and self._arc_evidence(wx, wy, up, dn, lf, rt, confirm=True) >= self.ARC_SOFT_HITS):
                    # Rasterisation can already leave the bend empty. Still
                    # register the observed curve for navigation's arc handling.
                    self._curve_bends.add((r, c))
                    self._persistent_curve_bends.add(key)
                    continue

                # Retain a supported curve across viewpoints, but allow later
                # evidence to disprove a bend assigned to a neighbouring arc.
                if key in self._persistent_curve_bends:
                    # Historical arc votes must not permanently erase a sharp
                    # junction once live occupancy disproves that old shape.
                    dx, dy = self.last_grid_offset
                    window = self._cell_grid_window(r, c, x0+dx, y1+dy, cell, 5)
                    contradicted = False
                    if window is not None and window.shape == (11, 11):
                        yy, xx = np.indices(window.shape)
                        off_axis = (abs(xx-5) >= 1) & (abs(yy-5) >= 1)
                        contradicted = (window[5, 5] >= self.OCC_T
                                        and not np.any((window >= self.OCC_T) & off_axis))
                        # A neighbouring arc can have generated this historical
                        # label. Its shape must fit THIS bend, not just be nearby.
                        if window[5, 5] > self.FREE_T:
                            contradicted |= self._arc_evidence(wx, wy, True, True, True, True, confirm=True) == 0
                    if contradicted:
                        self._persistent_curve_bends.discard(key)
                        self._confirmed_curve_quadrants = {m for m in self._confirmed_curve_quadrants if m[:2] != key}
                        self._scan_curve_votes = {m:v for m,v in self._scan_curve_votes.items() if m[:2] != key}
                    else:
                        matrix[r][c] = "0"
                        self._curve_bends.add((r, c))
                        continue

                if matrix[r][c] != "1":
                    continue
                if r in (min_r, max_r) or c in (min_c, max_c):
                    continue                     # unresolved corner on current wall extent
                has_v = up or dn
                has_h = lf or rt
                if not (has_v and has_h):
                    straight_run = (up and dn) or (lf and rt)
                    if self.MATRIX_ARC_CURVES and straight_run:
                        gx = int(round(wx / self.GRID_MM)) + self.c
                        gy = int(round(wy / self.GRID_MM)) + self.c
                        center_free = (0 <= gx < self.n and 0 <= gy < self.n
                                       and int(self.logodds[gy, gx]) <= self.FREE_T)
                        arc_ev = self._arc_evidence(wx, wy, up, dn, lf, rt, confirm=True)
                        if center_free and arc_ev >= self.ARC_SOFT_HITS:
                            matrix[r][c] = "0"
                            self._curve_bends.add((r, c))
                            self._persistent_curve_bends.add(key)
                    continue                     # no perpendicular arms -> done

                # Scan-time evidence whose geometry fits this bend.
                arc_ev = (self._arc_evidence(wx, wy, up, dn, lf, rt, confirm=True)
                          if self.MATRIX_ARC_CURVES else 0)

                # CURVE-PRIORITY at a T-junction. A curved wall's bend is a '0'
                # even where a STRAIGHT wall runs straight through the same vertex
                # (up+down or left+right). Use the fitted arc, rather than the
                # filled centre cell alone, to distinguish this from a sharp T.
                straight_through = (up and dn) or (lf and rt)
                if straight_through:
                    if arc_ev >= self.ARC_SOFT_HITS:
                        matrix[r][c] = "0"
                        self._curve_bends.add((r, c))
                        self._persistent_curve_bends.add(key)
                    continue

                # Fitted arc evidence identifies a bend even if a straight wall
                # through the same vertex makes its centre cell occupied.
                if arc_ev >= self.ARC_SOFT_HITS:
                    matrix[r][c] = "0"
                    self._curve_bends.add((r, c))
                    self._persistent_curve_bends.add(key)
                    continue

        # Materialise only models surviving the existing contradictory-view
        # veto above. Drawing earlier would manufacture raster arms that hide
        # a newly observed sharp junction from that veto.
        for bx, by, sx, sy, wx, wy in raw_models:
            if (bx, by, sx, sy) not in self._confirmed_curve_quadrants:
                continue
            r, c = round((y1-wy)/cell), round((wx-x0)/cell)
            for step in range(3):
                for rr, cc in ((r, c+sx*step), (r-sy*step, c)):
                    if 0 <= rr < rows and 0 <= cc < cols:
                        matrix[rr][cc] = "1" if step else "0"
            if 0 <= r < rows and 0 <= c < cols:
                self._curve_bends.add((r, c))
                self._persistent_curve_bends.add((bx, by))
        # Internally retain the occupied endpoint of every measured curve,
        # even when another curve has an empty bend at the same vertex.
        endpoints = {(bx+sx,by) for bx,by,sx,sy in self._confirmed_curve_quadrants}
        endpoints |= {(bx,by+sy) for bx,by,sx,sy in self._confirmed_curve_quadrants}
        for r,c in self._curve_bends:
            if self._vertex_key(x0+c*cell,y1-r*cell) in endpoints:
                matrix[r][c] = "1"

    def _vertex_key(self, wx, wy):
        """Stable physical id (60 mm grid) for a matrix vertex.

        (wx, wy) is in the drift-corrected matrix frame; undo the grid-lock
        offset to recover the true physical position, then snap to the 60 mm
        vertex grid. The result is identical across rebuilds regardless of how
        the matrix is cropped/reindexed or how much GPS drift was removed, so it
        can key a persistent set of curve bends.
        """
        dx, dy = self.last_grid_offset
        return (int(round((wx + dx) / self.QUARTER_MM)),
                int(round((wy + dy) / self.QUARTER_MM)))

    def _arc_evidence(self, wx, wy, up, dn, lf, rt, confirm=False):
        """Count scan evidence only when it fits a 60 mm quarter-circle bend.

        Repeated hits near a vertex alone cannot identify a curve: an arc ending
        at the vertex belongs to its neighbour. Around the correct bend, the
        distances from its two axes vary in opposite directions and fit a circle
        centred one quarter-tile away along both axes.
        """
        vertex = self._vertex_key(wx, wy)
        confirmed = vertex in self._persistent_curve_bends
        dx, dy = self.last_grid_offset
        wx, wy = wx+dx, wy+dy
        radius, grid = self.QUARTER_MM, self.GRID_MM
        gx, gy = round(wx/grid)+self.c, round(wy/grid)+self.c
        rad = math.ceil(radius/grid)
        x0, x1 = max(0, gx-rad), min(self.n, gx+rad+1)
        y0, y1 = max(0, gy-rad), min(self.n, gy+rad+1)
        votes = self.arc_grid[y0:y1, x0:x1]
        if 0 <= gx < self.n and 0 <= gy < self.n and (confirmed or self.logodds[gy, gx] <= self.FREE_T):
            # Outer-face hits lie closer to the axes and overlap pose noise on
            # straight walls. Confirm them from a seen-free bend first; once its
            # geometry is established, a straight wall through that bend must
            # not hide the previously measured outer face.
            votes = votes + self.convex_arc_grid[y0:y1, x0:x1]
        yy, xx = np.indices(votes.shape)
        xx = (xx+x0-self.c)*grid-wx
        yy = (yy+y0-self.c)*grid-wy
        best = 0
        x_dirs = [s for s, present in ((-1, lf), (1, rt)) if present] or [-1, 1]
        y_dirs = [s for s, present in ((-1, dn), (1, up)) if present] or [-1, 1]
        for sx in x_dirs:
            for sy in y_dirs:
                # Coarse voxels from a diagonal box edge can also fit the arc
                # template. When precise scan data is available, require its
                # independent measured curvature before accepting a model.
                if (self._native_curve_samples
                        and self._scan_curve_votes.get((*vertex, sx, sy), 0) < self.ARC_SOFT_HITS):
                    continue
                ax, ay = sx*xx, sy*yy
                quadrant = ((ax >= grid) & (ay >= grid)
                            & (ax <= radius) & (ay <= radius) & (votes > 0))
                weights = votes[quadrant]
                total = int(weights.sum())
                if total < self.ARC_SOFT_HITS:
                    continue
                x, y = ax[quadrant], ay[quadrant]
                fit = abs(np.hypot(radius-x, radius-y)-radius) <= self.ARC_RADIUS_TOL_MM
                fitted = int(weights[fit].sum())
                # Geometry depends on distinct hit locations, not how long one
                # face was viewed. Repeated hits on another nearby surface must
                # not outweigh and erase an already measured curved wall.
                if np.count_nonzero(fit) < 3 or np.count_nonzero(fit) <= len(x)/2:
                    continue
                x, y = x[fit], y[fit]
                if not (np.any(x < y) and np.any(x > y)):
                    continue
                covariance = np.mean((x-x.mean())*(y-y.mean()))
                if covariance < 0 and fitted >= self.ARC_SOFT_HITS:
                    model = (*vertex, sx, sy)
                    if model not in self._confirmed_curve_quadrants:
                        # A short diagonal object edge can resemble an arc's
                        # middle. Confirm both 60 mm endpoints before accepting
                        # a new quadrant. Later occlusion cannot undo that proof.
                        endpoints = ((gx+sx*rad, gy), (gx, gy+sy*rad))
                        if not all(0 < ex < self.n-1 and 0 < ey < self.n-1
                                   and np.any(self.logodds[ey-1:ey+2, ex-1:ex+2] >= self.OCC_T)
                                   for ex, ey in endpoints):
                            continue
                    if confirm:
                        self._confirmed_curve_quadrants.add(model)
                    best = max(best, fitted)
        # Partial outside views need not cross the arc's diagonal. Repeated
        # native-precision circle fits can identify that curvature while the
        # old voxel covariance is still inconclusive. A measured connection
        # establishes its wall role before accepting a new model.
        for model,count in self._scan_curve_votes.items():
            if model[:2]!=vertex or count<self.ARC_SOFT_HITS:
                continue
            _,_,sx,sy=model
            if sx not in x_dirs or sy not in y_dirs:
                continue
            if model not in self._confirmed_curve_quadrants:
                endpoints=((vertex[0]*radius+sx*radius,vertex[1]*radius),
                           (vertex[0]*radius,vertex[1]*radius+sy*radius))
                if not all(0<round(ex/grid)+self.c<self.n-1 and 0<round(ey/grid)+self.c<self.n-1
                    and np.any(self.logodds[round(ey/grid)+self.c-1:round(ey/grid)+self.c+2,
                                           round(ex/grid)+self.c-1:round(ex/grid)+self.c+2]>=self.OCC_T)
                    for ex,ey in endpoints):
                    continue
                connected = any(endpoint in ((ax,ay),(bx,by)) for endpoint in endpoints
                                for ax,ay,bx,by in self._confirmed_straight_walls)
                if not connected:
                    # Curves also join at their measured endpoints. An
                    # unconfirmed fit or a bend is not an independent anchor.
                    connected = any(endpoint in ((vx*radius+xs*radius, vy*radius),
                                                 (vx*radius, vy*radius+ys*radius))
                                    for endpoint in endpoints
                                    for vx,vy,xs,ys in self._confirmed_curve_quadrants)
                if not connected:
                    continue
            if confirm:self._confirmed_curve_quadrants.add(model)
            best=max(best,count)
        return best

    def _cell_grid_window(self, r, c, x0, y1, cell, rad):
        """High-res log-odds window covering matrix cell (r, c)."""
        wx = x0 + c * cell
        wy = y1 - r * cell
        gx = int(round(wx / self.GRID_MM)) + self.c
        gy = int(round(wy / self.GRID_MM)) + self.c
        if not (0 <= gx < self.n and 0 <= gy < self.n):
            return None
        view=np.s_[max(0, gy-rad):gy+rad+1,max(0, gx-rad):gx+rad+1]
        window=self.logodds[view]
        if self._obstacle_mask is not None:
            # Identified object cells are neither wall support nor seen free.
            # Rescue, denoise and closure all use this same semantic view.
            window=np.where(self._obstacle_mask[view],0,window)
        return window

    def _rescue_dropped_walls(self, matrix, rows, cols, x0, y1, cell):
        """Complete internal walls the matrix dropped but the high-res occupancy
        clearly shows - runs EVERY frame (not only on 'F').

        The denoise / grid-snap / curve passes occasionally drop a real wall cell
        (a curve arc, a half-wall seen at a grazing angle, a single-cell lidar
        hole in a straight run). This restores any empty matrix cell that (a) sits
        on the wall bounding box, (b) is orthogonally adjacent to an existing wall
        LINE, and (c) reads clearly occupied (and NOT seen-through) in the live
        high-res grid. The high-res check is essential: a plain 'wall-gap-wall'
        pattern is usually two real walls with a passage between them, so a gap can
        never be filled from grid rules alone. Keeping internal walls whole every
        frame is what makes the manual 'F' border-closure rarely needed. Matrix
        only; curve bend vertices are protected; the arena frame is never grown.
        """
        curve_bends = getattr(self, "_curve_bends", set())
        wb = [(r, c) for r in range(rows) for c in range(cols) if matrix[r][c] == "1"]
        if not wb:
            return
        bmnr = min(r for r, _ in wb); bmxr = max(r for r, _ in wb)
        bmnc = min(c for _, c in wb); bmxc = max(c for _, c in wb)
        add = []
        for r in range(rows):
            for c in range(cols):
                if matrix[r][c] != "0" or (r, c) in curve_bends:
                    continue
                if not (bmnr <= r <= bmxr and bmnc <= c <= bmxc):
                    continue                          # never grow the arena frame
                if not ((r > 0 and matrix[r - 1][c] == "1")
                        or (r < rows - 1 and matrix[r + 1][c] == "1")
                        or (c > 0 and matrix[r][c - 1] == "1")
                        or (c < cols - 1 and matrix[r][c + 1] == "1")):
                    continue                          # must extend an existing wall line
                win = self._cell_grid_window(r, c, x0, y1, cell, 1)
                if win is None or win.size == 0:
                    continue
                occ = int(np.count_nonzero(win >= self.OCC_T))
                free = int(np.count_nonzero(win <= self.FREE_T))
                if occ >= self.CLOSURE_RESCUE_MIN_OCC and free <= self.CLOSURE_RESCUE_MAX_FREE:
                    add.append((r, c))
        for r, c in add:
            matrix[r][c] = "1"

    def _apply_closure(self, matrix, rows, cols, x0, y1, cell):
        """Border / wall closure - runs only when toggled with 'F'.

        Two data-driven passes using the LIVE high-res occupancy:

          1. RESCUE - the denoise / curve passes sometimes delete real wall
             cells (curve arcs, half-walls seen at a grazing angle). For every
             empty matrix cell that touches an existing wall, if the high-res
             clearly shows a wall there (and it was not seen *through*), restore
             it. This recovers walls the lidar did reach but the matrix dropped.

          2. BORDER - the arena is fully enclosed. Where an explored/free cell
             sits directly against never-observed space across a tile-edge line,
             that edge must be a wall the lidar never grazed; add it.

        Everything is matrix-only and gated behind 'F', so the normal matrix and
        already-correct maps are never affected.
        """
        rad = 1                                   # 3x3 high-res window ~ one 30 mm matrix cell
        added = 0
        # Vertices the curve pass demoted to a bend '0' are final: a curved wall
        # forces its bend vertex to 0 even where a straight wall meets it
        # (Erebus stamps curves after walls). Closure must never turn these back
        # into a sharp '1'.
        curve_bends = getattr(self, "_curve_bends", set())

        # The arena is fully enclosed, so the outer wall frame is already in the
        # matrix from the raster pass. Closure must never grow that frame
        # outward - a rescued arc or border cell placed beyond it would enlarge
        # the cropped matrix and break the competition shape. Clamp every
        # candidate to the bounding box of the walls present *before* closure.
        wb_cells = [(r, c) for r in range(rows) for c in range(cols)
                    if matrix[r][c] == "1"]
        if wb_cells:
            bb_min_r = min(r for r, _ in wb_cells)
            bb_max_r = max(r for r, _ in wb_cells)
            bb_min_c = min(c for _, c in wb_cells)
            bb_max_c = max(c for _, c in wb_cells)
        else:
            bb_min_r = bb_min_c = 0
            bb_max_r, bb_max_c = rows - 1, cols - 1

        def _inside_frame(r, c):
            return bb_min_r <= r <= bb_max_r and bb_min_c <= c <= bb_max_c

        # walls present before any closure pass runs - used by the free-space
        # guard below to tell which cells closure itself introduced
        pre_closure = {(r, c) for r, c in wb_cells}

        # --- pass 1: rescue dropped walls backed by the high-res occupancy ---
        rescue = []
        for r in range(rows) if self.CLOSURE_PASS_RESCUE else ():
            for c in range(cols):
                if matrix[r][c] != "0":
                    continue
                if (r, c) in curve_bends:
                    continue                          # protected curve bend vertex
                if not _inside_frame(r, c):
                    continue                          # never extend the arena frame
                # Must extend an existing wall LINE: 4-connected (orthogonal) to
                # a wall. Using orthogonal (not diagonal) connectivity means a
                # curve's bend vertex - which only ever touches its arc on the
                # diagonal - is never re-filled, so rescue cannot turn a curve
                # back into a sharp corner.
                if not ((r > 0 and matrix[r - 1][c] == "1")
                        or (r < rows - 1 and matrix[r + 1][c] == "1")
                        or (c > 0 and matrix[r][c - 1] == "1")
                        or (c < cols - 1 and matrix[r][c + 1] == "1")):
                    continue
                win = self._cell_grid_window(r, c, x0, y1, cell, rad)
                if win is None or win.size == 0:
                    continue
                occ = int(np.count_nonzero(win >= self.OCC_T))
                free = int(np.count_nonzero(win <= self.FREE_T))
                if occ >= self.CLOSURE_RESCUE_MIN_OCC and free <= self.CLOSURE_RESCUE_MAX_FREE:
                    rescue.append((r, c))
        for r, c in rescue:
            matrix[r][c] = "1"
        added += len(rescue)

        # --- pass 2: close borders between explored-free and unobserved space ---
        # Only edge-line cells (those lying on a 60 mm tile-edge line, i.e. with
        # at least one even index) can carry a wall; check the high-res on either
        # side of the line. Free on one side + unknown on the other => border.
        border = []
        for r in range(rows) if self.CLOSURE_PASS_BORDER else ():
            for c in range(cols):
                if matrix[r][c] != "0":
                    continue
                if (r, c) in curve_bends:
                    continue                      # protected curve bend vertex
                if not _inside_frame(r, c):
                    continue                      # never extend the arena frame
                on_h_line = (r % 2 == 0)          # horizontal tile-edge line
                on_v_line = (c % 2 == 0)          # vertical tile-edge line
                if not (on_h_line or on_v_line):
                    continue
                # neighbours perpendicular to the line define the two sides
                if on_h_line and not on_v_line:
                    sides = ((r - 1, c), (r + 1, c))
                elif on_v_line and not on_h_line:
                    sides = ((r, c - 1), (r, c + 1))
                else:
                    continue                      # skip vertices (handled by walls/curves)
                a, b = sides
                wa = self._cell_grid_window(a[0], a[1], x0, y1, cell, rad)
                wb = self._cell_grid_window(b[0], b[1], x0, y1, cell, rad)
                if wa is None or wb is None or wa.size == 0 or wb.size == 0:
                    continue
                fa = int(np.count_nonzero(wa <= self.FREE_T))
                fb = int(np.count_nonzero(wb <= self.FREE_T))
                ua = int(np.count_nonzero((wa > self.FREE_T) & (wa < self.OCC_T)))
                ub = int(np.count_nonzero((wb > self.FREE_T) & (wb < self.OCC_T)))
                free_unknown = (fa >= self.CLOSURE_BORDER_FREE_MIN and ub >= self.CLOSURE_BORDER_UNK_MIN)
                unknown_free = (fb >= self.CLOSURE_BORDER_FREE_MIN and ua >= self.CLOSURE_BORDER_UNK_MIN)
                if free_unknown or unknown_free:
                    border.append((r, c))
        for r, c in border:
            matrix[r][c] = "1"
        added += len(border)

        # --- pass 3: trace the outer boundary (any shape) and bridge its gaps ---
        # Generalises the old rectangle-only pin-hole fill: it finds the arena's
        # outer wall ring whatever its shape (rectangle, L, stepped, curved) and
        # closes each disconnection with a single straight horizontal/vertical
        # run from the start of the gap to its end (never a diagonal staircase).
        if self.CLOSURE_PASS_OUTER:
            added += self._close_outer_boundary(matrix, rows, cols, x0, y1, cell,
                                                 curve_bends)

        vfill = 0
        for r in range(2, rows - 1, 2) if self.CLOSURE_PASS_VERTEX else ():
            for c in range(2, cols - 1, 2):
                if matrix[r][c] != "0" or (r, c) in curve_bends:
                    continue
                up = matrix[r - 1][c] == "1"; dn = matrix[r + 1][c] == "1"
                lf = matrix[r][c - 1] == "1"; rt = matrix[r][c + 1] == "1"
                straight_v = up and dn and not lf and not rt
                straight_h = lf and rt and not up and not dn
                if straight_v or straight_h:
                    matrix[r][c] = "1"; vfill += 1
        added += vfill

        # --- pass 5: seal gaps in the real (possibly non-rectangular) boundary ---
        # Pass the pre-closure wall bounding box so the membrane, like every other
        # pass, can NEVER place a wall outside the existing outer frame (a cell one
        # line beyond the border would enlarge the cropped matrix and shift the
        # whole map, breaking an already-correct arena).
        if self.CLOSURE_PASS_MEMBRANE:
            added += self._close_boundary_membrane(matrix, rows, cols, x0, y1, cell,
                                                    curve_bends,
                                                    (bb_min_r, bb_max_r,
                                                     bb_min_c, bb_max_c))

        # --- free-space guard: never let closure wall a cell the lidar clearly
        # saw THROUGH. Walls block the beam, so a cell that reads solidly free
        # cannot hold one; closure only guessed it while sealing a leak in an
        # under-observed pocket (this is the lower-right "double wall" artefact).
        # The single exception is a 1-cell gap that bridges two colinear walls -
        # a real wall line the lidar grazed - which we must keep. Cells closure
        # did not add, and unobserved boundary gaps (free ~ 0), are untouched, so
        # already-correct maps are unaffected.
        removed = self._drop_free_closure_cells(matrix, rows, cols, x0, y1, cell,
                                                pre_closure)
        added -= removed

        self.last_closure_added = added
        return added

    def _drop_free_closure_cells(self, matrix, rows, cols, x0, y1, cell,
                                 pre_closure):
        """Undo closure fills that sit on clearly-free high-res space.

        Iterated to convergence so that once a spurious parallel wall is peeled
        off, any stub that only bridged it is re-examined too.
        """
        def wall(r, c):
            return 0 <= r < rows and 0 <= c < cols and matrix[r][c] == "1"

        def colinear_bridge(r, c):
            return ((wall(r - 1, c) and wall(r + 1, c))
                    or (wall(r, c - 1) and wall(r, c + 1)))

        removed = 0
        changed = True
        while changed:
            changed = False
            for r in range(rows):
                for c in range(cols):
                    if matrix[r][c] != "1" or (r, c) in pre_closure:
                        continue
                    win = self._cell_grid_window(r, c, x0, y1, cell, 1)
                    if win is None or win.size == 0:
                        continue
                    occ = int(np.count_nonzero(win >= self.OCC_T))
                    free = int(np.count_nonzero(win <= self.FREE_T))
                    if occ == 0 and free >= self.CLOSURE_FREE_BLOCK_MIN \
                            and not colinear_bridge(r, c):
                        matrix[r][c] = "0"
                        removed += 1
                        changed = True
        return removed

    def _close_outer_boundary(self, matrix, rows, cols, x0, y1, cell,
                              curve_bends):
        """Find the arena's outer wall ring (ANY shape) and bridge its gaps.

        The arena is fully enclosed by a single outer loop of walls. The lidar
        can graze past a patch of it, leaving a run of empty cells between two
        ring walls that sit on the same 60 mm line. This pass:

          1. Locates the EXTERIOR - every cell reachable from the matrix border
             without crossing a wall or entering explored interior. Because the
             explored interior blocks the flood, the exterior hugs the arena's
             true outline whatever its shape and only pokes a short way through
             an actual (unknown-backed) wall gap.
          2. Walks every horizontal (even-row) and vertical (even-col) wall
             line. Between two consecutive walls on a line it treats the empty
             run as a candidate gap and confirms it is genuinely ON the outer
             boundary: one perpendicular side is exterior along the WHOLE run and
             the other side is explored interior. A doorway (explored on both
             sides) or an interior wall never passes this test.
          3. Fills the whole confirmed run at once - a clean straight segment
             from the start of the disconnection to its end, never a diagonal
             staircase.

        Matrix-only, gated behind 'F'. Filling always happens between two
        existing ring walls, so it can never grow the arena outward.
        """
        expl = self._matrix_explored_mask(rows, cols, x0, y1, cell)

        def wall(r, c):
            return 0 <= r < rows and 0 <= c < cols and matrix[r][c] == "1"

        # exterior flood: border-connected, blocked by walls AND by explored
        # interior (so it cannot leak across the whole inside through one gap).
        outside = [[False] * cols for _ in range(rows)]
        stack = []
        for r in range(rows):
            for c in (0, cols - 1):
                if not wall(r, c) and not expl[r][c] and not outside[r][c]:
                    outside[r][c] = True; stack.append((r, c))
        for c in range(cols):
            for r in (0, rows - 1):
                if not wall(r, c) and not expl[r][c] and not outside[r][c]:
                    outside[r][c] = True; stack.append((r, c))
        while stack:
            r, c = stack.pop()
            for dr, dc in ((1, 0), (-1, 0), (0, 1), (0, -1)):
                nr, nc = r + dr, c + dc
                if (0 <= nr < rows and 0 <= nc < cols and not outside[nr][nc]
                        and not wall(nr, nc) and not expl[nr][nc]):
                    outside[nr][nc] = True; stack.append((nr, nc))

        def OUT(r, c):
            return 0 <= r < rows and 0 <= c < cols and outside[r][c]

        def EXP(r, c):
            return 0 <= r < rows and 0 <= c < cols and expl[r][c]

        def _side_is_outside(span_cells):
            return all(OUT(r, c) for r, c in span_cells)

        def _side_is_inside(span_cells):
            # inside must never read as exterior and must be genuinely explored
            return (all(not OUT(r, c) for r, c in span_cells)
                    and any(EXP(r, c) for r, c in span_cells))

        fills = []

        # horizontal boundary lines (even rows): gap runs along columns
        for r in range(0, rows, 2):
            wcols = [c for c in range(cols) if wall(r, c)]
            for c1, c2 in zip(wcols, wcols[1:]):
                if c2 - c1 <= 1:
                    continue                       # contiguous, no gap
                span = list(range(c1 + 1, c2))
                up = [(r - 1, c) for c in span]
                dn = [(r + 1, c) for c in span]
                boundary = ((_side_is_outside(up) and _side_is_inside(dn))
                            or (_side_is_outside(dn) and _side_is_inside(up)))
                if boundary:
                    for c in span:
                        if (r, c) not in curve_bends and matrix[r][c] != "1":
                            fills.append((r, c))

        # vertical boundary lines (even cols): gap runs along rows
        for c in range(0, cols, 2):
            wrows = [r for r in range(rows) if wall(r, c)]
            for r1, r2 in zip(wrows, wrows[1:]):
                if r2 - r1 <= 1:
                    continue
                span = list(range(r1 + 1, r2))
                lf = [(r, c - 1) for r in span]
                rt = [(r, c + 1) for r in span]
                boundary = ((_side_is_outside(lf) and _side_is_inside(rt))
                            or (_side_is_outside(rt) and _side_is_inside(lf)))
                if boundary:
                    for r in span:
                        if (r, c) not in curve_bends and matrix[r][c] != "1":
                            fills.append((r, c))

        for r, c in fills:
            matrix[r][c] = "1"
        return len(fills)

    def _matrix_explored_mask(self, rows, cols, x0, y1, cell):
        """Per-matrix-cell mask: True where the high-res shows free or visited.

        A cell counts as explored if its 30 mm footprint touches any free
        (seen-through) or visited high-res cell. Occupied-but-not-free (a wall)
        and never-scanned space are both left False, so 'explored' means the
        robot actually cleared that spot.
        """
        expl = [[False] * cols for _ in range(rows)]
        for r in range(rows):
            wy = y1 - r * cell
            gy = int(round(wy / self.GRID_MM)) + self.c
            for c in range(cols):
                wx = x0 + c * cell
                gx = int(round(wx / self.GRID_MM)) + self.c
                y0 = max(0, gy - 1); x0i = max(0, gx - 1)
                wl = self.logodds[y0:gy + 2, x0i:gx + 2]
                vv = self.visited[y0:gy + 2, x0i:gx + 2]
                if wl.size and (np.any(wl <= self.FREE_T) or np.any(vv)):
                    expl[r][c] = True
        return expl

    def _close_boundary_membrane(self, matrix, rows, cols, x0, y1, cell,
                                 curve_bends, frame_bb=None):
        """Fill gaps in the arena's outer wall, whatever its shape.

        The arena is fully enclosed, but the lidar can miss a grazing patch of
        the boundary, leaving a hole. Rather than assuming a rectangular frame
        (pass 3), this finds the boundary by flood-filling the OUTSIDE region
        from the matrix edge through never-scanned space (walls and explored
        interior block it). A hole appears as an outside cell that has leaked in
        until it touches explored interior. We seal such a hole only when the
        cell lies on a 60 mm wall line and continues an existing wall, so real
        doorways and interior free space are never walled off.

        Matrix-only and gated behind 'F' (idea mirrors the boundary closure in
        old/Main.py, adapted to this occupancy representation).
        """
        expl = self._matrix_explored_mask(rows, cols, x0, y1, cell)

        if frame_bb is not None:
            f_min_r, f_max_r, f_min_c, f_max_c = frame_bb
        else:
            f_min_r, f_max_r, f_min_c, f_max_c = 0, rows - 1, 0, cols - 1

        def _in_frame(r, c):
            return f_min_r <= r <= f_max_r and f_min_c <= c <= f_max_c

        def wall(r, c):
            return 0 <= r < rows and 0 <= c < cols and matrix[r][c] == "1"

        # Flood-fill the exterior from the matrix border through cells that are
        # neither a wall nor explored interior (i.e. never-scanned outside).
        ext = [[False] * cols for _ in range(rows)]
        stack = []
        for r in range(rows):
            for c in (0, cols - 1):
                if not wall(r, c) and not expl[r][c] and not ext[r][c]:
                    ext[r][c] = True; stack.append((r, c))
        for c in range(cols):
            for r in (0, rows - 1):
                if not wall(r, c) and not expl[r][c] and not ext[r][c]:
                    ext[r][c] = True; stack.append((r, c))
        while stack:
            r, c = stack.pop()
            for dr, dc in ((1, 0), (-1, 0), (0, 1), (0, -1)):
                nr, nc = r + dr, c + dc
                if (0 <= nr < rows and 0 <= nc < cols and not ext[nr][nc]
                        and not wall(nr, nc) and not expl[nr][nc]):
                    ext[nr][nc] = True; stack.append((nr, nc))

        def EXT(r, c):
            return 0 <= r < rows and 0 <= c < cols and ext[r][c]

        def EXP(r, c):
            return 0 <= r < rows and 0 <= c < cols and expl[r][c]

        fill = []
        for r in range(rows):
            for c in range(cols):
                if matrix[r][c] == "1" or (r, c) in curve_bends:
                    continue
                if not _in_frame(r, c):
                    continue                      # never grow the arena outward
                r_even, c_even = (r % 2 == 0), (c % 2 == 0)
                if r_even and c_even:
                    # vertex: only seal if it bridges two colinear walls and the
                    # exterior has reached it (a genuine hole on the loop)
                    flank = ((wall(r, c - 1) and wall(r, c + 1))
                             or (wall(r - 1, c) and wall(r + 1, c)))
                    touch = EXT(r - 1, c) or EXT(r + 1, c) or EXT(r, c - 1) or EXT(r, c + 1)
                    if flank and touch:
                        fill.append((r, c))
                elif r_even and not c_even:
                    # horizontal wall line: needs a horizontal wall neighbour and
                    # must separate explored interior from exterior (a boundary)
                    line = wall(r, c - 1) or wall(r, c + 1)
                    sep = (EXP(r - 1, c) and EXT(r + 1, c)) or (EXP(r + 1, c) and EXT(r - 1, c))
                    if line and sep:
                        fill.append((r, c))
                elif c_even and not r_even:
                    line = wall(r - 1, c) or wall(r + 1, c)
                    sep = (EXP(r, c - 1) and EXT(r, c + 1)) or (EXP(r, c + 1) and EXT(r, c - 1))
                    if line and sep:
                        fill.append((r, c))
        for r, c in fill:
            matrix[r][c] = "1"
        return len(fill)

    def save_json(self, path=None):
        # DEBUG ONLY: local inspection file, not the supervisor submission.
        if not self.DEBUG_MAPPING_DUMP:
            return None
        path = path or self.JSON_PATH
        v_walls, h_walls = self._v_walls, self._h_walls
        payload = {
            "matrix": self.report_matrix,
            "matrix_kind": "internal_unmasked",
            "rows": len(self.report_matrix),
            "cols": len(self.report_matrix[0]) if self.report_matrix else 0,
            "start_tile": list(self.start_tile),
            "visited_tiles": sorted(map(list, self.visited_tiles)),
            "vertical_walls": sorted(map(list, v_walls)),
            "horizontal_walls": sorted(map(list, h_walls)),
            "obstacle_tiles": sorted(map(list, getattr(self, "obstacle_tiles", set()))),
        }
        with open(path, "w", encoding="utf-8") as f:
            json.dump(payload, f, indent=2)
        return path

    def _area4_border_tiles(self, labels, area4, rooms, tiles, occupied):
        """Scoring-only tile cover of measured Area4 boundary material.

        Erebus masks full 120mm tile footprints, including the outside portion
        of a curved border. Body visits cannot establish that tile cover. Use
        the already resolved free components to attribute observed wall material
        to its nearest observed floor side; never fill a rectangle/convex hull.
        Disconnected material and ambiguous/other-room floor supply no proof.
        """
        if not area4:
            return set()
        occupied = occupied.copy()
        forbidden = {tile for tile, room in self._room_tiles.items() if room != 4}
        forbidden.update(tile for tile, token in self._committed_floor_tiles.items()
                         if token in self.CS_CONNECTION_CHARS or token == "2")
        # Do not carry boundary ownership along a wall through a known portal
        # or another room into an as-yet unclassified region.
        for tx, ty in forbidden:
            occupied[np.ix_(tiles == ty, tiles == tx)] = False
        # Crop only empty padding; keep every observed floor/return so nearest
        # side selection is identical to the full-grid transform.
        ys, xs = np.nonzero((labels != 0) | occupied)
        y0, y1, x0, x1 = int(ys.min()), int(ys.max())+1, int(xs.min()), int(xs.max())+1
        labels = labels[y0:y1, x0:x1]
        occupied = occupied[y0:y1, x0:x1]
        free4 = np.isin(labels, area4)
        xtiles, ytiles = tiles[x0:x1], tiles[y0:y1]
        holes = [tile for tile, token in self._committed_floor_tiles.items() if token == "2"]
        if holes:
            # LiDAR free rays can cross a pit. Border ownership cannot use
            # unseeded floor on its far side as room identity evidence.
            for tx, ty in holes:
                free4[np.ix_(ytiles == ty, xtiles == tx)] = False
            pieces, _ = scipy_ndimage.label(free4)
            seeds = set()
            for (tx, ty), room in self._room_tiles.items():
                if room == 4:
                    seeds.update(np.unique(pieces[np.ix_(ytiles == ty, xtiles == tx)]))
            seeds.discard(0)
            free4 = np.isin(pieces, list(seeds))
        # A surface must have a measured connection to Area4 floor. Nearest
        # floor alone would assign arbitrarily distant, isolated returns.
        walls, _ = scipy_ndimage.label(occupied, structure=np.ones((3, 3)))
        touching = np.unique(walls[occupied & scipy_ndimage.binary_dilation(
            free4, structure=np.ones((3, 3)))])
        touching = touching[touching != 0]
        if not touching.size:
            return set()
        nearest = scipy_ndimage.distance_transform_edt(labels == 0,
            return_distances=False, return_indices=True)
        owned = (occupied & np.isin(walls, touching)
                 & free4[nearest[0], nearest[1]])
        ys, xs = np.nonzero(owned)
        # Move one evidence cell toward the observed interior so a wall lying
        # on a tile edge belongs to the inside tile, not its outside neighbor.
        iy = ys + np.sign(nearest[0, ys, xs] - ys)
        ix = xs + np.sign(nearest[1, ys, xs] - xs)
        result = set(zip(map(int, xtiles[ix]), map(int, ytiles[iy])))
        # A wall seen from Area4 can have back-face returns in the adjacent
        # room. Floor components, including unvisited tiles, veto ownership;
        # checking only the visited _room_tiles misses exactly those borders.
        other_labels = [label for label, known in enumerate(rooms) if known - {4}]
        ys, xs = np.nonzero(np.isin(labels, other_labels))
        forbidden.update(zip(map(int, xtiles[xs]), map(int, ytiles[ys])))
        return result - forbidden

    def _close_area4_export(self, established, context):
        # Single-entry memo: every input to the unchanged closure proof is
        # included. New free/occupied/visited cells, room labels, wall raster,
        # passage observations or coordinate settings immediately invalidate it.
        labels, _, _, tiles, occupied, visited = context
        key = (frozenset(established), frozenset(self._room_tiles.items()),
               frozenset(self._committed_floor_tiles.items()), self.n, self.c,
               self.GRID_MM, self.TILE_MM, NavigationMapper.ROBOT_RADIUS_MM,
               frozenset(self.CS_CONNECTION_CHARS), tuple(sorted((self._matrix_meta or {}).items())),
               tuple(tuple(row) for row in (self.report_matrix or ())),
               (labels != 0).tobytes(), tiles.tobytes(),
               occupied.tobytes(), visited.tobytes())
        cached = getattr(self, '_area4_closure_cache', None)
        if cached is None or cached[0] != key:
            cached = self._area4_closure_cache = (key, frozenset(
                self._compute_area4_export_closure(established, context)))
        return set(cached[1])

    def _compute_area4_export_closure(self, established, context):
        """Resolve export regions and close unsupported corners, never navigation.

        Raw ray cracks can merge rooms. Partition observed floor at output-cell
        and body clearance, accepting only components with unambiguous room4
        seeds. Unobserved passages and other-room floor remain barriers. The
        only extrapolation is enclosed islands or a simpler unsupported corner;
        measured re-entrant walls always win over a rectangular completion.
        """
        raw_labels, _, _, tile_axis, occupied, visited = context
        seen = raw_labels != 0
        for (tx, ty), token in self._committed_floor_tiles.items():
            if token in self.CS_CONNECTION_CHARS or token == "2":
                seen[np.ix_(tile_axis == ty, tile_axis == tx)] = False
        ys, xs = np.nonzero(seen | occupied)
        if not xs.size:
            return set(established)
        # A square crop preserves the existing x/y tile-axis helper contract.
        lo = max(0, int(min(xs.min(), ys.min()))-1)
        hi = min(self.n, int(max(xs.max(), ys.max()))+2)
        raw, material = seen[lo:hi, lo:hi], occupied[lo:hi, lo:hi]
        traversed = visited[lo:hi, lo:hi].copy()
        for (tx, ty), token in self._committed_floor_tiles.items():
            if token in self.CS_CONNECTION_CHARS or token == "2":
                traversed[np.ix_(tile_axis[lo:hi] == ty, tile_axis[lo:hi] == tx)] = False
        tiles = tile_axis[lo:hi]
        coords = (np.arange(lo, hi)-self.c)*self.GRID_MM
        central = abs(coords-tiles*self.TILE_MM) < self.TILE_MM/4
        interior = abs(coords-tiles*self.TILE_MM) < self.TILE_MM/2
        clearance = scipy_ndimage.distance_transform_edt(~material)*self.GRID_MM
        candidates, certain, uncertain = set(), set(), set()
        forbidden = {t for t, room in self._room_tiles.items() if room != 4}
        forbidden.update(t for t, ch in self._committed_floor_tiles.items()
                         if ch in self.CS_CONNECTION_CHARS or ch == "2")
        def tile_set(mask):
            y, x = np.nonzero(mask)
            return set(zip(map(int, tiles[x]), map(int, tiles[y])))
        for radius in sorted({self.TILE_MM/4, NavigationMapper.ROBOT_RADIUS_MM}):
            labels, count = scipy_ndimage.label((raw | traversed) & (clearance >= radius))
            domains = [set() for _ in range(count+1)]
            for (tx, ty), room in self._room_tiles.items():
                if self._committed_floor_tiles.get((tx, ty)) in self.CS_CONNECTION_CHARS:
                    continue
                crop = labels[np.ix_((tiles == ty) & central, (tiles == tx) & central)]
                for label in np.unique(crop):
                    if label:
                        domains[label].add(room)
            ids = [i for i, domain in enumerate(domains) if domain == {4}]
            if not ids:
                continue  # A mixed lower-clearance component cannot veto a proved one.
            nearest = scipy_ndimage.distance_transform_edt(labels == 0,
                return_distances=False, return_indices=True)
            projected = labels[nearest[0], nearest[1]]
            projected[~raw] = 0
            core = central[:, None] & central[None, :]
            certain |= tile_set(np.isin(projected, ids) & core)
            unknown = [i for i, domain in enumerate(domains) if i and (not domain or 4 in domain and len(domain)>1)]
            uncertain |= tile_set(np.isin(projected, unknown) & core)
            other = [i for i, domain in enumerate(domains) if domain and 4 not in domain]
            forbidden |= tile_set(np.isin(projected, other) & core)
            candidates |= tile_set(np.isin(labels, ids) & interior[:, None] & interior[None, :])
            candidates |= self._area4_border_tiles(projected, ids, domains, tiles, material)
        forbidden |= uncertain-certain
        result = set(established) | (candidates-forbidden)
        if not result:
            return result
        x0, y0 = np.min(list(result), axis=0)-1
        x1, y1 = np.max(list(result), axis=0)+1
        mask = np.zeros((y1-y0+1, x1-x0+1), bool)
        for x, y in result:
            mask[y-y0, x-x0] = True
        islands, count = scipy_ndimage.label(scipy_ndimage.binary_fill_holes(mask) & ~mask)
        for label in range(1, count+1):
            yy, xx = np.nonzero(islands == label)
            island = {(int(x+x0), int(y+y0)) for y, x in zip(yy, xx)}
            if not island & forbidden:
                result |= island
        # Close a gap on a supported straight outline or an outer rectangle
        # corner. Do not extend an interior notch merely to simplify its turns.
        outline = set(result)
        proposals = set()
        for x, y in outline:
            for dx, dy in ((-1,-1), (-1,0), (0,-1), (0,0)):
                bx, by = x+dx, y+dy
                box = {(bx,by), (bx+1,by), (bx,by+1), (bx+1,by+1)}
                missing = box-outline
                if len(missing) == 1:
                    proposals |= missing
        min_x = min(x for x,y in outline); max_x = max(x for x,y in outline)
        min_y = min(y for x,y in outline); max_y = max(y for x,y in outline)
        meta, matrix = self._matrix_meta, self.report_matrix
        for tx, ty in proposals-forbidden:
            straight_gap = ((tx-1,ty) in outline and (tx+1,ty) in outline or
                            (tx,ty-1) in outline and (tx,ty+1) in outline)
            outer_corner = tx in (min_x,max_x) and ty in (min_y,max_y)
            if not (straight_gap or outer_corner):
                continue
            if np.any(raw[np.ix_(tiles == ty, tiles == tx)]):
                continue  # Observed, unclassified floor may be an unseen passage.
            r, c = 4*(meta['max_ty']-ty), 4*(tx-meta['min_tx'])
            blocked = False
            for nx, ny in ((tx-1,ty), (tx+1,ty), (tx,ty-1), (tx,ty+1)):
                if (nx,ny) not in outline:
                    continue
                positions = ([(rr, c if nx<tx else c+4) for rr in range(r,r+5)]
                             if nx != tx else [(r if ny>ty else r+4, cc) for cc in range(c,c+5)])
                edge = [0 <= rr < len(matrix) and 0 <= cc < len(matrix[0]) and matrix[rr][cc] == "1"
                        for rr, cc in positions]
                if any(a and b for a, b in zip(edge, edge[1:])):
                    blocked = True
                    break
            if blocked:
                continue
            def turns(shape):
                total = 0
                for bx, by in ((tx-1,ty-1), (tx-1,ty), (tx,ty-1), (tx,ty)):
                    v = [(bx,by) in shape, (bx+1,by) in shape,
                         (bx,by+1) in shape, (bx+1,by+1) in shape]
                    total += int(sum(v) in (1,3)) + 2*int(sum(v) == 2 and v[0] == v[3])
                return total
            if turns(outline | {(tx,ty)}) < turns(outline):
                result.add((tx,ty))
        return result

    def _competition_area4_tiles(self):
        """Mask established observed Area4, including body-inaccessible floor.

        Keep export confidence separate from the inferred geometry region. The
        latter must not change physical wall or navigation processing merely
        because a downward floor observation has not been collected yet.
        """
        # These proofs use existing local room constraints, never world labels.
        # A changed passage invalidates the cache before the next matrix build.
        signature = tuple(sorted((tile, token) for tile, token in self._committed_floor_tiles.items()
                                 if token in self.CS_CONNECTION_CHARS))
        positive = {tile for tile, token in self._committed_floor_tiles.items() if token in ("3", "4")}
        extra = (self._competition_floor_area4 & positive
                 if signature == self._room_model_signature else set())
        # _update_area4_tiles already requires observed connected floor with
        # unambiguous Area4 evidence, bounded by established visits. Requiring
        # a SECOND body visit/downward sample here drops known obstacle-side
        # remnants from the scoring-only mask. Never expand to unknown tiles.
        established = self._area4_tiles if signature == self._room_model_signature else set()
        if signature == self._room_model_signature and self._competition_border_area4 is None:
            context = self._competition_border_context
            border = self._area4_border_tiles(*context[:5])
            self._competition_border_area4 = self._close_area4_export(established | extra | border, context)-established-extra
            self._competition_border_context = None
        border = self._competition_border_area4 if signature == self._room_model_signature else set()
        border = {tile for tile in border if self._committed_floor_tiles.get(tile) != "2"
                  and self._room_tiles.get(tile, 4) == 4}
        return established | extra | border

    def competition_matrix(self):
        """Copy the map, apply scoring-only curve conventions, and mask Area 4."""
        matrix = [row[:] for row in self.report_matrix]
        meta = self._matrix_meta
        if not meta:
            return matrix
        # Scoring conventions apply only to this detached submission copy.
        # Physical straight walls and curve endpoints remain occupied internally.
        for r,c in self._curve_bends:
            if 0 <= r < len(matrix) and 0 <= c < len(matrix[r]):
                matrix[r][c] = "0"
        writes = defaultdict(list)
        for bx,by,sx,sy in self._confirmed_curve_quadrants:
            cx,cy = (bx+sx/2)*self.QUARTER_MM, (by+sy/2)*self.QUARTER_MM
            tile = self._tile_of(cx,cy)
            quarter = int(cx>tile[0]*self.TILE_MM)+2*int(cy<tile[1]*self.TILE_MM)
            writes[(bx,by)].append((tile,quarter,"0"))
            for endpoint in ((bx+sx,by),(bx,by+sy)):
                writes[endpoint].append((tile,quarter,"1"))
        # Official creator: x columns first, then z rows. Shared straight walls
        # are owned by the west/north tile; interior walls belong to their tile.
        # Straight stamps precede its four curve quarters. Apply this convention
        # only in this copy, never to measured material or navigation.
        export_straights=set(self._confirmed_straight_walls)
        # A curve can share its nominal bend with an independently observed
        # straight arm. The curve template clears that cell even when the
        # opposite arm's two30mm samples have measured axis-plane support.
        # Recover only this submission ownership, never physical occupancy.
        dx,dy=self.last_grid_offset
        for bx,by,sx,sy in self._confirmed_curve_quadrants:
            for ox,oy,signature in ((-sx,0,self._horizontal_surface_hits),
                                     (0,-sy,self._vertical_surface_hits)):
                supported=True
                for distance in (self.QUARTER_MM/2,self.QUARTER_MM):
                    wx,wy=bx*self.QUARTER_MM+ox*distance,by*self.QUARTER_MM+oy*distance
                    gx=round((wx+dx)/self.GRID_MM)+self.c;gy=round((wy+dy)/self.GRID_MM)+self.c
                    if not (1<=gx<self.n-1 and 1<=gy<self.n-1):supported=False;break
                    crop=np.s_[gy-1:gy+2,gx-1:gx+2]
                    evidence=(signature[crop]>0)&(self.logodds[crop]>=self.OCC_T)
                    if self._confirmed_obstacle_mask is not None:
                        evidence&=~self._confirmed_obstacle_mask[crop]
                    if not np.any(evidence):
                        supported=False;break
                if supported:
                    a=(bx*self.QUARTER_MM,by*self.QUARTER_MM)
                    b=((bx+ox)*self.QUARTER_MM,(by+oy)*self.QUARTER_MM)
                    a,b=sorted((a,b));export_straights.add((*a,*b))
        for ax,ay,bx,by in export_straights:
            if ax == bx:
                tile = (round((ax-60 if ax % 120 else ax)/120),
                        round((ay+by)/240))
            else:
                tile = (round((ax+bx)/240),
                        round((ay+60 if ay % 120 else ay)/120))
            # This is the earliest possible owner. At an outer west/north
            # boundary, the owner moves east/south and is later, so a winning
            # straight1 stays1. Do not infer a boundary from the observed crop;
            # an otherwise ambiguous0 retains the curve convention.
            for endpoint in ((round(ax/60),round(ay/60)),
                             (round(bx/60),round(by/60))):
                writes[endpoint].append((tile,-1,"1"))
        for r,c in self._curve_bends:
            key = self._vertex_key(meta['x0_mm']+c*meta['cell_mm'],
                                   meta['y1_mm']-r*meta['cell_mm'])
            values = writes[key]
            if values:
                matrix[r][c] = max(values,key=lambda v:(v[0][0],-v[0][1],v[1]))[2]
        token_map = getattr(self, 'token_map', None)
        if token_map is not None:
            token_map.stamp(matrix)
        area4 = self._competition_area4_tiles()
        if area4:
            # A curved border can identify a logical tile beyond the physical
            # report crop. Retain its entire 5x5 footprint in this copy only.
            min_tx = min(meta['min_tx'], min(x for x, y in area4))
            max_tx = max(meta['max_tx'], max(x for x, y in area4))
            min_ty = min(meta['min_ty'], min(y for x, y in area4))
            max_ty = max(meta['max_ty'], max(y for x, y in area4))
            left, top = 4*(meta['min_tx']-min_tx), 4*(max_ty-meta['max_ty'])
            if (min_tx, max_tx, min_ty, max_ty) != (meta['min_tx'], meta['max_tx'], meta['min_ty'], meta['max_ty']):
                expanded = [['0']*(4*(max_tx-min_tx+1)+1) for _ in range(4*(max_ty-min_ty+1)+1)]
                for r, row in enumerate(matrix):
                    expanded[r+top][left:left+len(row)] = row
                matrix = expanded
        for tx, ty in area4:
            br, bc = 4*(max_ty-ty), 4*(tx-min_tx)
            for dr in range(5):
                for dc in range(5):
                    r, c = br+dr, bc+dc
                    if 0 <= r < len(matrix) and 0 <= c < len(matrix[r]):
                        matrix[r][c] = '*'
        return matrix

    def _crop_matrix_to_wall_bounds(self, matrix):
        """Crop empty padding while retaining start and every mapped feature.

        Partial scans need not enclose the start with walls. Wall-only bounds
        could remove the5 markers the supervisor requires for alignment.
        """
        if not matrix:
            return matrix
        rows = len(matrix)
        cols = len(matrix[0]) if matrix else 0
        features = [(r, c) for r in range(rows) for c in range(cols)
                    if matrix[r][c] != "0"]
        if not features:
            return matrix
        min_r = min(r for r, _ in features)
        max_r = max(r for r, _ in features)
        min_c = min(c for _, c in features)
        max_c = max(c for _, c in features)
        return [row[min_c:max_c + 1] for row in matrix[min_r:max_r + 1]]

    def competition_map_packet(self):
        """Supervisor Robot.py protocol: two native int32 dimensions then CSV.

        Pure formatting, available with DEBUG=False; the caller decides when
        to send these bytes through the robot emitter. Never writes a file.
        """
        matrix = self.competition_matrix()
        if not matrix or not matrix[0] or any(len(row) != len(matrix[0]) for row in matrix):
            raise ValueError("Competition map must be nonempty and rectangular")
        if not any("5" in row for row in matrix):
            raise ValueError("Competition map requires the start tile ('5')")
        matrix = self._crop_matrix_to_wall_bounds(matrix)
        # MapScorer pads only3x the submitted shape before alignment. Small
        # partial maps can otherwise produce a truncated slice and score0.
        # Cover this controller's supported tile extent with harmless zeros;
        # this is packet-only padding, never measured or inferred geometry.
        minimum_side = 4 * (2*math.ceil(self.HALF_WORLD_MM/self.TILE_MM)+1)+1
        padding = ((0,max(0,minimum_side-len(matrix))),
                   (0,max(0,minimum_side-len(matrix[0]))))
        matrix = np.pad(np.asarray(matrix,dtype=str),padding,constant_values='0')
        return struct.pack('2i', *matrix.shape) + ','.join(matrix.ravel()).encode('utf-8')

    def format_emitter_matrix_text(self, title="Captured map matrix."):
        """Competition matrix as an importable Python MATRIX list.

        Format matches Awesome world-2_correct_matrix.py: docstring, MATRIX = [
        one indented row per line, spaces after commas. Empty padding is cropped.
        """
        matrix = self._crop_matrix_to_wall_bounds(self.competition_matrix())
        if not matrix:
            return f'"""{title}"""\n\nMATRIX = []\n'
        row_lines = []
        for row in matrix:
            cells = ", ".join(repr(cell) for cell in row)
            row_lines.append(f"    [{cells}],")
        body = "\n".join(row_lines)
        return f'"""{title}"""\n\nMATRIX = [\n{body}\n]\n'

    def save_emitter_matrix_txt(self, path, title="Captured map matrix."):
        # DEBUG ONLY: disabling file output must not disable matrix formatting.
        if not self.DEBUG_MAPPING_DUMP:
            return None
        with open(path, "w", encoding="utf-8") as f:
            f.write(self.format_emitter_matrix_text(title=title))
        return path

    # ------------------------------------------------------------------ #
    #  Debug dump
    # ------------------------------------------------------------------ #
    def _update_debug_snapshot(self):
        if not self.DEBUG_MAPPING_DUMP:
            return self.debug_snapshot
        ys, xs = np.where(self.logodds >= self.OCC_T)
        if xs.size:
            bbox = [int(xs.min() - self.c), int(ys.min() - self.c),
                    int(xs.max() - self.c), int(ys.max() - self.c)]
        else:
            bbox = None

        self.debug_snapshot = {
            "origin_mm": [self.origin_x, self.origin_y],
            "gps_ref_mm": list(self._gps_ref_mm) if self._gps_ref_mm else None,
            "start_yaw_deg": self._start_yaw,
            "yaw_offset_deg": round(self._yaw_offset_deg, 3),
            "occupied_cells": int(xs.size),
            "free_cells": int(np.count_nonzero(self.logodds <= self.FREE_T)),
            "occupied_bbox_cells": bbox,
            "visited_tiles": sorted(map(list, self.visited_tiles)),
            "current_room": self._current_room,
            "area4_tiles": sorted(map(list, self._area4_tiles)),
            "competition_area4_tiles": sorted(map(list, self._competition_area4_tiles())),
            "room_transitions": self._room_transitions,
            "vertical_walls": sorted(map(list, self._v_walls)),
            "horizontal_walls": sorted(map(list, self._h_walls)),
            "matrix_rows": len(self.report_matrix),
            "matrix_cols": len(self.report_matrix[0]) if self.report_matrix else 0,
            "grid_lock_offset_mm": [round(self.last_grid_offset[0], 2),
                                    round(self.last_grid_offset[1], 2)],
            "curve_bends": sorted(map(list, getattr(self, "_curve_bends", set()))),
            "persistent_curve_bends": len(getattr(self, "_persistent_curve_bends", set())),
            "confirmed_curve_quadrants": sorted(map(list, self._confirmed_curve_quadrants)),
            "scan_curve_votes": [list(k)+[v] for k,v in sorted(self._scan_curve_votes.items())],
            "native_curve_samples": self._native_curve_samples,
            "confirmed_straight_walls": sorted(map(list, self._confirmed_straight_walls)),
            "arc_evidence_cells": int(np.count_nonzero(self.arc_grid)),
            "lidar_debug": self.last_lidar_debug,
        }
        return self.debug_snapshot

    def save_gap_debug_dump(self, path=None):
        if not self.DEBUG_MAPPING_DUMP:
            return None
        path = path or self.DEBUG_GAP_DUMP_FILE
        with open(path, "w", encoding="utf-8") as f:
            json.dump(self.debug_snapshot, f, indent=2)
        return path

    def save_raw_debug(self, npz_path, stages_path=None):
        """Dump everything needed to re-run the matrix pipeline OFFLINE.

        The PNG is lossy; this saves the exact integer log-odds grid (cropped to
        the known region), the visited mask, the most recent raw LiDAR scan +
        pose, the full matrix<->world mapping, and per-stage matrix snapshots
        (raster/denoise/curve/closure/odd_guard). With this, the curve and
        denoise passes can be analysed and retuned without guessing from images.

        Saves a compressed .npz (grids + metadata) and a JSON of stage matrices.
        """
        if not self.DEBUG_MAPPING_DUMP:
            return None
        known = (self.logodds >= self.OCC_T) | (self.logodds <= self.FREE_T) | self.visited
        ys, xs = np.where(known)
        if xs.size:
            pad = 4
            gx0 = max(0, int(xs.min()) - pad); gx1 = min(self.n - 1, int(xs.max()) + pad)
            gy0 = max(0, int(ys.min()) - pad); gy1 = min(self.n - 1, int(ys.max()) + pad)
        else:
            gx0 = gy0 = 0; gx1 = gy1 = self.n - 1

        meta = {
            "GRID_MM": self.GRID_MM, "TILE_MM": self.TILE_MM, "QUARTER_MM": self.QUARTER_MM,
            "OCC_T": self.OCC_T, "FREE_T": self.FREE_T,
            "arc_band_mm": self.ARC_BAND_MM,
            "arc_convex_band_mm": self.ARC_CONVEX_BAND_MM,
            "n": self.n, "center_idx": self.c,
            "crop_gx0": gx0, "crop_gy0": gy0, "crop_gx1": gx1, "crop_gy1": gy1,
            "origin_mm": [self.origin_x, self.origin_y],
            "start_tile": list(self.start_tile),
            "grid_lock_offset_mm": list(self.last_grid_offset),
            "persistent_curve_bends": [list(k) for k in sorted(self._persistent_curve_bends)],
            "confirmed_curve_quadrants": [list(k) for k in sorted(self._confirmed_curve_quadrants)],
            "scan_curve_votes": [list(k)+[v] for k,v in sorted(self._scan_curve_votes.items())],
            "native_curve_samples": self._native_curve_samples,
            "confirmed_straight_walls": [list(k) for k in sorted(self._confirmed_straight_walls)],
            "closure_enabled": self.closure_enabled,
            "matrix_meta": self._matrix_meta,
            "internal_matrix": self.report_matrix,
            "last_pose": self._last_pose or {},
            "persistent_obstacle_tiles": [list(t) for t in self._persistent_obstacle_tiles],
            "pending_nonwall_cells": sorted(self._pending_nonwall_cells),
            "raw_obstacle_cells": [[*tile,sorted(cells)] for tile,cells in sorted(self._raw_obstacle_cells.items())],
            "raw_obstacle_regions": [[*tile,sorted(cells)] for tile,cells in sorted(self._raw_obstacle_regions.items())],
            "raw_curve_wall_cells": sorted(self._raw_curve_wall_cells),
            "curve_raster_decisions": self.last_curve_raster_debug,
            "obstacle_model_version": 2,
            "cam_obstacle_tiles": [list(t) for t in sorted(self._cam_obstacle_tiles)],
            "cam_obstacle_votes": {f"{tx},{ty}": v
                                   for (tx, ty), v in sorted(self._obst_votes.items())},
            "cam_wall_ref": {
                "n": int(self._wallref_n),
                "Hw": (round(math.degrees(math.atan2(self._wallref_cy, self._wallref_cx)) % 360, 1)
                       if self._wallref_n else None),
                "Sw": (round(math.hypot(self._wallref_cx, self._wallref_cy) / self._wallref_n, 3)
                       if self._wallref_n else None),
            },
            "last_cam_fusion": self.last_cam_fusion,
            "committed_floor_tiles": {f"{tx},{ty}": ch
                                      for (tx, ty), ch in sorted(self._committed_floor_tiles.items())},
            "floor_commit_distances_mm": {f"{tx},{ty}": distance
                                           for (tx, ty), distance in self._cs_commit_dist.items()},
            "current_room": self._current_room,
            "room_tiles": {f"{tx},{ty}": room for (tx, ty), room in self._room_tiles.items()},
            "nonpassage_floor_tiles": sorted(map(list, self._nonpassage_floor_tiles)),
            "room_topology_version": 1 if self._room_model_signature is not None else 0,
            "area4_tiles": sorted(map(list, self._area4_tiles)),
            "competition_area4_tiles": sorted(map(list, self._competition_area4_tiles())),
            "room_transitions": self._room_transitions,
            "floor_votes": {f"{tx},{ty}": v
                            for (tx, ty), v in sorted(self._floor_votes.items())},
            "last_floor_fusion": self.last_floor_fusion,
            "last_color_sensor_fusion": self.last_color_sensor_fusion,
            "provisional_holes": [list(t) for t in sorted(self._provisional_holes)],
        }
        pcells = (np.array(sorted(self._persistent_obstacle_cells), dtype=np.int32)
                  if self._persistent_obstacle_cells else np.zeros((0, 2), dtype=np.int32))
        np.savez_compressed(
            npz_path,
            logodds=self.logodds[gy0:gy1 + 1, gx0:gx1 + 1].astype(np.int16),
            visited=self.visited[gy0:gy1 + 1, gx0:gx1 + 1],
            arc_grid=self.arc_grid[gy0:gy1 + 1, gx0:gx1 + 1].astype(np.int32),
            convex_arc_grid=self.convex_arc_grid[gy0:gy1 + 1, gx0:gx1 + 1].astype(np.int32),
            horizontal_surface_hits=self._horizontal_surface_hits[gy0:gy1 + 1, gx0:gx1 + 1],
            vertical_surface_hits=self._vertical_surface_hits[gy0:gy1 + 1, gx0:gx1 + 1],
            last_scan=(self._last_scan if self._last_scan is not None
                       else np.zeros(0, dtype=np.float32)),
            persistent_obstacle_cells=pcells,
            meta_json=np.array(json.dumps(meta)),
            **({'physical_logodds': self._physical_evidence.logodds,
                'physical_grid_mm': np.array(self._physical_evidence.GRID_MM),
                'physical_phase_mm': np.array(self._physical_evidence.phase_mm),
                'physical_template_clear': np.array(sorted(self._physical_evidence.template_clear), dtype=np.int64),
                'physical_coarse_guard': np.array(sorted(self._physical_coarse_guard), dtype=np.int32).reshape(-1, 2)}
               if self._physical_evidence is not None else {}),
        )

        if stages_path is not None:
            with open(stages_path, "w", encoding="utf-8") as f:
                json.dump({
                    "matrix_meta": self._matrix_meta,
                    "stages": self._stage_snapshots,
                }, f)
        return npz_path

    # ------------------------------------------------------------------ #
    #  Visualisation
    # ------------------------------------------------------------------ #
    def _map_figure_inches(self, image, px_per_cell):
        """Figure size that keeps a fixed pixel size per map cell."""
        h, w = image.shape[:2]
        px_w = min(max(w * px_per_cell, self.MAP_FIG_MIN_W_PX), self.MAP_FIG_MAX_W_PX)
        px_h = min(max(h * px_per_cell, self.MAP_FIG_MIN_H_PX), self.MAP_FIG_MAX_H_PX)
        dpi = self.MAP_FIG_DPI
        return px_w / dpi, px_h / dpi

    def _map_viewport_cells(self, image, px_per_cell):
        """How many image cells fit in the capped figure window."""
        h, w = image.shape[:2]
        vw = min(w, self.MAP_FIG_MAX_W_PX / px_per_cell)
        vh = min(h, self.MAP_FIG_MAX_H_PX / px_per_cell)
        return w, h, vw, vh

    def _default_map_limits(self, image, px_per_cell):
        """Full image, or a centered viewport when the map exceeds the window."""
        w, h, vw, vh = self._map_viewport_cells(image, px_per_cell)
        if vw >= w and vh >= h:
            return 0.0, float(w), float(h), 0.0
        x0 = max(0.0, (w - vw) * 0.5)
        y0 = max(0.0, (h - vh) * 0.5)
        return x0, x0 + vw, y0 + vh, y0

    def _apply_map_image(self, ax, im, image):
        im.set_data(image)
        h, w = image.shape[:2]
        im.set_extent((0.0, float(w), float(h), 0.0))

    def _layout_map_axes(self, fig, ax, image, px_per_cell):
        fig.set_dpi(self.MAP_FIG_DPI)
        fig.set_size_inches(*self._map_figure_inches(image, px_per_cell))
        ax.set_aspect("equal", adjustable="box")
        fig.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02)

    def _set_map_limits(self, ax, x0, x1, y0, y1):
        ax.set_xlim(x0, x1)
        ax.set_ylim(y1, y0)   # image coords: row 0 = North / top

    def _scroll_zoom_map(self, ax, event):
        """Mouse wheel: zoom. Shift + wheel: pan horizontally. Ctrl + wheel: pan vertically."""
        if event.inaxes != ax or event.xdata is None or event.ydata is None:
            return
        step = getattr(event, "step", 0)
        if not step:
            return

        key = (event.key or "").lower()
        pan = 18.0 * step
        x0, x1 = ax.get_xlim()
        y_bottom, y_top = ax.get_ylim()   # imshow: bottom > top numerically

        if key == "shift":
            self._set_map_limits(ax, x0 - pan, x1 - pan, y_top, y_bottom)
            return
        if key in ("control", "ctrl"):
            self._set_map_limits(ax, x0, x1, y_top - pan, y_bottom - pan)
            return

        scale = self.MAP_ZOOM_SCROLL ** (-step)
        xdata, ydata = event.xdata, event.ydata
        new_w = (x1 - x0) * scale
        new_h = (y_bottom - y_top) * scale
        relx = (xdata - x0) / (x1 - x0) if x1 != x0 else 0.5
        rely = (ydata - y_top) / (y_bottom - y_top) if y_bottom != y_top else 0.5
        nx0 = xdata - new_w * relx
        nx1 = xdata + new_w * (1.0 - relx)
        ny_top = ydata - new_h * rely
        ny_bottom = ydata + new_h * (1.0 - rely)
        self._set_map_limits(ax, nx0, nx1, ny_top, ny_bottom)

    def _attach_map_view_tools(self, fig, ax, view_key):
        """Scroll zoom/pan, keyboard reset, and matplotlib navigation toolbar."""
        flag = f"_map_{view_key}_user_view"

        def mark_user_view(_event=None):
            setattr(self, flag, True)

        def on_scroll(event):
            if event.inaxes != ax:
                return
            self._scroll_zoom_map(ax, event)
            mark_user_view()

        def on_key(event):
            if event.key != "r":
                return
            image = self._highres_image() if view_key == "hi" else self._report_image()
            x0, x1, y0, y1 = self._default_map_limits(image,
                self.MAP_HI_PX_PER_CELL if view_key == "hi" else self.MAP_MATRIX_PX_PER_CELL)
            self._set_map_limits(ax, x0, x1, y0, y1)
            setattr(self, flag, False)
            fig.canvas.draw_idle()

        fig.canvas.mpl_connect("scroll_event", on_scroll)
        fig.canvas.mpl_connect("button_release_event", mark_user_view)
        fig.canvas.mpl_connect("key_press_event", on_key)

        try:
            backend = plt.get_backend().lower()
            if "qt" in backend:
                from matplotlib.backends.backend_qtagg import NavigationToolbar2QT as Toolbar
                toolbar = Toolbar(fig.canvas, fig.canvas.manager.window)
                fig.canvas.manager.window.addToolBar(toolbar)
            elif "tk" in backend:
                import tkinter as tk
                from matplotlib.backends.backend_tkagg import NavigationToolbar2Tk as Toolbar
                win = fig.canvas.manager.window
                frame = tk.Frame(win)
                frame.pack(side=tk.BOTTOM, fill=tk.X)
                Toolbar(fig.canvas, frame)
        except Exception:
            pass

    def _create_map_view(self, title, image, px_per_cell, view_key):
        fig, ax = plt.subplots(num=title)
        im = ax.imshow(image, interpolation="nearest", aspect="equal")
        ax.axis("off")
        self._layout_map_axes(fig, ax, image, px_per_cell)
        x0, x1, y0, y1 = self._default_map_limits(image, px_per_cell)
        self._set_map_limits(ax, x0, x1, y0, y1)
        self._attach_map_view_tools(fig, ax, view_key)
        fig.canvas.draw_idle()
        return fig, ax, im

    def _refresh_map_view(self, fig, ax, im, image, px_per_cell, view_key):
        user_view = getattr(self, f"_map_{view_key}_user_view", False)
        self._apply_map_image(ax, im, image)
        self._layout_map_axes(fig, ax, image, px_per_cell)
        if not user_view:
            x0, x1, y0, y1 = self._default_map_limits(image, px_per_cell)
            self._set_map_limits(ax, x0, x1, y0, y1)
        fig.canvas.draw_idle()

    def _highres_image(self):
        known = (self.logodds >= self.OCC_T) | (self.logodds <= self.FREE_T) | self.visited
        ys, xs = np.where(known)
        if xs.size == 0:
            return np.full((20, 20, 3), 0.30, dtype=np.float32)

        pad = 3
        x0, x1 = max(0, xs.min() - pad), min(self.n - 1, xs.max() + pad)
        y0, y1 = max(0, ys.min() - pad), min(self.n - 1, ys.max() + pad)

        sub = self.logodds[y0:y1 + 1, x0:x1 + 1]
        vis = self.visited[y0:y1 + 1, x0:x1 + 1]

        img = np.full(sub.shape + (3,), 0.30, dtype=np.float32)     # unknown
        img[sub <= self.FREE_T] = (0.82, 0.82, 0.82)                # free
        img[vis] = (0.55, 0.80, 0.55)                               # visited
        # special coloured floor tiles: stamp the committed char over the tile's
        # 120 mm cell interior (same start-anchored grid as the matrix), so the
        # high-res navigation map also carries hole/swamp/checkpoint/connection
        # tiles. Drawn before walls/obstacles so those still show on top.
        if self.FLOOR_FUSE and self._committed_floor_tiles:
            per = self.TILE_MM // self.GRID_MM              # 12 cells per tile
            half = per // 2 - 1                             # interior half-span (avoid wall lines)
            for (tx, ty), ch in self.mapped_floor_tiles().items():
                color = self.FLOOR_HIRES_PALETTE.get(ch)
                if color is None:
                    continue
                ccx = tx * per + self.c - x0
                ccy = ty * per + self.c - y0
                r0 = max(0, ccy - half); r1 = min(sub.shape[0], ccy + half + 1)
                c0 = max(0, ccx - half); c1 = min(sub.shape[1], ccx + half + 1)
                if r1 > r0 and c1 > c0:
                    img[r0:r1, c0:c1] = color
        img[sub >= self.OCC_T] = (0.10, 0.05, 0.25)                 # walls
        # obstacle shapes tinted distinctly so they can be navigated around
        if self._confirmed_obstacle_mask is not None:
            om = self._confirmed_obstacle_mask[y0:y1 + 1, x0:x1 + 1]
            img[om] = (0.95, 0.60, 0.15)
        return img[::-1]                                            # +Y upward

    def _report_image(self):
        matrix = self.report_matrix
        rows = len(matrix)
        cols = len(matrix[0]) if rows else 1
        cell = 16
        img = np.full((rows * cell, cols * cell, 3), 0.95, dtype=np.float32)

        palette = {
            "1": (0.10, 0.05, 0.25),   # wall
            "5": (0.20, 0.70, 0.30),   # start
            "x": (0.95, 0.60, 0.15),   # obstacle
            "2": (0.10, 0.10, 0.10),   # hole
            "3": (0.85, 0.75, 0.20),   # swamp
            "4": (0.30, 0.55, 0.95),   # checkpoint
            "b": (0.10, 0.10, 0.90),
            "y": (0.95, 0.85, 0.10),
            "g": (0.10, 0.80, 0.10),
            "p": (0.50, 0.10, 0.75),
            "o": (0.95, 0.60, 0.10),
            "r": (0.90, 0.10, 0.10),
            "*": (0.60, 0.60, 0.60),
        }

        for r in range(rows):
            for c in range(cols):
                token = matrix[r][c]
                if token != "0":
                    color = palette.get(token, (0.10, 0.75, 0.75))
                    img[r * cell:(r + 1) * cell, c * cell:(c + 1) * cell] = color
                img[r * cell, c * cell:(c + 1) * cell] = (0.6, 0.85, 0.6)
                img[r * cell:(r + 1) * cell, c * cell] = (0.6, 0.85, 0.6)
        return img

    def draw_high_resolution_map(self):
        image = self._highres_image()
        if self.fig_hi is None:
            self.fig_hi, self.ax_hi, self.img_hi = self._create_map_view(
                "High Resolution Occupancy Map", image,
                self.MAP_HI_PX_PER_CELL, "hi")
        else:
            self._refresh_map_view(
                self.fig_hi, self.ax_hi, self.img_hi, image,
                self.MAP_HI_PX_PER_CELL, "hi")
        plt.pause(0.001)

    def draw_competition_matrix_map(self):
        image = self._report_image()
        if self.fig_rep is None:
            self.fig_rep, self.ax_rep, self.img_rep = self._create_map_view(
                "Competition Matrix Map", image,
                self.MAP_MATRIX_PX_PER_CELL, "rep")
        else:
            self._refresh_map_view(
                self.fig_rep, self.ax_rep, self.img_rep, image,
                self.MAP_MATRIX_PX_PER_CELL, "rep")
        plt.pause(0.001)

    def _save_rgb_png(self, path, image):
        # DEBUG ONLY: explicit PNG capture; independent of live map visibility.
        if not self.DEBUG_MAPPING_DUMP:
            return None
        arr = np.clip(image * 255.0, 0, 255).astype(np.uint8)
        plt.imsave(path, arr)

    def save_map_images(self, high_res_path=None, matrix_path=None):
        if not self.DEBUG_MAPPING_DUMP or not self.SAVE_MAP_PNG:
            return None, None
        hi_path = high_res_path or self.HIGH_RES_MAP_PNG
        mat_path = matrix_path or self.MATRIX_MAP_PNG
        self._save_rgb_png(hi_path, self._highres_image())
        self._save_rgb_png(mat_path, self._report_image())
        return hi_path, mat_path

    # ------------------------------------------------------------------ #
    #  Status
    # ------------------------------------------------------------------ #
    def summary(self):
        return {
            "wall_hits": self.wall_hits,
            "occupied_cells": int(np.count_nonzero(self.logodds >= self.OCC_T)),
            "free_cells": int(np.count_nonzero(self.logodds <= self.FREE_T)),
            "visited_tiles": len(self.visited_tiles),
            "confirmed_walls": len(self._v_walls) + len(self._h_walls),
            "grid_lock_offset_mm": [round(self.last_grid_offset[0], 2),
                                    round(self.last_grid_offset[1], 2)],
            "curve_bends": len(getattr(self, "_curve_bends", set())),
            "obstacles": len(getattr(self, "obstacle_tiles", set())),
            "obstacle_tiles": sorted(map(list, getattr(self, "obstacle_tiles", set()))),
            "report_rows": len(self.report_matrix),
            "report_cols": len(self.report_matrix[0]) if self.report_matrix else 0,
            "json_path": self.JSON_PATH,
            "gap_debug_file": self.DEBUG_GAP_DUMP_FILE,
            "highres_png": self.HIGH_RES_MAP_PNG,
            "matrix_png": self.MATRIX_MAP_PNG,
        }


class RelayOccupancyMapper(OccupancyMapper):
    """SuperTeam is Area 1; station colours describe slots, not passages."""
    @classmethod
    def classify_color_sensor(cls, r, g, b):
        token = super().classify_color_sensor(r, g, b)
        return token if token == '3' else None

    @classmethod
    def classify_floor_rgb(cls, r, g, b):
        token = super().classify_floor_rgb(r, g, b)
        return token if token == '3' else None

    def _track_room_crossing(self, tile, now):
        self._current_room = 1
        self._room_tiles[tile] = 1
        self._room_pending_passage = None
        self._room_last_tile = tile


class PhysicalEvidenceGrid:
    """Measured physical material; canonical10mm semantics stay independent.

    Explicit qualified floating GPS/device packets support1mm material cells.
    Legacy packets retain the original2mm geometry. Inferred straight-template
    pieces may be omitted only after the existing full-footprint inverse model
    observes clear space there; raw hits and startup guards are never omitted.
    """
    _clear_visible_cells = OccupancyMapper._clear_visible_cells
    PRECISE_GRID_MM = 1

    @staticmethod
    def _pose_is_precise(data):
        position = data.get('position', {})
        return (position.get('map_physical_pose_ready', position.get('map_pose_ready', False))
            and position.get('mapping_reference') == 'gps_device_xz_mm'
            and all(isinstance(position.get(k), (int, float, np.number))
                and np.isfinite(position[k]) for k in ('map_x_float', 'map_y_float')))

    @staticmethod
    def _registration_allowance(data):
        # The allowance is a declared probabilistic policy for the qualified
        # transported legal GPS mean, not an absolute Gaussian-noise bound.
        # Missing/invalid quality cannot authorize removal of positive cells.
        radius = data.get('position', {}).get('map_registration_allowance_mm')
        samples = data.get('position', {}).get('map_position_samples', MappingGPSFilter.WINDOW)
        if (PhysicalEvidenceGrid._pose_is_precise(data)
                and isinstance(samples, (int, np.integer))
                and MappingGPSFilter.WINDOW <= samples <= MappingGPSFilter.PHYSICAL_WINDOW
                and isinstance(radius, (int, float, np.number)) and np.isfinite(radius)
                and radius >= MappingGPSFilter.REGISTRATION_SIGMAS*(MappingGPSFilter.RAW_GPS_SIGMA_MM/math.sqrt(samples))):
            return float(radius)
        return math.inf

    def __init__(self, mapper, data=None, grid_mm=None, phase_mm=None):
        precise = self._pose_is_precise(data or {})
        self.GRID_MM = (self.PRECISE_GRID_MM if precise else 2) if grid_mm is None else grid_mm
        self.phase_mm = (0.5 if self.GRID_MM == 1 else 0.0) if phase_mm is None else phase_mm
        self.precise = self.GRID_MM == 1 and self.phase_mm == 0.5
        self.c = round(mapper.c*mapper.GRID_MM/self.GRID_MM)
        self.n = 2*self.c+1
        self.logodds = np.zeros((self.n, self.n), dtype=np.int16)
        for name in ('L_OCC', 'L_FREE', 'L_CLAMP', 'OCC_T', 'FREE_T'):
            setattr(self, name, getattr(mapper, name))
        self.DEBUG_MAPPING_DUMP = False
        self.version = 0
        self.template_clear = set()
        self._template_models = None
        self._template_points = np.empty((0, 2))
        self._template_keys = np.empty(0, dtype=np.int64)
        self._proof = None
        self._occupied_keys = set() if self.precise else None
        self._fine_material_version = -1
        self._fine_material_points = None

    def rebuild_material_index(self):
        """Explicit restoration/test hook after externally loading logodds."""
        if self.precise:
            self._occupied_keys = set(map(int, np.flatnonzero(self.logodds.ravel() >= self.OCC_T)))
        self.version += 1
        self._fine_material_version = -1

    def occupied_centres(self):
        if self._occupied_keys is None:
            kernel = getattr(NavigationMapper, '_compiled_occupied_centres', None)
            if kernel is not None:
                return kernel(self.logodds, self.OCC_T, self.c, self.GRID_MM, self.phase_mm)
            y, x = np.nonzero(self.logodds >= self.OCC_T)
            return np.column_stack(((x-self.c)*self.GRID_MM+self.phase_mm,
                                    (y-self.c)*self.GRID_MM+self.phase_mm))
        if self._fine_material_version != self.version:
            keys = np.sort(np.fromiter(self._occupied_keys, dtype=np.int64))
            self._fine_material_points = np.column_stack((
                (keys%self.n-self.c)*self.GRID_MM+self.phase_mm,
                (keys//self.n-self.c)*self.GRID_MM+self.phase_mm))
            self._fine_material_points.flags.writeable = False
            self._fine_material_version = self.version
        return self._fine_material_points

    def _update_templates(self, mapper):
        models = frozenset(mapper._confirmed_straight_walls)
        if models == self._template_models:
            return
        self._template_models = models
        mask = mapper._straight_wall_mask(models)
        self._template_points = mapper._physical_subdivide(mask, self.GRID_MM)
        cells = np.rint((self._template_points-self.phase_mm)/self.GRID_MM).astype(int)+self.c
        inside = np.all((cells >= 0)&(cells < self.n), axis=1)
        self._template_keys = np.where(inside, cells[:,1]*self.n+cells[:,0], -1)

    @staticmethod
    def _template_membership(keys, cleared):
        values = np.fromiter(cleared, dtype=np.int64)
        # Integer lookup avoids repeatedly sorting the growing wall-sample
        # population. Bound temporary lookup memory to8MiB; older NumPy and
        # wider fields retain the original exact membership calculation.
        if values.size and int(values.max())-int(values.min()) < 8*1024*1024:
            try:
                return np.isin(keys, values, kind='table')
            except TypeError:
                pass  # NumPy before1.24 has no algorithm-selection keyword.
        return np.isin(keys, values)

    def template_material(self, mapper):
        self._update_templates(mapper)
        keep = ((self._template_keys < 0)
                | ~self._template_membership(self._template_keys, self.template_clear))
        return self._template_points[keep]

    def _certify_templates(self, mapper, laser, rays, dist, hit, clear, hit_keys, registration_mm):
        # Any new measured hit revokes an earlier clear-template certificate,
        # even before the separate measured grid reaches its occupied threshold.
        self.template_clear.difference_update(map(int, hit_keys))
        self._update_templates(mapper)
        if not len(self._template_points):
            return
        nearby = ((self._template_keys >= 0)
            & (np.linalg.norm(self._template_points-laser, axis=1) <= mapper.MAX_RANGE_MM-mapper.FREE_GAP_MM))
        if self.template_clear:
            indices = np.flatnonzero(nearby)
            nearby[indices] &= ~self._template_membership(self._template_keys[indices], self.template_clear)
        points, keys = self._template_points[nearby], self._template_keys[nearby]
        if not len(points):
            return
        if self._proof is None:
            proof = object.__new__(PhysicalEvidenceGrid)
            for name in ('GRID_MM','c','n','L_FREE','L_CLAMP','DEBUG_MAPPING_DUMP'):
                setattr(proof, name, getattr(self, name))
            # Sparse-touched positive scratch invokes the unchanged guarded
            # branch without ever editing the measured occupancy grid.
            proof.logodds = np.zeros((self.n, self.n), dtype=np.int8)
            self._proof = proof
        flat = self._proof.logodds.ravel()
        flat[keys] = self.OCC_T
        reach = np.maximum(dist-mapper.FREE_GAP_MM, 0)*clear
        try:
            self._proof._clear_visible_cells(*(laser-self.phase_mm), rays, reach,
                points[:,0]-self.phase_mm, points[:,1]-self.phase_mm,
                registration_mm=registration_mm)
            self.template_clear.update(map(int, keys[flat[keys] < self.OCC_T]))
        finally:
            flat[keys] = 0

    def integrate(self, mapper, rx, ry, yaw, data):
        precise_pose = self._pose_is_precise(data)
        position = data.get('position', {})
        original = data.get('lidar_raw_m')
        if precise_pose:
            rx = position['map_x_float']-mapper.origin_x
            ry = -position['map_y_float']-mapper.origin_y
        if precise_pose and original is not None and len(original) == len(data['lidar']):
            lidar = np.asarray(original, dtype=float)*1000
        else:
            lidar = np.asarray(data['lidar'], dtype=np.float32)
        h = int(data.get('lidar_hres', len(lidar)))
        layer = min(mapper.FRONT_INDEX//h, len(lidar)//h-1)
        indices = np.arange(0, h, mapper.BEAM_STEP)
        dist = lidar[layer*h+indices].copy()
        hit = np.isfinite(dist)&(dist >= mapper.MIN_RANGE_MM)&(dist <= mapper.MAX_RANGE_MM)
        clear = (dist >= mapper.MIN_RANGE_MM)&~np.isnan(dist)
        dist[~np.isfinite(dist)] = mapper.MAX_RANGE_MM
        dist = np.clip(dist, 0, float(mapper.MAX_RANGE_MM))
        if precise_pose:
            offset, rays = mapper._physical_lidar_projection(yaw, data, indices)
        else:
            offset, rays = mapper._lidar_projection(yaw, data, indices)
        laser = np.array([rx, ry])+offset
        grid_laser = laser-self.phase_mm
        reach = np.maximum(dist-mapper.FREE_GAP_MM, 0)
        ts = np.linspace(0, 1, mapper.FREE_SAMPLES).reshape(-1, 1)
        free_x = (grid_laser[0]+ts*reach[clear]*rays[clear,0]).ravel()
        free_y = (grid_laser[1]+ts*reach[clear]*rays[clear,1]).ravel()
        registration_mm = self._registration_allowance(data) if self.precise else 0.0
        self._clear_visible_cells(*grid_laser, rays, reach*clear, free_x, free_y,
            registration_mm=registration_mm)
        points = grid_laser+rays[hit]*dist[hit,None]
        cells = np.rint(points/self.GRID_MM).astype(int)+self.c
        cells = cells[np.all((cells >= 0)&(cells < self.n), axis=1)]
        keys, counts = np.unique(cells[:,1]*self.n+cells[:,0], return_counts=True)
        flat = self.logodds.ravel()
        flat[keys] = np.clip(flat[keys]+counts*self.L_OCC, -self.L_CLAMP, self.L_CLAMP)
        if self._occupied_keys is not None:
            free_cells = np.rint(np.column_stack((free_x,free_y))/self.GRID_MM).astype(int)+self.c
            free_cells = free_cells[np.all((free_cells >= 0)&(free_cells < self.n), axis=1)]
            touched = np.union1d(free_cells[:,1]*self.n+free_cells[:,0], keys)
            self._occupied_keys.difference_update(map(int, touched))
            self._occupied_keys.update(map(int, touched[flat[touched] >= self.OCC_T]))
        if self.precise and precise_pose:
            self._certify_templates(mapper, laser, rays, dist, hit, clear, keys, registration_mm)
        self.version += 1


class NavigationMapper:
    """30 mm debug navigation map built from the 10 mm OccupancyMapper grid.

    Step 1 only: 30x30 mm cells represent floor. Walls are kept separately as
    edge data between neighbouring cells, matching the Erebus matrix style.
    """

    # ------------------------------------------------------------------ #
    #  Geometry / data model
    # ------------------------------------------------------------------ #
    NAV_GRID_MM = 30
    HALF_WORLD_MM = OccupancyMapper.HALF_WORLD_MM
    EDGE_BAND_CELLS = 1
    EDGE_SEG_CELLS = 4
    EDGE_MIN_SPAN_CELLS = 3
    CURVE_STAIR_RADIUS_CELLS = 2

    FLOOR_NORMAL = 0
    FLOOR_HOLE = 2
    FLOOR_SWAMP = 3
    FLOOR_CHECKPOINT = 4
    FLOOR_START = 5
    FLOOR_PASSAGE_B = 10
    FLOOR_PASSAGE_Y = 11
    FLOOR_PASSAGE_G = 12
    FLOOR_PASSAGE_P = 13
    FLOOR_PASSAGE_O = 14
    FLOOR_PASSAGE_R = 15

    FLOOR_CHAR_TO_CODE = {
        "0": FLOOR_NORMAL,
        "2": FLOOR_HOLE,
        "3": FLOOR_SWAMP,
        "4": FLOOR_CHECKPOINT,
        "5": FLOOR_START,
        "b": FLOOR_PASSAGE_B,
        "y": FLOOR_PASSAGE_Y,
        "g": FLOOR_PASSAGE_G,
        "p": FLOOR_PASSAGE_P,
        "o": FLOOR_PASSAGE_O,
        "r": FLOOR_PASSAGE_R,
    }
    FLOOR_CODE_TO_CHAR = {v: k for k, v in FLOOR_CHAR_TO_CODE.items()}
    FLOOR_RGB = {
        FLOOR_NORMAL: (0.94, 0.94, 0.90),
        FLOOR_HOLE: (0.02, 0.02, 0.02),
        FLOOR_SWAMP: (0.45, 0.28, 0.12),
        FLOOR_CHECKPOINT: (0.72, 0.72, 0.70),
        FLOOR_START: (0.25, 0.85, 0.55),
        FLOOR_PASSAGE_B: (0.10, 0.25, 0.95),
        FLOOR_PASSAGE_Y: (0.95, 0.88, 0.08),
        FLOOR_PASSAGE_G: (0.05, 0.65, 0.18),
        FLOOR_PASSAGE_P: (0.55, 0.18, 0.80),
        FLOOR_PASSAGE_O: (0.95, 0.48, 0.05),
        FLOOR_PASSAGE_R: (0.90, 0.08, 0.08),
    }

    CELL_UNKNOWN = 0
    CELL_UNVISITED = 1
    CELL_VISITED = 2
    CELL_STATE_TO_CHAR = {
        CELL_UNKNOWN: "?",
        CELL_UNVISITED: "u",
        CELL_VISITED: "v",
    }
    CELL_STATE_RGB = {
        CELL_UNKNOWN: (0.55, 0.55, 0.55),
        CELL_UNVISITED: (0.78, 0.88, 0.96),
        CELL_VISITED: (0.62, 0.86, 0.64),
    }
    CURRENT_BEAM_RGB = (0.05, 0.95, 0.95)
    DISCOVER_FREE_MIN_CELLS = 1
    ROBOT_BEAM_OPEN_SIDE_CELLS = 2
    ROBOT_BEAM_WALL_SIDE_CELLS = 3
    ROBOT_BEAM_LENGTH_CELLS = 2
    PATH_REPLAN_PERIOD_S = 0.25
    PATH_FINE_GRID_MM = 5
    # Eager fine graphs have at most 64 transitions per node; cap construction
    # at 262144 transitions. Larger domains use the identical lazy heading graph.
    PATH_FINE_EAGER_MAX_NODES = 4096
    PATH_SWITCH_MIN_ABS_GAIN = 3.0
    PATH_SWITCH_MIN_REL_GAIN = 0.08
    PATH_KEEP_NEAR_CELLS = 2
    PATH_SWAMP_COST = 5.0
    PATH_VISITED_FLOOR_COST = 1.0  # wall-side inspection owns coverage value; empty floor is neutral
    PATH_DESTINATION_ADJUST_CELLS = 4  # local120mm repair; never choose a remote target
    PATH_CLEARANCE_TARGET_CELLS = 2
    PATH_CLEARANCE_DEFICIT_PENALTY = 1000000.0
    PATH_TURN_COST = 0.85
    PATH_RIGHT_TURN_EXTRA_COST = 0.85
    PATH_OPEN_RIGHT_TURN_EXTRA_COST = 0.0  # price each bend consistently; do not split90deg turns
    PATH_60_GRID_OFF_PENALTY = 0.18
    PATH_30_GRID_OFF_PENALTY = 0.40
    PATH_WALL_FOLLOW_NEAR_PENALTY = 0.12
    PATH_WALL_FOLLOW_FAR_PENALTY = 0.18
    PATH_INSPECT_WALLS = True
    PATH_GATEWAY_LOOKUP_CELLS = 5
    PATH_LINE_RGB = (0.0, 0.65, 1.0)
    PATH_DIR_VECS = (
        (1, 0), (-1, 0), (0, 1), (0, -1),
        (1, 1), (-1, 1), (1, -1), (-1, -1),
    )

    # ------------------------------------------------------------------ #
    #  Visualization
    # ------------------------------------------------------------------ #
    DRAW_PERIOD_S = 0.35
    SHOW_NAV_MAP = globals().get("SHOW_MAPS", True)
    MAP_PX_PER_CELL = 4
    MAP_FIG_DPI = 96
    MAP_FIG_MAX_W_PX = 900
    MAP_FIG_MAX_H_PX = 700
    USED_PAD_CELLS = 8
    # custom_robot.proto collision objects, in millimetres. The forward IR
    # sphere is checked in the travel direction and throughout each turn.
    ROBOT_RADIUS_MM = 37.0
    FRONT_SENSOR_OFFSET_MM = 37.0
    FRONT_SENSOR_RADIUS_MM = 3.0
    CARDINAL_DIRS = (
        (0, 1, "N"),
        (1, 0, "E"),
        (0, -1, "S"),
        (-1, 0, "W"),
    )
    DIAGONAL_DIRS = (
        (1, 1),
        (1, -1),
        (-1, -1),
        (-1, 1),
    )

    def __init__(self, async_planning=False, *, inspect_walls=True):
        if not isinstance(inspect_walls, bool):
            raise TypeError("inspect_walls must be bool")
        self.PATH_INSPECT_WALLS = inspect_walls
        n = 2 * (self.HALF_WORLD_MM // self.NAV_GRID_MM)
        self.n = int(n)
        self.c = self.n // 2  # edge index at world 0; start sits between 4 cells

        self.floor = np.zeros((self.n, self.n), dtype=np.uint8)
        self.cell_state = np.zeros((self.n, self.n), dtype=np.uint8)
        self.current_beam = np.zeros((self.n, self.n), dtype=bool)
        self.wall_adjacent_discovered = np.zeros((self.n, self.n), dtype=bool)
        self.wall_v = np.zeros((self.n, self.n + 1), dtype=bool)
        self.wall_h = np.zeros((self.n + 1, self.n), dtype=bool)
        self.wall_v_counts = np.zeros((self.n, self.n + 1), dtype=np.uint16)
        self.wall_h_counts = np.zeros((self.n + 1, self.n), dtype=np.uint16)
        self.start_nav = (self.c, self.c)
        self.current_nav = None
        self.current_nav_xy = None
        self.current_world_mm = None
        self.current_yaw_deg = None
        self.current_pose_source = self.current_yaw_source = None
        self.origin_ready = False
        self.last_update_t = -1e9
        self.last_draw = -1e9
        self.fig_nav = self.ax_nav = self.img_nav = None
        self.wall_collection = None
        self.robot_dot = None
        self.robot_circle = None
        self.robot_arrow = None
        self.start_dot = None
        self.path_line = None
        self.destination_dot = None
        self.gateway_dots = []
        self.nav_click_cid = None
        self.show_measured_geometry = False
        self.nav_legend = None
        self._last_used_bbox = None
        self.last_wall_source = "none"
        self.last_curve_stairs = 0
        self.last_curve_stairs_skipped = 0
        self._curve_stair_keys = set()
        self.last_wall_adjacent_discovered_cells = 0
        self.last_current_beam_cells = 0
        self._current_pose_for_planning = None
        self._initial_escape = None
        self.plan_destination = None
        self.plan_destination_corner = None
        self.plan_requested_corner = None
        self.plan_destination_click = None
        self.plan_path = []
        self.plan_corner_path = []
        self.plan_raw_points = []
        self._plan_source_point = None
        self._plan_source_prefix=()
        self.plan_waypoints = []
        self.plan_annotations = []
        self.plan_status = "no destination"
        self.plan_cost = None
        self.plan_last_t = -1e9
        self.plan_last_start = None
        self.plan_last_goal = None
        self._plan_wall_dist = None
        self._plan_hole_dist = None
        self._plan_swamp_dist = None
        self._plan_v_lane_clear = None
        self._plan_h_lane_clear = None
        self._plan_corner_clear = None
        self._plan_clearance_deficit = None
        self._plan_normal_clear = None
        self._plan_tight_clear = None
        self._planning_mapper = None
        self._plan_obstacle_tree = None
        self._plan_material_tree = None
        self._plan_graph_signature = None
        self._plan_geometry_cache = None
        self._plan_graph_cache = {}
        self._plan_search_cache = {}
        self.async_planning = bool(async_planning)
        self._plan_executor = None
        self._plan_job = None
        self._plan_preview_owner = None
        self._plan_cancel_event = None
        self._plan_request_id = 0
        self._plan_completed_input = None
        self._planning_material_snapshot = None
        self._planning_input_signature = None
        self._planning_signature_cache = None
        self._plan_display_proof = None
        self._plan_display_points = None
        self._plan_tail_proof = None
        self._plan_remaining_cost_cache = None
        self._planner_closed = False
        self._plan_display_checked_t = -1e9
        self._plan_preview_queue = None
        self._plan_preview_offered = False
        self._warm_implicit_fine()

    # ------------------------------------------------------------------ #
    #  Coordinates
    # ------------------------------------------------------------------ #
    def world_to_nav(self, rx, ry):
        return (int(math.floor(rx / self.NAV_GRID_MM)) + self.c,
                int(math.floor(ry / self.NAV_GRID_MM)) + self.c)

    def nav_to_world(self, gx, gy):
        return ((gx - self.c + 0.5) * self.NAV_GRID_MM,
                (gy - self.c + 0.5) * self.NAV_GRID_MM)

    def in_bounds(self, gx, gy):
        return 0 <= gx < self.n and 0 <= gy < self.n

    # ------------------------------------------------------------------ #
    #  Graph access for future planning
    # ------------------------------------------------------------------ #
    def cell_walls(self, gx, gy):
        if not self.in_bounds(gx, gy):
            return None
        return {
            "N": bool(self.wall_h[gy + 1, gx]),
            "E": bool(self.wall_v[gy, gx + 1]),
            "S": bool(self.wall_h[gy, gx]),
            "W": bool(self.wall_v[gy, gx]),
        }

    def movement_blocked(self, gx, gy, nx, ny):
        if not self.in_bounds(gx, gy) or not self.in_bounds(nx, ny):
            return True
        dx = nx - gx
        dy = ny - gy
        if abs(dx) + abs(dy) != 1:
            return True
        if dx == 1:
            return bool(self.wall_v[gy, gx + 1])
        if dx == -1:
            return bool(self.wall_v[gy, gx])
        if dy == 1:
            return bool(self.wall_h[gy + 1, gx])
        return bool(self.wall_h[gy, gx])

    def neighbors4(self, gx, gy):
        for dx, dy, _ in self.CARDINAL_DIRS:
            nx, ny = gx + dx, gy + dy
            if not self.movement_blocked(gx, gy, nx, ny):
                yield nx, ny, 1.0

    def neighbors8(self, gx, gy):
        yield from self.neighbors4(gx, gy)
        root2 = math.sqrt(2.0)
        for dx, dy in self.DIAGONAL_DIRS:
            nx, ny = gx + dx, gy + dy
            if not self.in_bounds(nx, ny):
                continue
            # A diagonal step is valid only when both adjacent cardinal steps are
            # open, so the robot never clips through a wall corner.
            if (not self.movement_blocked(gx, gy, gx + dx, gy) and
                    not self.movement_blocked(gx, gy, gx, gy + dy) and
                    not self.movement_blocked(gx + dx, gy, nx, ny) and
                    not self.movement_blocked(gx, gy + dy, nx, ny)):
                yield nx, ny, root2

    def cell_known(self, gx, gy):
        return self.in_bounds(gx, gy) and self.cell_state[gy, gx] != self.CELL_UNKNOWN

    def cell_visited(self, gx, gy):
        return self.in_bounds(gx, gy) and self.cell_state[gy, gx] == self.CELL_VISITED

    # ------------------------------------------------------------------ #
    #  Main update
    # ------------------------------------------------------------------ #
    def update(self, data, mapper, show=None):
        """Refresh the 30 mm debug map from the current OccupancyMapper."""
        self._current_pose_for_planning = None
        self.current_pose_source = self.current_yaw_source = None
        if mapper is None or mapper.origin_x is None or mapper.origin_y is None:
            return

        self.origin_ready = True
        self._planning_mapper = mapper
        self._sync_walls_from_highres(mapper)
        self._sync_discovered_cells(mapper)
        self._sync_floor_tiles(mapper)
        self._mark_current_robot(data, mapper)
        self._mark_robot_visited()

        now = float(data.get("time", 0.0))
        self.last_update_t = now
        self._maybe_replan(now)
        show_nav = self.SHOW_NAV_MAP if show is None else bool(show)
        if show_nav and now - self.last_draw >= self.DRAW_PERIOD_S:
            self.last_draw = now
            self.draw_navigation_map()

    # ------------------------------------------------------------------ #
    #  Wall extraction
    # ------------------------------------------------------------------ #
    def _clear_walls(self):
        self.wall_v[:, :] = False
        self.wall_h[:, :] = False
        self.wall_v_counts[:, :] = 0
        self.wall_h_counts[:, :] = 0

    # Matrix conversion is kept as a debug/alternate source. It is clean for
    # straight scoring walls, but high-res geometry is the primary navigation
    # source because it preserves curves and is less tied to matrix repair passes.
    def _sync_walls_from_matrix(self, mapper):
        matrix = getattr(mapper, "report_matrix", None)
        meta = getattr(mapper, "_matrix_meta", {})
        if not matrix or not meta or "x0_mm" not in meta or "y1_mm" not in meta:
            return False

        rows = len(matrix)
        cols = len(matrix[0]) if rows else 0
        if rows <= 1 or cols <= 1:
            return False

        cell = float(meta.get("cell_mm", self.NAV_GRID_MM))
        if abs(cell - self.NAV_GRID_MM) > 1e-6:
            return False

        self._clear_walls()
        x0 = float(meta["x0_mm"])
        y1 = float(meta["y1_mm"])

        for r, row in enumerate(matrix):
            wy = y1 - r * cell
            edge_y = int(round(wy / self.NAV_GRID_MM)) + self.c
            for col in range(cols - 1):
                if row[col] == "1" and row[col + 1] == "1":
                    wx0 = x0 + col * cell
                    cell_x = int(math.floor(wx0 / self.NAV_GRID_MM)) + self.c
                    if 0 <= edge_y <= self.n and 0 <= cell_x < self.n:
                        self.wall_h[edge_y, cell_x] = True
                        self.wall_h_counts[edge_y, cell_x] += 1

        for r in range(rows - 1):
            wy0 = y1 - (r + 1) * cell
            cell_y = int(math.floor(wy0 / self.NAV_GRID_MM)) + self.c
            for col in range(cols):
                if matrix[r][col] == "1" and matrix[r + 1][col] == "1":
                    wx = x0 + col * cell
                    edge_x = int(round(wx / self.NAV_GRID_MM)) + self.c
                    if 0 <= cell_y < self.n and 0 <= edge_x <= self.n:
                        self.wall_v[cell_y, edge_x] = True
                        self.wall_v_counts[cell_y, edge_x] += 1

        self.last_wall_source = "report_matrix"
        return True

    def _sync_walls_from_highres(self, mapper):
        self._planning_mapper = mapper
        occ = mapper.occupied_geometry()
        obstacle_mask = getattr(mapper, "_obstacle_mask", None)
        if obstacle_mask is not None and obstacle_mask.shape == mapper.logodds.shape:
            occ = occ | obstacle_mask

        if not np.any(occ):
            self._clear_walls()
            self.last_wall_source = "highres_empty"
            return

        self._clear_walls()
        scale = self.NAV_GRID_MM // mapper.GRID_MM

        nav_edges = np.arange(self.n + 1, dtype=np.int32) - self.c
        hi_edge = mapper.c + nav_edges * scale
        row_starts = mapper.c + (np.arange(self.n, dtype=np.int32) - self.c) * scale
        row_ends = row_starts + self.EDGE_SEG_CELLS

        v_evidence = np.zeros((mapper.n, self.n + 1), dtype=bool)
        for d in range(-self.EDGE_BAND_CELLS, self.EDGE_BAND_CELLS + 1):
            cols = np.clip(hi_edge + d, 0, mapper.n - 1)
            v_evidence |= occ[:, cols]
        row_starts = np.clip(row_starts, 0, mapper.n)
        row_ends = np.clip(row_ends, 0, mapper.n)
        count_dtype=np.uint16 if mapper.n<=np.iinfo(np.uint16).max else np.uint64
        v_cum = np.vstack((np.zeros((1, self.n + 1), dtype=np.uint16),
                           np.cumsum(v_evidence, axis=0, dtype=count_dtype)))
        v_counts = v_cum[row_ends, :] - v_cum[row_starts, :]
        self.wall_v_counts[:, :] = v_counts.astype(np.uint16)
        self.wall_v[:, :] = v_counts >= self.EDGE_MIN_SPAN_CELLS

        col_starts = mapper.c + (np.arange(self.n, dtype=np.int32) - self.c) * scale
        col_ends = col_starts + self.EDGE_SEG_CELLS
        h_evidence = np.zeros((self.n + 1, mapper.n), dtype=bool)
        for d in range(-self.EDGE_BAND_CELLS, self.EDGE_BAND_CELLS + 1):
            rows = np.clip(hi_edge + d, 0, mapper.n - 1)
            h_evidence |= occ[rows, :]
        col_starts = np.clip(col_starts, 0, mapper.n)
        col_ends = np.clip(col_ends, 0, mapper.n)
        h_cum = np.hstack((np.zeros((self.n + 1, 1), dtype=np.uint16),
                           np.cumsum(h_evidence, axis=1, dtype=count_dtype)))
        h_counts = h_cum[:, col_ends] - h_cum[:, col_starts]
        self.wall_h_counts[:, :] = h_counts.astype(np.uint16)
        self.wall_h[:, :] = h_counts >= self.EDGE_MIN_SPAN_CELLS
        self._apply_curve_stair_steps(mapper)
        for ax, ay, bx, by in mapper._confirmed_straight_walls:
            self._set_wall_segment(round(ax/self.NAV_GRID_MM)+self.c,
                round(ay/self.NAV_GRID_MM)+self.c, round(bx/self.NAV_GRID_MM)+self.c,
                round(by/self.NAV_GRID_MM)+self.c)
        self.last_wall_source = "highres_edge_sampler"

    def _apply_curve_stair_steps(self, mapper):
        """Replace only confirmed curve bends with one thin stair-step path."""
        self.last_curve_stairs = 0
        self.last_curve_stairs_skipped = 0
        self._curve_stair_keys = set()
        bends = getattr(mapper, "_curve_bends", set())
        matrix = getattr(mapper, "report_matrix", None)
        meta = getattr(mapper, "_matrix_meta", {})
        if not bends or not matrix or not meta:
            return
        x0 = float(meta.get("x0_mm", 0.0))
        y1 = float(meta.get("y1_mm", 0.0))
        cell = float(meta.get("cell_mm", self.NAV_GRID_MM))

        curves = []
        for r, col in bends:
            if not (0 <= r < len(matrix) and 0 <= col < len(matrix[r])):
                continue
            wx = x0 + col * cell
            wy = y1 - r * cell
            bx = int(round(wx / self.NAV_GRID_MM)) + self.c
            by = int(round(wy / self.NAV_GRID_MM)) + self.c
            vertex = mapper._vertex_key(wx, wy)
            directions = sorted((sx, sy) for vx, vy, sx, sy in mapper._confirmed_curve_quadrants
                                if (vx, vy) == vertex)
            if not directions:
                directions = [self._curve_arm_dirs(mapper, matrix, r, col, wx, wy)]
            for sx, sy in directions:
                if sx == 0 or sy == 0:
                    self.last_curve_stairs_skipped += 1
                    continue
                if mapper._tile_of(wx+sx*30, wy+sy*30) in mapper._area4_tiles:
                    # Area 4 permits arbitrary geometry; retain the measured edges
                    # rather than imposing the Areas 1-3 quarter-circle template.
                    self.last_curve_stairs_skipped += 1
                    continue
                # A real straight wall may pass through a curved bend. Preserve its
                # near-bend segment when the raw axis itself is occupied; the arc
                # alone can alias onto that axis only near the far endpoint.
                dx, dy = mapper.last_grid_offset
                gx = round((wx+dx)/mapper.GRID_MM)+mapper.c
                gy = round((wy+dy)/mapper.GRID_MM)+mapper.c
                steps = np.arange(self.NAV_GRID_MM//mapper.GRID_MM+1)
                iy, ix = gy+sy*steps, gx+sx*steps
                keep_v = keep_h = False
                if (0 <= gx < mapper.n and 0 <= gy < mapper.n
                        and np.all((iy >= 0) & (iy < mapper.n) & (ix >= 0) & (ix < mapper.n))):
                    keep_v = np.count_nonzero(mapper.logodds[iy, gx] >= mapper.OCC_T) >= 3
                    keep_h = np.count_nonzero(mapper.logodds[gy, ix] >= mapper.OCC_T) >= 3
                curves.append((bx, by, sx, sy, keep_v, keep_h))

        # Clear all measured quadrants before drawing: adjacent arcs can share
        # a boundary edge, which a later clear must not erase.
        for bx, by, sx, sy, _, _ in curves:
            self._clear_curve_quadrant(bx, by, sx, sy)
        for bx, by, sx, sy, keep_v, keep_h in curves:
            self._draw_curve_stair(bx, by, sx, sy)
            if keep_v:
                self._set_wall_segment(bx, by, bx, by+sy)
            if keep_h:
                self._set_wall_segment(bx, by, bx+sx, by)
            self._curve_stair_keys.add((bx, by, sx, sy))
            self.last_curve_stairs += 1

    def _curve_arm_dirs(self, mapper, matrix, r, col, wx, wy):
        """Use the same fitted scan evidence as the competition curve detector."""
        vertex = mapper._vertex_key(wx, wy)
        confirmed = [(sx, sy) for bx, by, sx, sy in mapper._confirmed_curve_quadrants
                     if (bx, by) == vertex]
        if vertex in mapper._persistent_curve_bends and len(confirmed) == 1:
            # Fitting and contradiction checks belong to the mapper. A later
            # partial view must not make navigation discard a still-confirmed
            # curve while the competition matrix retains the very same bend.
            return confirmed[0]
        up = r > 0 and matrix[r-1][col] == "1"
        dn = r+1 < len(matrix) and matrix[r+1][col] == "1"
        lf = col > 0 and matrix[r][col-1] == "1"
        rt = col+1 < len(matrix[r]) and matrix[r][col+1] == "1"
        best, direction = 0, (0, 0)
        for sx, horizontal in ((-1, lf), (1, rt)):
            for sy, vertical in ((-1, dn), (1, up)):
                if (horizontal and vertical) or mapper._vertex_key(wx, wy) in mapper._persistent_curve_bends:
                    evidence = mapper._arc_evidence(wx, wy, sy > 0, sy < 0, sx < 0, sx > 0)
                    if evidence >= mapper.ARC_SOFT_HITS and evidence > best:
                        best, direction = evidence, (sx, sy)
        return direction

    def _clear_curve_quadrant(self, bx, by, sx, sy):
        rad = self.CURVE_STAIR_RADIUS_CELLS
        xlo, xhi = sorted((bx, bx + sx * rad))
        ylo, yhi = sorted((by, by + sy * rad))
        xlo = max(0, min(self.n, xlo))
        xhi = max(0, min(self.n, xhi))
        ylo = max(0, min(self.n, ylo))
        yhi = max(0, min(self.n, yhi))
        if xlo < xhi and ylo < yhi:
            self.wall_v[ylo:yhi, xlo:xhi + 1] = False
            self.wall_h[ylo:yhi + 1, xlo:xhi] = False

    def _draw_curve_stair(self, bx, by, sx, sy):
        rad = self.CURVE_STAIR_RADIUS_CELLS
        x = bx + sx * rad
        y = by
        target_x = bx
        target_y = by + sy * rad
        # The circle is centred at (bx+sx*rad, by+sy*rad). Move toward
        # the bend first; the opposite order rounds the other square corner.
        vertical_next = False
        while (x, y) != (target_x, target_y):
            if vertical_next and y != target_y:
                ny = y + sy
                self._set_wall_segment(x, y, x, ny)
                y = ny
            elif x != target_x:
                nx = x - sx
                self._set_wall_segment(x, y, nx, y)
                x = nx
            else:
                ny = y + sy
                self._set_wall_segment(x, y, x, ny)
                y = ny
            vertical_next = not vertical_next

    def _set_wall_segment(self, x0, y0, x1, y1):
        if x0 == x1:
            for y in range(min(y0,y1),max(y0,y1)):
                if 0 <= y < self.n and 0 <= x0 <= self.n:
                    self.wall_v[y, x0] = True
                    self.wall_v_counts[y, x0] = max(
                        self.wall_v_counts[y, x0], self.EDGE_MIN_SPAN_CELLS)
        elif y0 == y1:
            for x in range(min(x0,x1),max(x0,x1)):
                if 0 <= y0 <= self.n and 0 <= x < self.n:
                    self.wall_h[y0, x] = True
                    self.wall_h_counts[y0, x] = max(
                        self.wall_h_counts[y0, x], self.EDGE_MIN_SPAN_CELLS)

    # ------------------------------------------------------------------ #
    #  Floor classification
    # ------------------------------------------------------------------ #
    def _tile_nav_bounds(self, tx, ty):
        x_min = tx * OccupancyMapper.TILE_MM - OccupancyMapper.TILE_MM / 2.0
        x_max = tx * OccupancyMapper.TILE_MM + OccupancyMapper.TILE_MM / 2.0
        y_min = ty * OccupancyMapper.TILE_MM - OccupancyMapper.TILE_MM / 2.0
        y_max = ty * OccupancyMapper.TILE_MM + OccupancyMapper.TILE_MM / 2.0
        gx0 = int(math.floor(x_min / self.NAV_GRID_MM)) + self.c
        gx1 = int(math.floor((x_max - 1e-6) / self.NAV_GRID_MM)) + self.c + 1
        gy0 = int(math.floor(y_min / self.NAV_GRID_MM)) + self.c
        gy1 = int(math.floor((y_max - 1e-6) / self.NAV_GRID_MM)) + self.c + 1
        return (max(0, gx0), min(self.n, gx1),
                max(0, gy0), min(self.n, gy1))

    def _stamp_floor_tile(self, tx, ty, code):
        gx0, gx1, gy0, gy1 = self._tile_nav_bounds(tx, ty)
        if gx0 < gx1 and gy0 < gy1:
            self.floor[gy0:gy1, gx0:gx1] = code

    def _sync_floor_tiles(self, mapper):
        self.floor[:, :] = self.FLOOR_NORMAL
        for (tx, ty), ch in mapper.mapped_floor_tiles().items():
            code = self.FLOOR_CHAR_TO_CODE.get(ch)
            if code is not None:
                self._stamp_floor_tile(tx, ty, code)

        sx, sy = getattr(mapper, "start_tile", (0, 0))
        self._stamp_floor_tile(sx, sy, self.FLOOR_START)

    # ------------------------------------------------------------------ #
    #  Cell discovery / visitation state
    # ------------------------------------------------------------------ #
    def _sync_discovered_cells(self, mapper):
        self.last_wall_adjacent_discovered_cells = 0
        free = mapper.logodds <= mapper.FREE_T
        scale = self.NAV_GRID_MM // mapper.GRID_MM
        discovered = np.zeros((self.n, self.n), dtype=bool)
        if scale <= 0:raise ValueError('navigation grid must cover a mapping cell')
        offset=0 if mapper.n==self.n*scale else mapper.c-self.c*scale
        extent=self.n*scale
        before=max(0,-offset);after=max(0,offset+extent-mapper.n)
        cells=np.pad(free,((before,after),(before,after)),constant_values=False)if before or after else free
        start=offset+before
        cells=cells[start:start+extent,start:start+extent]
        counts=cells.reshape(self.n,scale,self.n,scale).sum(axis=(1,3))
        discovered[:,:]=counts>=self.DISCOVER_FREE_MIN_CELLS

        wall_added = self._wall_adjacent_discovered_mask(discovered)
        self.wall_adjacent_discovered[:, :] = wall_added
        discovered |= wall_added
        self.last_wall_adjacent_discovered_cells = int(np.count_nonzero(wall_added))

        keep_visited = self.cell_state == self.CELL_VISITED
        self.cell_state[:, :] = self.CELL_UNKNOWN
        self.cell_state[discovered] = self.CELL_UNVISITED
        self.cell_state[keep_visited] = self.CELL_VISITED

    def _wall_adjacent_discovered_mask(self, discovered):
        wall_adjacent = np.zeros_like(discovered, dtype=bool)
        wall_adjacent |= self.wall_v[:, 1:]
        wall_adjacent |= self.wall_v[:, :-1]
        wall_adjacent |= self.wall_h[1:, :]
        wall_adjacent |= self.wall_h[:-1, :]

        same_side_support = np.zeros_like(discovered, dtype=bool)
        same_side_support[:, 1:] |= discovered[:, :-1] & ~self.wall_v[:, 1:self.n]
        same_side_support[:, :-1] |= discovered[:, 1:] & ~self.wall_v[:, 1:self.n]
        same_side_support[1:, :] |= discovered[:-1, :] & ~self.wall_h[1:self.n, :]
        same_side_support[:-1, :] |= discovered[1:, :] & ~self.wall_h[1:self.n, :]
        return wall_adjacent & same_side_support & ~discovered

    def _mark_robot_visited(self):
        self.current_beam[:, :] = False
        self.last_current_beam_cells = 0
        if self.current_world_mm is None or self.current_yaw_deg is None:
            return
        rx, ry = self.current_world_mm
        cx, cy = self.world_to_nav(rx, ry)
        if not self.in_bounds(cx, cy):
            return

        bang = math.radians(OccupancyMapper.YAW_OFFSET_DEG +
                            OccupancyMapper.YAW_SIGN * self.current_yaw_deg)
        fwd = (math.sin(bang), math.cos(bang))
        right = (math.cos(bang), -math.sin(bang))
        # Physical presence proves the current cell is visited even when
        # a rotated side-camera sample lattice falls around its centre.
        beam = {(cx, cy)}
        lateral_offsets = self._robot_beam_lateral_offsets(rx, ry, cx, cy, right)
        forward_offsets = (
            np.arange(self.ROBOT_BEAM_LENGTH_CELLS, dtype=np.float32)
            - (self.ROBOT_BEAM_LENGTH_CELLS - 1) / 2.0
        ) * self.NAV_GRID_MM
        for lateral in lateral_offsets:
            for forward in forward_offsets:
                wx = rx + right[0] * float(lateral) + fwd[0] * float(forward)
                wy = ry + right[1] * float(lateral) + fwd[1] * float(forward)
                gx, gy = self.world_to_nav(wx, wy)
                if self.in_bounds(gx, gy) and self._line_unblocked(cx, cy, gx, gy):
                    beam.add((gx, gy))
        for gx, gy in beam:
            self.current_beam[gy, gx] = True
            self.cell_state[gy, gx] = self.CELL_VISITED
        self.last_current_beam_cells = len(beam)

    def _robot_beam_lateral_offsets(self, rx, ry, cx, cy, right):
        offsets = []
        for side in (-1.0, 1.0):
            cells = self.ROBOT_BEAM_WALL_SIDE_CELLS
            if not self._side_probe_reaches_wall(rx, ry, cx, cy, right, side):
                cells = self.ROBOT_BEAM_OPEN_SIDE_CELLS
            side_offsets = (np.arange(cells, dtype=np.float32) + 0.5)
            offsets.append(side * side_offsets * self.NAV_GRID_MM)
        return np.concatenate(offsets)

    def _side_probe_reaches_wall(self, rx, ry, cx, cy, right, side):
        probe_cells = self.ROBOT_BEAM_WALL_SIDE_CELLS + 1
        probe_dist = probe_cells * self.NAV_GRID_MM
        gx, gy = self.world_to_nav(rx + right[0] * side * probe_dist,
                                   ry + right[1] * side * probe_dist)
        if not self.in_bounds(gx, gy):
            return False
        return not self._line_unblocked(cx, cy, gx, gy)

    def _line_unblocked(self, gx, gy, tx, ty):
        cells = self._grid_line_cells(gx, gy, tx, ty)
        if not cells:
            return False
        for (ax, ay), (bx, by) in zip(cells, cells[1:]):
            dx = bx - ax
            dy = by - ay
            if dx == 0 and dy == 0:
                continue
            if abs(dx) + abs(dy) == 1:
                if self.movement_blocked(ax, ay, bx, by):
                    return False
            elif abs(dx) == 1 and abs(dy) == 1:
                if (self.movement_blocked(ax, ay, ax + dx, ay)
                        or self.movement_blocked(ax, ay, ax, ay + dy)
                        or self.movement_blocked(ax + dx, ay, bx, by)
                        or self.movement_blocked(ax, ay + dy, bx, by)):
                    return False
            else:
                return False
        return True

    @staticmethod
    def _grid_line_cells(gx, gy, tx, ty):
        dx = tx - gx
        dy = ty - gy
        steps = max(abs(dx), abs(dy))
        if steps == 0:
            return [(gx, gy)]
        cells = []
        for i in range(steps + 1):
            x = int(round(gx + dx * (i / steps)))
            y = int(round(gy + dy * (i / steps)))
            if not cells or cells[-1] != (x, y):
                cells.append((x, y))
        return cells

    # ------------------------------------------------------------------ #
    #  Path planning
    # ------------------------------------------------------------------ #
    def set_plan_destination(self, gx, gy, *, inspect_walls=None):
        if not self.in_bounds(gx, gy):
            return
        corner = self._nearest_nav_corner_edge(gx, gy)
        self.set_plan_destination_corner(corner, preferred=(int(gx), int(gy)),
                                         inspect_walls=inspect_walls)

    def set_plan_destination_corner(self, corner, preferred=None, *, inspect_walls=None):
        if inspect_walls is not None:
            if not isinstance(inspect_walls, bool):
                raise TypeError("inspect_walls must be bool")
            if inspect_walls != self.PATH_INSPECT_WALLS:
                self.PATH_INSPECT_WALLS = inspect_walls
                # A route chosen under the old mission objective is not an
                # incumbent for the new mode. Pending jobs are invalidated below.
                self._set_plan_failed("planning")
        self._plan_preview_owner = None
        self._plan_tail_proof = self._plan_remaining_cost_cache = self._plan_display_proof = None
        self._plan_request_id += 1
        if self._plan_job is not None:
            self._plan_job[1]._plan_cancel_event.set()
        self.plan_destination_corner = tuple(corner)
        self.plan_requested_corner = tuple(corner)
        self.plan_destination_click = tuple(preferred) if preferred is not None else None
        self._plan_display_points = None
        self.plan_destination = self._corner_adjacent_cell(
            corner, preferred=self.plan_destination_click)
        self.plan_last_t = -1e9
        if self.async_planning:
            self._set_plan_failed("planning")
        self._maybe_replan(float(self.last_update_t))

    def set_wall_inspection(self, enabled):
        """Change the mission preference and replan the current destination."""
        if not isinstance(enabled, bool):
            raise TypeError("enabled must be bool")
        if enabled == self.PATH_INSPECT_WALLS:
            return
        if self.plan_destination_corner is None:
            self.PATH_INSPECT_WALLS = enabled
        else:
            self.set_plan_destination_corner(self.plan_requested_corner or self.plan_destination_corner,
                preferred=self.plan_destination_click, inspect_walls=enabled)

    def clear_plan_destination(self):
        self._plan_preview_owner = None
        self._plan_tail_proof = self._plan_remaining_cost_cache = self._plan_display_proof = None
        self._plan_request_id += 1
        if self._plan_job is not None:
            self._plan_job[1]._plan_cancel_event.set()
        self._plan_display_points = None
        self._plan_completed_input = None
        self.plan_destination = None
        self.plan_destination_corner = None
        self.plan_requested_corner = None
        self.plan_destination_click = None
        self.plan_path = []
        self.plan_corner_path = []
        self.plan_raw_points = []
        self._plan_source_point = None
        self._plan_source_prefix=()
        self.plan_waypoints = []
        self.plan_annotations = []
        self.plan_status = "no destination"
        self.plan_cost = None

    def _plan_endpoint_failure(self, start, goal):
        """The existing necessary floor/bounds gates need no geometry model."""
        sx, sy = start
        gx, gy = goal
        if not (self.in_bounds(sx, sy) and self.in_bounds(gx, gy)):
            return "start or destination outside nav map"
        if (self.cell_state[sy, sx] == self.CELL_UNKNOWN
                or self.floor[sy, sx] == self.FLOOR_HOLE):
            return "start is not on known safe floor"
        if (self.cell_state[gy, gx] == self.CELL_UNKNOWN
                or self.floor[gy, gx] == self.FLOOR_HOLE):
            return "destination is unknown or blocked"
        return None

    def _reject_invalid_plan_endpoints(self):
        # Stale legal-pose handling keeps its existing priority. A missing
        # source/destination also keeps its existing caller-specific status.
        if (self.current_nav is None or self.plan_destination is None
                or (self.async_planning and not self._planning_pose_current())):
            return False
        reason = self._plan_endpoint_failure(self.current_nav, self.plan_destination)
        if reason is None:
            return False
        self._set_plan_failed(reason)
        self._plan_completed_input = None
        self.plan_last_t = -1e9  # A newly valid endpoint can recover immediately.
        job = self._plan_job
        if job is not None:
            future, worker, request_id, _ = job
            worker._plan_cancel_event.set()
            if request_id == self._plan_request_id:
                self._plan_request_id += 1  # Invalidate this job exactly once.
            if future.done():
                self._plan_job = None
                try:
                    future.result()
                except Exception:
                    pass  # Obsolete worker errors cannot publish over rejection.
        return True

    def _maybe_replan(self, now):
        if self._reject_invalid_plan_endpoints():
            return
        if self.plan_path and self.current_world_mm is not None:
            self._refresh_displayed_plan()
        if self.async_planning:
            self._poll_plan_job()
            if self.plan_destination is not None and not self._planning_pose_current():
                self._set_plan_failed("planning (no current pose)")
                self._plan_completed_input=None
                self.plan_last_t=-1e9
                return
        if self._planner_closed or self.plan_destination is None:
            if self.plan_destination_corner is not None:
                self.plan_status = "destination corner has no adjacent nav cell"
            return
        if self.current_nav is None:
            self.plan_status = "no current pose"
            return
        start, goal = tuple(self.current_nav), tuple(self.plan_destination)
        if self.async_planning and self._plan_job is not None:
            return  # one running request; later clicks coalesce, never queue
        if (start == self.plan_last_start and goal == self.plan_last_goal
                and now - self.plan_last_t < self.PATH_REPLAN_PERIOD_S):
            return
        self.plan_last_t, self.plan_last_start, self.plan_last_goal = now, start, goal
        if self.async_planning:
            mapper = self._planning_mapper
            material, grid = (mapper.physical_material() if mapper is not None
                              else (np.empty((0, 2)), 10))
            key = (self._planning_signature(material, grid), self.current_world_mm,
                   self.current_yaw_deg, self.plan_destination_corner,
                   self.plan_destination_click, self._plan_request_id)
            if key != self._plan_completed_input:
                worker = self._planning_snapshot()
                self._start_plan_job(worker, start, goal, key)
            return
        previous = self._snapshot_plan()
        previous_proof = self._plan_tail_proof
        self._plan_path(start, goal)
        self._retain_previous_if_stable(previous, previous_proof)

    def _planning_pose_current(self):
        return (self.current_world_mm is not None and self.current_yaw_deg is not None
                and all(math.isfinite(float(v)) for v in (*self.current_world_mm,self.current_yaw_deg,self.last_update_t))
                and self._current_pose_for_planning ==
                    (self.current_world_mm,self.current_yaw_deg,self.last_update_t))

    def _planning_signature(self, material, grid_mm):
        """Reuse a digest only when every original input byte is unchanged."""
        mapper = self._planning_mapper
        arrays = tuple(np.ascontiguousarray(array) for array in
            (self.cell_state, self.floor, self.wall_v, self.wall_h,
             mapper.logodds <= mapper.FREE_T if mapper is not None else np.empty(0), material))
        # Names change only when the normal class/MRO dictionaries change.
        # Read every actual parameter value on EVERY call, including instance
        # overrides and mutable values. Custom metaclasses retain dir() exactly.
        cls = type(self)
        layout = (tuple((base, tuple(vars(base))) for base in cls.__mro__)
                  if type(cls) is type else None)
        names_cache = getattr(self, "_planning_parameter_names_cache", None)
        if layout is None or names_cache is None or names_cache[0] != layout:
            names = tuple(name for name in dir(cls) if name.startswith("PATH_"))
            self._planning_parameter_names_cache = layout, names
        else:
            names = names_cache[1]
        parameters = [(name, getattr(self, name)) for name in names]
        encoded = repr((grid_mm, self.ROBOT_RADIUS_MM, self.FRONT_SENSOR_OFFSET_MM,
                        self.FRONT_SENSOR_RADIUS_MM, parameters)).encode()
        cached = getattr(self, "_planning_signature_cache", None)
        # Only physical_material's owning readonly precise cache has immutable
        # identity. External/mutable/view fixtures always compare their bytes.
        owned = (material if mapper is not None
            and getattr(mapper, "_physical_material_cache", (None,))[0] is material
            and material.flags.owndata and not material.flags.writeable else None)
        saved = tuple((array.shape, array.dtype.str,
            cached[1][i][2] if i == 5 and owned is not None and cached is not None
                and cached[3] is owned and cached[1][i][:2] == (array.shape, array.dtype.str)
            else array.tobytes()) for i, array in enumerate(arrays))
        if cached is not None and cached[0] == encoded and cached[1] == saved:
            return cached[2]
        signature = hashlib.blake2b(digest_size=16)
        for shape, dtype, data in saved:
            signature.update(data)
        signature.update(encoded)
        digest = signature.digest()
        self._planning_signature_cache = encoded, saved, digest, owned
        return digest

    def _planning_snapshot(self):
        """Own only the arrays/material used by planning; never share live maps.

        Computing material here avoids copying the 18 MB evidence grid or
        reading mutable wall models on another thread. GUI artists are never
        read or modified by the worker.
        """
        worker = copy.copy(self)
        worker.async_planning = False
        worker._plan_job = worker._plan_executor = None
        worker._plan_cancel_event = None
        worker._plan_graph_cache = self._plan_graph_cache.copy()
        worker._plan_search_cache = self._plan_search_cache.copy()
        worker._plan_display_points = None
        for name in ("floor", "cell_state", "wall_v", "wall_h"):
            array = getattr(self, name).copy()
            array.flags.writeable = False
            setattr(worker, name, array)
        mapper = self._planning_mapper
        material_reader = getattr(self, '_planning_material', None)
        material, grid = ((material_reader() if material_reader is not None else mapper.physical_material())
                          if mapper is not None else (np.empty((0,2)), 10))
        material = np.array(material, copy=True)
        material.flags.writeable = False
        worker._planning_material_snapshot = material, grid
        if mapper is not None:
            worker._planning_mapper = copy.copy(mapper)
            worker._planning_mapper.logodds = mapper.logodds.copy()
            worker._planning_mapper.logodds.flags.writeable = False
        worker._planning_input_signature = worker._planning_signature(material, grid)
        return worker

    def _check_plan_cancelled(self):
        if self._plan_cancel_event is not None and self._plan_cancel_event.is_set():
            raise CancelledError()

    def _start_plan_job(self, worker, start, goal, key):
        self._plan_preview_owner = None
        # A provisional route is not an independently committed incumbent.
        # Store only defensive plan data, never a mutable geometry proof.
        worker._plan_committed_previous = self._snapshot_plan()
        if not self.plan_path:
            self._set_plan_failed("planning")
        worker._plan_cancel_event = Event()
        worker._plan_preview_queue = Queue(maxsize=1)
        worker._plan_preview_offered = False
        if self._plan_executor is None:
            self._plan_executor = ThreadPoolExecutor(max_workers=1, thread_name_prefix="route")
        future = self._plan_executor.submit(worker._run_plan_job, start, goal)
        self._plan_job = future, worker, self._plan_request_id, key

    def _offer_plan_preview(self):
        if (self._plan_preview_queue is not None and not self._plan_preview_offered
                and self.plan_status == "ok"):
            self._plan_preview_queue.put_nowait((self._snapshot_plan(),self._path_corner_points()))
            self._plan_preview_offered = True

    def _run_plan_job(self, start, goal):
        self._plan_path(start,goal)
        self._check_plan_cancelled()
        self._plan_display_points = self._path_corner_points()

    def _adopt_planning_context(self, worker):
        for name in ("_plan_wall_dist", "_plan_hole_dist", "_plan_swamp_dist",
                     "_plan_v_lane_clear", "_plan_h_lane_clear", "_plan_corner_clear",
                     "_plan_clearance_deficit", "_plan_normal_clear", "_plan_tight_clear",
                     "_plan_obstacle_tree", "_plan_material_tree", "_plan_grid_mm",
                     "_initial_escape", "_plan_fine_search_stats", "_plan_geometry_cache", "_plan_rectangle_cache"):
            if hasattr(worker, name):
                setattr(self, name, getattr(worker, name))

    def _rebase_plan(self, snapshot):
        """Keep fixed route anchors; replace, never revisit, an old body source."""
        self._plan_display_points = None
        if (not (snapshot["path"] or snapshot["corner_path"] or snapshot.get("raw_points"))
                or (self.async_planning and not self._planning_pose_current())
                or self.current_world_mm is None
                or not all(math.isfinite(v) for v in self.current_world_mm)
                or (self.current_yaw_deg is not None and not math.isfinite(float(self.current_yaw_deg)))):
            return False
        # Destination metadata may have changed before the old route was
        # snapshotted. Its actual last anchor must still own that destination.
        anchors = snapshot.get("raw_points") or snapshot["corner_path"]
        if anchors:
            target = snapshot["destination_corner"]
            if target is None and snapshot["destination"] is not None:
                target = tuple(v+.5 for v in snapshot["destination"])
        else:
            anchors, target = snapshot["path"], snapshot["destination"]
        if target is not None and tuple(anchors[-1]) != tuple(target):
            return False
        self._restore_plan(snapshot)
        source = tuple(v/self.NAV_GRID_MM+self.c for v in self.current_world_mm)
        raw = bool(self.plan_raw_points)
        progress_prefix = ()
        points = (list(self.plan_raw_points) if raw else list(self.plan_corner_path)
                  if self.plan_corner_path else [(x+.5,y+.5) for x,y in self.plan_path])
        if raw and snapshot.get("source_point") is not None:
            # This role was assigned by the fine constructor/rebase, not guessed
            # from an epsilon or from a point accidentally equal to today's pose.
            prefix=tuple(map(tuple,snapshot.get("source_prefix") or (snapshot["source_point"],)))
            if (prefix and prefix[0]==tuple(snapshot["source_point"])
                    and tuple(map(tuple,points[:len(prefix)]))==prefix):
                progress_prefix = prefix
                points = points[len(prefix):]
            else:
                return False
        points = [tuple(p) for i,p in enumerate(points) if i == 0 or tuple(p) != tuple(points[i-1])]
        if not points:
            target=snapshot["destination_corner"]
            if target is None and snapshot["destination"] is not None:
                target=tuple(v+.5 for v in snapshot["destination"])
            if target is None or tuple(snapshot.get("source_point") or ()) != tuple(target):
                return False
            points=[tuple(target)]  # A reached goal stays fixed if the body moves away.
        if len(points)>1:
            # The incoming leg remains evidence of progress even though its
            # old dynamic source/connector can never become route anchors.
            # Omitting it lets tiny sideways jitter project onto a much later
            # leg and incorrectly skip an inspection bend ahead of the robot.
            p=np.asarray(list(progress_prefix)+points,dtype=float);delta=np.diff(p,axis=0)
            length2=np.sum(delta*delta,axis=1)
            t=np.divide(np.sum((source-p[:-1])*delta,axis=1),length2,
                        out=np.zeros_like(length2),where=length2>0)
            projected=p[:-1]+np.clip(t,0,1)[:,None]*delta
            order=np.argsort(np.sum((projected-source)**2,axis=1),kind="stable")
            # Proximity cannot prove visibility/progress: a closer later leg
            # can be behind a wall. Prefer its forward endpoint, then a needed
            # fixed entry corner; never restore the stripped dynamic source.
            choices=list(dict.fromkeys(j for i in order for j in
                ((0,) if i<len(progress_prefix) else
                 ((int(i)-len(progress_prefix)+1,int(i)-len(progress_prefix)) if t[i]>0 else
                  (int(i)-len(progress_prefix),int(i)-len(progress_prefix)+1)))))
        else:choices=[0]
        self._route_tail_clear(points)  # Validate ownership once, not once per candidate.
        proof=self._plan_tail_proof
        offset=len(proof[0][2])-len(points)  # Current tail may reuse a longer proved route.
        for index in choices:
            self._check_plan_cancelled()
            if not proof[2][offset+index]:
                continue
            connector=self._compass_connector(source,points[index],points[index+1:index+2])
            if connector is None:
                continue
            anchors=points[index:]  # Allocate the retained tail only after all gates pass.
            display=connector+anchors[1:]
            display=[p for i,p in enumerate(display) if i == 0 or p != display[i-1]]
            source_prefix=max(1,len(connector)-1)
            simplify=getattr(self,"_simplify_plan_points",None)
            if raw and simplify is not None:
                display,source_prefix=simplify(display,source_prefix)
            if len(display)==1 and self.current_yaw_deg is not None:
                heading=math.pi/2-math.radians(OccupancyMapper.YAW_OFFSET_DEG+OccupancyMapper.YAW_SIGN*self.current_yaw_deg)
                if not self._heading_turn_mask(*source,heading,heading):continue
            old_path=list(self.plan_path)
            if raw:
                changed = (anchors != points or not old_path or old_path[0] != self.current_nav
                           or display != list(snapshot.get("raw_points", ())))
                self.plan_raw_points=display
                self._plan_source_point=source
                self._plan_source_prefix=tuple(display[:source_prefix])
                if changed:
                    self.plan_path=self._raster_plan_cells(display) or [tuple(self.current_nav)]
            elif self.plan_corner_path:
                changed=anchors != self.plan_corner_path or not old_path or old_path[0] != self.current_nav
                self.plan_corner_path=anchors
                if changed:self.plan_path=self._cells_from_corner_path(anchors,self._plan_tight_clear,start=tuple(self.current_nav))
            else:
                changed=anchors != points or not old_path or old_path[0] != self.current_nav
                self.plan_path=[(int(math.floor(x)),int(math.floor(y))) for x,y in anchors]
                if self.plan_path[0] != self.current_nav:self.plan_path.insert(0,tuple(self.current_nav))
            if not raw and self._plan_tight_clear is not None and not all(
                    self.in_bounds(x,y) and self._plan_tight_clear[y,x] for x,y in self.plan_path):
                continue
            self._plan_display_points=display
            if changed:self._annotate_plan(self.plan_path)
            return bool(self.plan_path)
        return False

    @staticmethod
    def _raster_plan_cells(raw):
        """Batch the existing5mm linspace sampling, preserving endpoint ownership."""
        if len(raw)<2:return []
        points=np.asarray(raw,float);delta=np.diff(points,axis=0)
        counts=np.asarray([max(2,math.ceil(math.dist(a,b)*6)+1)for a,b in zip(raw,raw[1:])],np.int64)
        labels=np.repeat(np.arange(len(counts)),counts)
        local=np.arange(int(counts.sum()))-np.repeat(np.cumsum(np.r_[0,counts])[:-1],counts)
        samples=points[labels]+local[:,None]*(delta[labels]/(counts[labels]-1)[:,None])
        end=local==counts[labels]-1;samples[end]=points[labels[end]+1]
        cells=np.floor(samples).astype(np.int64);keep=np.r_[True,np.any(cells[1:]!=cells[:-1],axis=1)]
        return list(map(tuple,cells[keep].tolist()))

    def _route_tail_clear(self, anchors):
        """Prove all fixed suffixes once; current connector/join stay uncached.

        The batch-independent turn primitive preserves its scalar safety bound.
        A suffix starting i excludes the turn AT i, whose incoming direction is
        today's connector, and includes every internal turn strictly after i.
        """
        kind="raw" if self.plan_raw_points else "corner" if self.plan_corner_path else "cell"
        points=tuple(anchors)
        key=(self._plan_graph_signature,kind,points)
        cached=self._plan_tail_proof
        if (cached is not None and len(cached)>2
                and cached[0][:2] == key[:2]
                and len(points)<=len(cached[0][2])
                and cached[0][2][-len(points):] == points):
            return cached[2][len(cached[0][2])-len(points)]
        self._check_plan_cancelled()
        escape=self._initial_escape
        self._initial_escape=None  # Tail anchors cannot inherit a body-departure exception.
        try:
            n=len(points)
            valid=np.ones(n,dtype=bool)
            valid[-1]=self._raw_segment_clear(points[-1],points[-1])
            if kind == "corner":
                valid &= np.array([0<=x<=self.n and 0<=y<=self.n
                    and self._plan_corner_clear[int(y),int(x)]>=1 for x,y in points])
            elif kind == "cell" and self._plan_tight_clear is not None:
                valid &= np.array([self.in_bounds(int(math.floor(x)),int(math.floor(y)))
                    and self._plan_tight_clear[int(math.floor(y)),int(math.floor(x))] for x,y in points])
            edges=np.zeros(max(0,n-1),dtype=bool)
            physical=np.zeros(max(0,n-1),dtype=bool)
            p=np.asarray(points)
            # The same body/front-sensor predicate is independent per segment.
            # Bound query-owner temporaries; never batch an entire long route.
            for first in range(0,n-1,512):
                self._check_plan_cancelled()
                last=min(first+512,n-1)
                physical[first:last]=self._physical_segment_mask(
                    p[first:last,0],p[first:last,1],p[first+1:last+1,0],p[first+1:last+1,1])
                self._check_plan_cancelled()
            for i,(a,b) in enumerate(zip(points,points[1:])):
                self._check_plan_cancelled()
                edges[i]=(physical[i] and self._observed_segment_clear(a,b)
                    and (kind != "corner" or self._corner_segment_clear(a,b,physical_proven=True)))
            turns=np.ones(n,dtype=bool)
            if n>2:
                p=np.asarray(points);v=np.diff(p,axis=0);angle=np.arctan2(v[:,1],v[:,0])
                self._check_plan_cancelled()
                turns[1:-1]=self._heading_turn_mask(p[1:-1,0],p[1:-1,1],angle[:-1],angle[1:])
                self._check_plan_cancelled()
            valid[:-1] &= edges & turns[1:]
            suffix=np.logical_and.accumulate(valid[::-1])[::-1]
            self._check_plan_cancelled()
        finally:
            self._initial_escape=escape
        suffix=tuple(map(bool,suffix))
        self._plan_tail_proof=key,suffix[0],suffix
        return suffix[0]

    def _refresh_displayed_plan(self):
        """Reuse an exact current-head proof; new pose/map inputs revalidate it."""
        if not self.plan_path:return
        # A geometry certificate never substitutes for a fresh legal packet.
        if self.async_planning and not self._planning_pose_current():
            self._set_plan_failed("planning (no current pose)")
            return
        if self._reject_invalid_plan_endpoints():
            return
        self._planning_maps(prepare_escape=False)
        self._check_plan_cancelled()
        mapper = self._planning_mapper
        def proof_key():
            return (self._plan_graph_signature, tuple(self.current_world_mm), self.current_yaw_deg,
                tuple(self.current_nav) if self.current_nav is not None else None,
                self._plan_request_id, tuple(self.plan_destination) if self.plan_destination is not None else None,
                tuple(self.plan_destination_corner) if self.plan_destination_corner is not None else None,
                tuple(self.plan_destination_click) if self.plan_destination_click is not None else None,
                tuple(map(tuple,self.plan_path)), tuple(map(tuple,self.plan_corner_path)),
                tuple(map(tuple,self.plan_raw_points)),
                tuple(self._plan_source_point) if self._plan_source_point is not None else None,
                tuple(map(tuple,self._plan_display_points or ())),
                self.NAV_GRID_MM, self.n, self.c,
                (mapper.GRID_MM, mapper.n, mapper.c) if mapper is not None else None,
                OccupancyMapper.YAW_OFFSET_DEG, OccupancyMapper.YAW_SIGN)
        key = proof_key()
        fresh = self._planning_pose_current()
        if fresh and getattr(self, "_plan_display_proof", None) == key:
            self._plan_display_checked_t = self.last_update_t
            return
        # Exact same current-head geometry needs no repeated departure query.
        # Any map, pose, yaw, request or route change prepares it before proof.
        self._prepare_initial_escape()
        if not self._rebase_plan(self._snapshot_plan()):
            self._set_plan_failed("planning (previous route no longer clear)")
        elif fresh:
            self._plan_display_proof = proof_key()
        self._plan_display_checked_t = self.last_update_t

    def _poll_plan_job(self):
        if self._reject_invalid_plan_endpoints():
            return
        job=self._plan_job
        if job is None:return
        def current_proof(candidate):
            # This short-lived attestation is created only after this packet's
            # successful rebase and installation. Never cache it across calls.
            context=(candidate._plan_graph_signature,candidate.current_world_mm,
                candidate.current_yaw_deg,candidate.last_update_t,candidate._current_pose_for_planning,
                candidate.current_nav,candidate._plan_request_id,candidate.plan_destination,
                candidate.plan_destination_corner,candidate.plan_destination_click)
            return (candidate,context,candidate._snapshot_plan(),
                tuple(candidate._plan_display_points),candidate._plan_tail_proof)
        future,worker,request_id,key=job
        if self.async_planning and not self._planning_pose_current():
            worker._plan_cancel_event.set()
            if future.done():self._plan_job=None
            if request_id == self._plan_request_id and self.plan_destination is not None:
                self._set_plan_failed("planning (no current pose)")
            self._plan_completed_input=None
            self.plan_last_t=-1e9
            return
        if not future.done():
            try:preview,points=worker._plan_preview_queue.get_nowait()
            except Empty:return
            if request_id != self._plan_request_id or self.plan_destination is None:return
            previous=self._snapshot_plan()
            previous_proof=self._plan_tail_proof
            if self._plan_geometry_cache is None:self._plan_geometry_cache=worker._plan_geometry_cache
            self._planning_maps()
            # A provisional comparison cannot replace a valid committed
            # route. Only this job's final result enters hysteresis.
            committed=copy.copy(self)
            if (previous["status"].startswith("ok")
                    and previous["destination"] == self.plan_destination
                    and previous["destination_corner"] == self.plan_destination_corner
                    and committed._rebase_plan(previous)):
                self._restore_plan(committed._snapshot_plan())
                self._plan_display_points=committed._plan_display_points
                self._plan_tail_proof=committed._plan_tail_proof
                self.plan_status="ok (kept stable path)"
                self._plan_display_checked_t=self.last_update_t
                return
            candidate=copy.copy(self)
            if candidate._rebase_plan(preview):
                self._restore_plan(candidate._snapshot_plan())
                self._plan_display_points=candidate._plan_display_points
                self._plan_tail_proof=candidate._plan_tail_proof
                self.plan_status="ok"
                if not self._retain_previous_if_stable(previous, previous_proof, current_proof(candidate)):
                    self.plan_status="ok (refining)"
                    self._plan_preview_owner=(worker,request_id)
            self._plan_display_checked_t=self.last_update_t
            return
        own_preview=self._plan_preview_owner == (worker,request_id)
        self._plan_preview_owner=None
        self._plan_job=None
        try:future.result()
        except CancelledError:
            self.plan_last_t=-1e9
            return
        except Exception as error:
            if request_id == self._plan_request_id:
                self._set_plan_failed("planning failed: "+str(error))
                self._plan_completed_input=key
                self.plan_last_t=self.last_update_t
            return
        if request_id != self._plan_request_id or self.plan_destination is None:
            self.plan_last_t=-1e9
            return
        if self._plan_geometry_cache is None:self._plan_geometry_cache=worker._plan_geometry_cache
        self._planning_maps()
        if self._plan_graph_signature == worker._planning_input_signature:
            self._plan_graph_cache=worker._plan_graph_cache
            self._plan_search_cache=worker._plan_search_cache
        self.plan_last_t=self.last_update_t
        self._plan_completed_input=key
        self._plan_display_checked_t=self.last_update_t
        previous=self._snapshot_plan()
        previous_proof=self._plan_tail_proof
        candidate=copy.copy(self)
        if worker.plan_status == "ok" and candidate._rebase_plan(worker._snapshot_plan()):
            self._restore_plan(candidate._snapshot_plan())
            self._plan_display_points=candidate._plan_display_points
            self._plan_tail_proof=candidate._plan_tail_proof
            self.plan_status="ok"
            # Refine this job against its committed predecessor, not its
            # own coarse preview, whose graph objective may differ.
            incumbent=worker._plan_committed_previous if own_preview else previous
            self._retain_previous_if_stable(incumbent, None if own_preview else previous_proof, current_proof(candidate))
        elif previous["status"].startswith("ok") and self._rebase_plan(previous):
            self.plan_status="ok (kept stable path)"
        else:self._set_plan_failed(worker.plan_status if worker.plan_status != "ok" else "planning (map or pose changed)")

    def close_planner(self):
        self._plan_preview_owner = None
        self._planner_closed = True
        if self._plan_job is not None:
            self._plan_job[1]._plan_cancel_event.set()
        if self._plan_executor is not None:
            self._plan_executor.shutdown(wait=True, cancel_futures=True)
        self._plan_executor = self._plan_job = None

    def _plan_path(self, start, goal):
        self._check_plan_cancelled()
        self._plan_display_points = None
        self.plan_raw_points = []
        self._plan_source_point = None
        self._plan_source_prefix=()
        requested_corner = self.plan_destination_corner
        reason = self._plan_endpoint_failure(start, goal)
        if reason is not None:
            self._set_plan_failed(reason)
            return
        walkable, normal_clear, tight_clear, wall_dist = self._planning_maps()
        self._plan_wall_dist = wall_dist
        self._plan_normal_clear = normal_clear
        self._plan_tight_clear = tight_clear

        sx, sy = start
        gx, gy = goal
        if self._plan_minimum_cost_straight(start, goal, requested_corner):
            return
        corner_plan = None
        if tight_clear[gy, gx]:
            if (self._plan_corner_path_with_scipy(start, goal, walkable, normal_clear,
                    tight_clear, wall_dist,
                    min_corner_clear=self.PATH_CLEARANCE_TARGET_CELLS)
                    or self._plan_corner_path_with_scipy(start, goal, walkable, normal_clear,
                    tight_clear, wall_dist, min_corner_clear=1)):
                corner_plan = self._snapshot_plan()
            # The two 30 mm lattices sample different safe lanes. A valid
            # corner route can still make a long detour around a passage whose
            # safe centre lies between corners. Compare the existing cell graph
            # before accepting that detour; keep its physical and known-floor gates.
            self.plan_corner_path = []
            self._plan_path_with_scipy(start, goal, walkable, normal_clear,
                                      tight_clear, wall_dist)
            if corner_plan is not None and (self.plan_status != "ok"
                    or self._stable_plan_score(corner_plan)
                       <= self._stable_plan_score(self._snapshot_plan())):
                self._restore_plan(corner_plan)
                self.plan_status = corner_plan["status"]
        else:
            self._set_plan_failed("destination has no coarse-grid clearance")
        if self._planning_mapper is not None:
            # A successful 30 mm route can still miss a lower-cost lane that
            # lies between both coarse lattices. Compare the existing fine
            # graph under the same cost and swept-body/known-floor gates.
            coarse_plan = self._snapshot_plan() if self.plan_status == "ok" else None
            if coarse_plan is not None:
                self._offer_plan_preview()
            self.plan_corner_path = []
            self._plan_path_with_scipy(start, goal, walkable, normal_clear,
                                      tight_clear, wall_dist, fine=True,
                                      requested_corner=requested_corner)
            if coarse_plan is not None and (self.plan_status != "ok"
                    or self._stable_plan_score(coarse_plan)
                       <= self._stable_plan_score(self._snapshot_plan())):
                self._restore_plan(coarse_plan)
                self.plan_status = coarse_plan["status"]

    def _query_physical_plan_graph(self,key,graph,sources,goals,node_xy,positions,
                                   native=False,observe_edges=True):
        """Exact lazy proof with at most eight candidate searches plus fallback.

        The heading count is an algorithm-selection work budget, not a
        clearance threshold. On exhaustion prove every remaining transition
        and query the exact graph once; no path or cost is abandoned.
        """
        for array in (graph.data,graph.indices,graph.indptr):array.flags.writeable=False
        escape=self._initial_escape
        escape_key=(None if escape is None else
            (tuple(escape['source']),np.asarray(escape['excluded_indices']).tobytes(),
             id(escape['remaining'])))
        tag=(bool(observe_edges),self._plan_graph_signature,
             None if escape is None or self.current_world_mm is None else tuple(self.current_world_mm),
             None if escape is None else self.current_yaw_deg,
             id(self._plan_material_tree),id(self._plan_obstacle_tree),self._plan_grid_mm,
             escape_key,None if escape is None else self._current_pose_for_planning ==
                (self.current_world_mm,self.current_yaw_deg,self.last_update_t))
        cached=getattr(graph,'_physical_query_cache',None)
        if cached is not None and cached[0] == tag:
            active,proved=cached[1:]
        else:
            active,proved=graph.copy(),np.zeros(graph.nnz,dtype=bool)
        px,py=positions
        sources=set(map(int,sources));goals=np.asarray(goals,dtype=np.intp)
        stats=dict(iterations=0,proved_transitions=0,rejected_transitions=0,
                   possible_transitions=graph.nnz,cache_hit=cached is not None and cached[0]==tag,
                   eager_fallback=False,fallback_transitions=0)
        def freeze():
            for a in (active.data,active.indices,active.indptr,proved):a.flags.writeable=False
        def publish():
            freeze();graph._physical_query_cache=tag,active,proved
            self._plan_coarse_physical_stats=stats
        def failed(dist,pred):
            bad=dist.copy();bad[goals]=np.inf;bad.flags.writeable=False
            publish();return bad,pred
        def prove(edges,before,after):
            nonlocal active,proved
            self._check_plan_cancelled()
            # One directed translation has the same body/front and known-floor
            # proof for every incoming heading. Keep only this <=512 batch.
            pairs,owners=np.unique(np.column_stack((before//8,after//8)),
                                   axis=0,return_inverse=True)
            a,b=pairs[:,0],pairs[:,1]
            physical=self._physical_segment_mask(px[a],py[a],px[b],py[b])
            turn=np.zeros(len(edges),dtype=bool)
            good=np.flatnonzero(physical[owners])
            if len(good):
                turn[good]=self._turn_mask_from_dir(px[(before//8)[good]],py[(before//8)[good]],
                    before[good]%8,after[good]%8)
            observed=np.ones(len(pairs),dtype=bool)
            if observe_edges and self._planning_mapper is not None:
                for owner in np.unique(owners[turn]):
                    self._check_plan_cancelled()
                    observed[owner]=self._observed_segment_clear(
                        (px[a[owner]],py[a[owner]]),(px[b[owner]],py[b[owner]]))
            self._check_plan_cancelled()
            if not proved.flags.writeable:proved=proved.copy()
            rejected=[]
            for j,edge in enumerate(edges):
                self._check_plan_cancelled();owner=int(owners[j])
                if physical[owner] and turn[j] and observed[owner]:
                    proved[edge]=True;stats['proved_transitions']+=1
                elif not physical[owner] or not observed[owner]:
                    for heading in range(8):
                        row=int(a[owner])*8+heading
                        lo,hi=int(active.indptr[row]),int(active.indptr[row+1])
                        match=np.flatnonzero(active.indices[lo:hi]==after[j])
                        rejected.extend(lo+int(k)for k in match)
                else:rejected.append(int(edge))
            if not rejected:return 0
            rejected=np.unique(rejected)
            removed=int(np.count_nonzero(np.isfinite(active.data[rejected])))
            assert removed
            if not active.data.flags.writeable:active=active.copy()
            # Invalidate before mutation: cancellation during a later batch
            # must never leave distances cached for this changed graph.
            previous=self._plan_search_cache.get(key)
            metric=previous[5] if previous is not None else None
            self._plan_search_cache[key]=active,(),None,None,None,metric
            active.data[rejected]=np.inf;stats['rejected_transitions']+=removed
            return removed
        try:
            for _ in range(len(self.PATH_DIR_VECS)):
                self._check_plan_cancelled();stats['iterations']+=1
                dist,pred=(self._query_native_plan_graph(key,active,sorted(sources)) if native
                    else self._query_plan_graph(key,active,sorted(sources),goals,node_xy))
                cur=int(goals[np.argmin(dist[goals])])
                if not np.isfinite(dist[cur]):publish();return dist,pred
                path=[]
                for _ in range(graph.shape[0]):
                    self._check_plan_cancelled();path.append(cur)
                    if cur in sources:break
                    cur=int(pred[cur])
                    if not 0<=cur<graph.shape[0]:return failed(dist,pred)
                else:return failed(dist,pred)
                path=np.asarray(path[::-1],dtype=np.intp)
                if len(path)==1:
                    node=int(path[0])//8;a=(px[node],py[node])
                    if (not self._physical_segment_clear(a,a)
                            or (observe_edges and self._planning_mapper is not None and not self._observed_segment_clear(a,a))):
                        return failed(dist,pred)
                    publish();return dist,pred
                edges=[]
                for a,b in zip(path,path[1:]):
                    lo,hi=int(active.indptr[a]),int(active.indptr[a+1])
                    index=np.flatnonzero(active.indices[lo:hi]==b)
                    if len(index)!=1:return failed(dist,pred)
                    edges.append(lo+int(index[0]))
                edges=np.asarray(edges,dtype=np.intp);unknown=np.flatnonzero(~proved[edges])
                removed=0
                for first in range(0,len(unknown),512):
                    ids=unknown[first:first+512]
                    removed+=prove(edges[ids],path[ids],path[ids+1])
                if not removed:publish();return dist,pred
            # A finite graph can still contain millions of candidate paths.
            # Bound repeated SSSP work, then finish all exact proofs once.
            stats['eager_fallback']=True
            for first in range(0,graph.nnz,512):
                self._check_plan_cancelled()
                edges=np.arange(first,min(first+512,graph.nnz),dtype=np.intp)
                edges=edges[(~proved[edges])&np.isfinite(active.data[edges])]
                if not len(edges):continue
                before=np.searchsorted(active.indptr,edges,side='right')-1
                after=active.indices[edges]
                stats['fallback_transitions']+=len(edges)
                prove(edges,before,after)
            self._check_plan_cancelled();stats['iterations']+=1
            dist,pred=(self._query_native_plan_graph(key,active,sorted(sources)) if native
                    else self._query_plan_graph(key,active,sorted(sources),goals,node_xy))
            publish();return dist,pred
        finally:
            freeze()  # Never expose writable arrays, including cancellation.

    def _plan_fine_path_indexed(self, start, goal, normal_clear, wall_dist, requested_corner):
        """A* over the same fine heading graph, generating only queried edges.

        No world graph with 92 million edges is constructed. Four fixed-size state
        arrays bound search storage to 20 bytes per possible heading state;
        each state enters an indexed heap once and is expanded at most once.
        Nonnegative costs and an octile lower bound preserve shortest cost.
        """
        self._check_plan_cancelled()
        mapper = self._planning_mapper
        key = ("fine", self.PATH_FINE_GRID_MM)
        model = self._plan_graph_cache.get(key)
        if model is None:
            ky,kx = np.nonzero((self.cell_state != self.CELL_UNKNOWN)
                               & (self.floor != self.FLOOR_HOLE))
            if not len(kx):
                self._set_plan_failed("no known safe cells")
                return
            grid = self.PATH_FINE_GRID_MM
            bound = mapper.c*mapper.GRID_MM
            def coordinates(lo,hi):
                low = max(-bound,(lo-self.c)*self.NAV_GRID_MM)
                high = min(bound,(hi+1-self.c)*self.NAV_GRID_MM)
                return np.arange(math.ceil(low/grid)*grid, high+grid, grid)
            xc,yc = coordinates(kx.min(),kx.max()),coordinates(ky.min(),ky.max())
            rx = np.rint(xc/mapper.GRID_MM).astype(int)+mapper.c
            ry = np.rint(yc/mapper.GRID_MM).astype(int)+mapper.c
            cx = np.clip(np.floor(xc/self.NAV_GRID_MM).astype(int)+self.c,0,self.n-1)
            cy = np.clip(np.floor(yc/self.NAV_GRID_MM).astype(int)+self.c,0,self.n-1)
            observed = ((self.cell_state != self.CELL_UNKNOWN)
                        & (self.floor != self.FLOOR_HOLE))[cy[:,None],cx[None,:]]
            observed &= mapper.logodds[np.ix_(ry,rx)] <= mapper.FREE_T
            yy,xx = np.nonzero(observed)
            count = len(xx)
            node = np.full(observed.shape,-1,dtype=np.int32)
            node[yy,xx] = np.arange(count,dtype=np.int32)
            component,_ = scipy_ndimage.label(observed)
            model = dict(xc=xc,yc=yc,cx=cx,cy=cy,xx=xx,yy=yy,node=node,observed=observed,
                component=component,body=np.full(count,-1,dtype=np.int8),
                edge_known=np.zeros(count,dtype=np.uint8),edge_clear=np.zeros(count,dtype=np.uint8),
                turn_known=np.zeros((count,8),dtype=np.uint8),turn_clear=np.zeros((count,8),dtype=np.uint8),
                full_turn=np.full(count,-1,dtype=np.int8))
            self._plan_graph_cache[key] = model
        xc,yc,cx,cy,xx,yy,node,observed,component = (model[k] for k in
            ("xc","yc","cx","cy","xx","yy","node","observed","component"))
        count = len(xx)
        def positions(ids):
            return xc[xx[ids]]/self.NAV_GRID_MM+self.c, yc[yy[ids]]/self.NAV_GRID_MM+self.c
        def bodies(ids):
            unknown = ids[model['body'][ids] < 0]
            if len(unknown):
                x,y = positions(unknown)
                model['body'][unknown] = self._physical_segment_mask(x,y,x,y)
            return model['body'][ids] > 0
        source = (tuple(v/self.NAV_GRID_MM+self.c for v in self.current_world_mm)
                  if self.current_world_mm is not None else (start[0]+.5,start[1]+.5))
        target = requested_corner if requested_corner is not None else (goal[0]+.5,goal[1]+.5)
        def nearest(point, reverse=False):
            ix = np.flatnonzero(abs(xc/self.NAV_GRID_MM+self.c-point[0]) <= 1)
            iy = np.flatnonzero(abs(yc/self.NAV_GRID_MM+self.c-point[1]) <= 1)
            ids = node[np.ix_(iy,ix)].ravel()
            ids = ids[ids >= 0]
            ids = ids[bodies(ids)]
            px,py = positions(ids)
            order = np.lexsort((ids,(px-point[0])**2+(py-point[1])**2))
            for j in order:
                pos = float(px[j]),float(py[j])
                a,b = (pos,point) if reverse else (point,pos)
                if self._raw_segment_clear(a,b):
                    return int(ids[j])
            return None
        sn,gn = nearest(source),nearest(target,reverse=True)
        if sn is None or gn is None:
            self._set_plan_failed("no observed fine-grid connection to start or destination")
            return
        if component[yy[sn],xx[sn]] != component[yy[gn],xx[gn]]:
            self._set_plan_failed("no path through known safe cells")
            return
        if sn == gn:
            route = [sn]
            best_cost = 0.
        else:
            sx,sy = positions(np.array([sn]))
            headings = np.flatnonzero(self._start_heading_mask(float(sx[0]),float(sy[0])))
            if not len(headings):
                self._set_plan_failed("no safe initial heading")
                return
            search_key = sn,gn,tuple(map(int,headings))
            previous_search = model.get('search')
            if previous_search is not None and previous_search[0] == search_key:
                if previous_search[1] is None:
                    self._set_plan_failed("no path through known safe cells")
                    self._plan_fine_search_stats = dict(possible_nodes=count,possible_states=count*8,
                        expansions=0,state_storage_bytes=0,cache_hit=True)
                    return
                route,best_cost = previous_search[1],previous_search[2]
                self._plan_fine_search_stats = dict(possible_nodes=count,possible_states=count*8,
                    expansions=0,state_storage_bytes=0,cache_hit=True)
            else:
                state_count = count*8
                distance = np.full(state_count,np.inf,dtype=np.float64)
                predecessor = np.full(state_count,-1,dtype=np.int32)
                heap = np.empty(state_count,dtype=np.int32)
                where = np.full(state_count,-1,dtype=np.int32)
                size = 0
                scale = self.PATH_FINE_GRID_MM/self.NAV_GRID_MM
                cardinal,diagonal = float(np.float32(scale)),float(np.float32(math.sqrt(2)*scale))
                cost_grid = self._path_cost_grid(normal_clear,wall_dist)
                heuristic = self._fine_relaxed_goal_distances(model,gn,cost_grid,cardinal,diagonal)
                turn = np.array([self._turn_costs_from_dir(i,np.arange(8)) for i in range(8)])
                right = np.array([self._right_turn_mask_from_dir(i,np.arange(8)) for i in range(8)])
                vectors = np.asarray(self.PATH_DIR_VECS)
                angles = np.arctan2(vectors[:,1],vectors[:,0])
                def priority(state):
                    n = int(state)//8
                    h = float(heuristic[n])
                    return distance[state]+h,h,int(state)
                def push(state):
                    nonlocal size
                    i = int(where[state])
                    if i == -2:
                        return
                    if i < 0:
                        i = size;size += 1;heap[i] = state;where[state] = i
                    while i:
                        parent = (i-1)//2
                        if priority(heap[parent]) <= priority(state):
                            break
                        heap[i] = heap[parent];where[heap[i]] = i;i = parent
                    heap[i] = state;where[state] = i
                def pop():
                    nonlocal size
                    state = int(heap[0]);size -= 1;where[state] = -2
                    if size:
                        last = int(heap[size]);i = 0
                        while i*2+1 < size:
                            child = i*2+1
                            if child+1 < size and priority(heap[child+1]) < priority(heap[child]):
                                child += 1
                            if priority(last) <= priority(heap[child]):
                                break
                            heap[i] = heap[child];where[heap[i]] = i;i = child
                        heap[i] = last;where[last] = i
                    return state
                starts = set((sn*8+headings).tolist())
                for state in sorted(starts):
                    distance[state] = 0.;push(state)
                best = None
                expansions = 0
                while size:
                    self._check_plan_cancelled()
                    state = pop();n,previous = divmod(state,8);expansions += 1
                    if n == gn:
                        best = state;break
                    nx,ny = xx[n]+vectors[:,0],yy[n]+vectors[:,1]
                    valid = (nx>=0)&(ny>=0)&(nx<node.shape[1])&(ny<node.shape[0])
                    dirs = np.flatnonzero(valid)
                    neighbours = node[ny[dirs],nx[dirs]]
                    valid[dirs] &= neighbours >= 0
                    dirs = np.flatnonzero(valid);neighbours = node[ny[dirs],nx[dirs]]
                    valid[dirs] &= bodies(neighbours)
                    for d in (4,5,6,7):
                        if valid[d] and not (observed[yy[n],nx[d]] and observed[ny[d],xx[n]]):
                            valid[d] = False
                    dirs = np.flatnonzero(valid)
                    unknown = np.array([d for d in dirs if not (int(model['edge_known'][n]) & (1<<int(d)))],dtype=int)
                    if len(unknown):
                        ax,ay = positions(np.array([n]));dest = node[ny[unknown],nx[unknown]]
                        bx,by = positions(dest)
                        clear = self._physical_segment_mask(np.full(len(dest),ax[0]),np.full(len(dest),ay[0]),bx,by)
                        for d,ok in zip(unknown,clear):
                            model['edge_known'][n] |= np.uint8(1<<int(d))
                            if ok:model['edge_clear'][n] |= np.uint8(1<<int(d))
                    dirs = np.array([d for d in dirs if int(model['edge_clear'][n]) & (1<<int(d))],dtype=int)
                    unknown = np.array([d for d in dirs if not (int(model['turn_known'][n,previous]) & (1<<int(d)))],dtype=int)
                    if len(unknown):
                        ax,ay = positions(np.array([n]))
                        if model['full_turn'][n] < 0:
                            model['full_turn'][n] = self._disk_segment_mask(float(ax[0]),float(ay[0]),float(ax[0]),float(ay[0]),
                                self.FRONT_SENSOR_OFFSET_MM+self.FRONT_SENSOR_RADIUS_MM,self._plan_material_tree)
                        clear = (np.ones(len(unknown),dtype=bool) if model['full_turn'][n] else
                            self._heading_turn_mask_model(
                                np.full(len(unknown),ax[0]),np.full(len(unknown),ay[0]),
                                angles[previous],angles[unknown],self._plan_obstacle_tree,
                                body_clear=bool(model['body'][n] > 0),
                                full_turn_clear=bool(model['full_turn'][n])))
                        for d,ok in zip(unknown,clear):
                            model['turn_known'][n,previous] |= np.uint8(1<<int(d))
                            if ok:model['turn_clear'][n,previous] |= np.uint8(1<<int(d))
                    for d in dirs:
                        if not (int(model['turn_clear'][n,previous]) & (1<<int(d))):continue
                        dst = int(node[ny[d],nx[d]]);following = dst*8+int(d)
                        if where[following] == -2:continue
                        step = np.float32(diagonal if d>=4 else cardinal)
                        base = np.float32(cost_grid[cy[ny[d]],cx[nx[d]]]*step)
                        extra = np.float32(self.PATH_OPEN_RIGHT_TURN_EXTRA_COST if right[previous,d]
                            and normal_clear[cy[yy[n]],cx[xx[n]]] and normal_clear[cy[ny[d]],cx[nx[d]]] else 0.)
                        weight = np.float32(np.float32(base+turn[previous,d])+extra)
                        new = distance[state]+float(weight)
                        if new < distance[following]:
                            distance[following]=new;predecessor[following]=state;push(following)
                self._plan_fine_search_stats = dict(possible_nodes=count,possible_states=state_count,
                    expansions=expansions,state_storage_bytes=distance.nbytes+predecessor.nbytes+heap.nbytes+where.nbytes)
                if best is None:
                    model['search'] = search_key,None,None
                    self._set_plan_failed("no path through known safe cells")
                    return
                best_cost = float(distance[best]);route=[];state=best
                for _ in range(state_count):
                    self._check_plan_cancelled();route.append(state//8)
                    if state in starts:break
                    state=int(predecessor[state])
                    if not 0 <= state < state_count:
                        self._set_plan_failed("path reconstruction failed");return
                else:
                    self._set_plan_failed("path predecessor cycle");return
                route.reverse()
                model['search'] = search_key,tuple(route),best_cost
        px,py = positions(np.asarray(route,dtype=int))
        points = [source]+list(zip(px.tolist(),py.tolist()))+[target]
        simplified=[]
        for point in points:
            if simplified and point == simplified[-1]:continue
            if (len(simplified)>=2 and self._path_step_dir(simplified[-2],simplified[-1]) == self._path_step_dir(simplified[-1],point)
                    and abs((simplified[-1][0]-simplified[-2][0])*(point[1]-simplified[-1][1])
                            -(simplified[-1][1]-simplified[-2][1])*(point[0]-simplified[-1][0])) < 1e-10):
                simplified[-1]=point
            else:simplified.append(point)
        if (not self._raw_polyline_clear(simplified)
                or not self._path_turns_clear(simplified)):
            self._set_plan_failed("fine path failed final observation or clearance check")
            return
        self.plan_path=[]
        for x,y in points:
            cell=int(math.floor(x)),int(math.floor(y))
            if not self.plan_path or self.plan_path[-1]!=cell:self.plan_path.append(cell)
        self.plan_destination=goal
        self.plan_destination_corner=requested_corner
        self.plan_corner_path=[]
        self.plan_raw_points=simplified
        self._plan_source_point=source
        self._plan_source_prefix=(tuple(source),)
        self.plan_cost=best_cost
        self.plan_status="ok"
        self._annotate_plan(self.plan_path)
        for annotation in self.plan_annotations:
            x,y=annotation['cell']
            annotation['corner']=min(simplified,key=lambda q:(q[0]-x-.5)**2+(q[1]-y-.5)**2)

    def _query_native_plan_graph(self,key,graph,source_states):
        """Bounded native fine query; exact existing float32 CSR weights."""
        self._check_plan_cancelled()
        states=tuple(map(int,source_states));previous=self._plan_search_cache.get(key)
        if previous is not None and previous[0] is graph and previous[1] == states and previous[4] is None:
            return previous[2],previous[3]
        dist,pred,_=scipy_dijkstra(graph,directed=True,indices=states,
            min_only=True,return_predecessors=True)
        self._check_plan_cancelled()
        dist.flags.writeable=pred.flags.writeable=False
        self._plan_search_cache[key]=graph,states,dist,pred,None,None
        return dist,pred


    def _plan_minimum_cost_straight(self, start, goal, requested_corner):
        """Return only a physically proven attained global route-cost bound.

        Every route pays at least minimum cell cost times endpoint distance;
        bends add nonnegative cost. A straight segment entirely at that minimum
        attains the bound, so constructing either search graph cannot improve
        the current boundary-integral objective. Other cases keep both searches.
        """
        if (self._planning_mapper is None or self.current_world_mm is None
                or self.current_yaw_deg is None
                or any(not math.isfinite(float(v)) for v in
                       (*self.current_world_mm, self.current_yaw_deg))
                or not all(math.isfinite(v) and v >= 0 for v in
                           (self.PATH_TURN_COST, self.PATH_RIGHT_TURN_EXTRA_COST,
                            self.PATH_OPEN_RIGHT_TURN_EXTRA_COST))):
            return False
        source = tuple(v / self.NAV_GRID_MM + self.c for v in self.current_world_mm)
        target = tuple(requested_corner) if requested_corner is not None else (goal[0]+.5, goal[1]+.5)
        if any(not math.isfinite(float(v)) for v in target):
            return False
        direction = np.subtract(target, source)
        # Exact binary-rational cuts prevent a near-corner crossing from
        # rounding its tiny positive-length cell interval onto its neighbour.
        origin = tuple(Fraction(float(v)) for v in source)
        delta = tuple(Fraction(float(v))-a for v, a in zip(target, origin))
        cuts = {Fraction(0), Fraction(1)}
        for axis in (0, 1):
            if delta[axis] != 0:
                lo, hi = sorted((source[axis], target[axis]))
                if lo < 0 or hi >= self.n:
                    return False
                cuts.update((v-origin[axis])/delta[axis]
                            for v in range(math.floor(lo)+1, math.ceil(hi)))
        cuts = sorted(cuts)
        cells = np.asarray([tuple(math.floor(a+(lo+hi)/2*d) for a, d in zip(origin, delta))
                            for lo, hi in zip(cuts, cuts[1:])], dtype=int)
        if np.any((cells < 0) | (cells >= self.n)):
            return False
        if (np.any(self.cell_state[cells[:, 1], cells[:, 0]] == self.CELL_UNKNOWN)
                or np.any(self.floor[cells[:, 1], cells[:, 0]] == self.FLOOR_HOLE)):
            return False
        grid = self._path_cost_grid(self._plan_normal_clear, self._plan_wall_dist)
        minimum = float(grid.min())
        if not (math.isfinite(minimum) and minimum > 0
                and np.all(grid[cells[:, 1], cells[:, 0]] == minimum)):
            return False
        self._check_plan_cancelled()
        points = [source] if source == target else [source, target]
        if (not self._raw_segment_clear(source, target)
                or not self._path_turns_clear(points)):
            return False
        if source == target:
            heading = math.pi/2-math.radians(OccupancyMapper.YAW_OFFSET_DEG
                                           + OccupancyMapper.YAW_SIGN*self.current_yaw_deg)
            if not self._heading_turn_mask(*source, heading, heading):
                return False
        self._check_plan_cancelled()
        path = []
        for cell in [tuple(map(math.floor, source)), *map(tuple, cells), tuple(map(math.floor, target))]:
            if not path or cell != path[-1]:
                path.append(cell)
        self.plan_path = path
        self.plan_corner_path = []
        self.plan_raw_points = points
        self._plan_source_point = source
        self._plan_source_prefix=(tuple(source),)
        self.plan_destination = goal
        self.plan_destination_corner = requested_corner
        self.plan_cost = float(np.linalg.norm(direction))*minimum
        self.plan_status = "ok"
        self._annotate_plan(path)
        for annotation in self.plan_annotations:
            x, y = annotation['cell']
            annotation['corner'] = min(points, key=lambda p: (p[0]-x-.5)**2+(p[1]-y-.5)**2)
        return True


    def _snapshot_plan(self):
        return {
            "destination": self.plan_destination,
            "destination_corner": self.plan_destination_corner,
            "requested_corner": self.plan_requested_corner,
            "path": list(self.plan_path),
            "corner_path": list(self.plan_corner_path),
            "raw_points": list(self.plan_raw_points),
            "source_point": self._plan_source_point if self.plan_raw_points else None,
            "source_prefix": self._plan_source_prefix if self.plan_raw_points else (),
            "waypoints": list(self.plan_waypoints),
            "annotations": [dict(a) for a in self.plan_annotations],
            "status": self.plan_status,
            "cost": self.plan_cost,
        }

    def _restore_plan(self, snapshot):
        self.plan_destination = snapshot["destination"]
        self.plan_destination_corner = snapshot["destination_corner"]
        self.plan_requested_corner = snapshot.get("requested_corner", self.plan_requested_corner)
        self.plan_path = list(snapshot["path"])
        self.plan_corner_path = list(snapshot["corner_path"])
        self.plan_raw_points = list(snapshot.get("raw_points", []))
        self._plan_source_point = snapshot.get("source_point")
        self._plan_source_prefix=tuple(map(tuple,snapshot.get('source_prefix',())))
        self.plan_waypoints = list(snapshot["waypoints"])
        self.plan_annotations = [dict(a) for a in snapshot["annotations"]]
        self.plan_cost = snapshot["cost"]

    def _should_keep_previous_plan(self, previous, start, goal, prepared=None, current_proof=None):
        if previous.get("requested_corner", self.plan_requested_corner) != self.plan_requested_corner:
            return False
        if not previous["status"].startswith("ok") or not previous["path"]:
            return False
        if (previous["destination_corner"] != self.plan_destination_corner
                or previous["destination"] != self.plan_destination):
            return False
        # A prepared result belongs only to this synchronous transaction and
        # exact input snapshot/pose/map/request. It never caches a live source.
        context=lambda obj:(obj._plan_graph_signature,obj.current_world_mm,
            obj.current_yaw_deg,obj.last_update_t,obj._current_pose_for_planning,
            obj.current_nav,obj._plan_request_id,obj.plan_destination,
            obj.plan_destination_corner,obj.plan_destination_click)
        ready=(prepared is not None and prepared[0] is previous
            and prepared[1] == context(self) and context(prepared[2]) == context(self))
        retained=prepared[2] if ready else copy.copy(self)
        if ready and self.current_world_mm is not None:
            source=tuple(v/self.NAV_GRID_MM+self.c for v in self.current_world_mm)
            ready=bool(retained._plan_display_points
                and tuple(retained._plan_display_points[0]) == source)
        if self.current_world_mm is None:
            if previous["path"][0] != start or not self._previous_plan_still_valid(previous,goal):
                return False
            retained._restore_plan(previous)
        elif not ready and not retained._rebase_plan(previous):
            return False
        if self.plan_status != "ok":return True
        old=retained._snapshot_plan()
        if self.current_world_mm is None and ((old["corner_path"] and old["corner_path"] == self.plan_corner_path)
                or old["path"] == self.plan_path):
            return True
        if self._distance_to_path_cells(start,old["path"]) > self.PATH_KEEP_NEAR_CELLS:
            return False
        if self.current_world_mm is None:
            old_score,new_score=self._stable_plan_score(old),self._stable_plan_score(self._snapshot_plan())
            shared=min(old_score-float(old["cost"]),new_score-float(self.plan_cost))
        else:
            self._check_plan_cancelled()
            candidate=None
            if (current_proof is not None and current_proof[1] == context(self)
                    and context(current_proof[0]) == context(self)
                    and (not self.async_planning or self._planning_pose_current())):
                proved,_,snapshot,display,tail=current_proof
                installed=self._snapshot_plan();bound=snapshot.copy();actual=proved._snapshot_plan()
                # Poll normalizes the installed status to "ok"; status is
                # not part of geometry, objective or route identity.
                installed["status"]=bound["status"]=actual["status"]=None
                source=tuple(v/self.NAV_GRID_MM+self.c for v in self.current_world_mm)
                if (installed == bound == actual and display and display[0] == source
                        and tuple(self._plan_display_points or ()) == display
                        and tuple(proved._plan_display_points or ()) == display
                        and self._plan_tail_proof is tail is proved._plan_tail_proof):
                    candidate=proved
            if candidate is None:
                candidate=copy.copy(self)
                if not candidate._rebase_plan(self._snapshot_plan()):return True
            policy=getattr(self,"_route_policy_choice",None)
            if policy is not None:
                choice=policy(retained,candidate)
                if choice:return choice<0
            old_score,old_additive=retained._remaining_route_score()
            if retained._plan_remaining_cost_cache[0] == self._plan_graph_signature == candidate._plan_graph_signature:
                self._plan_remaining_cost_cache=retained._plan_remaining_cost_cache
                candidate._plan_remaining_cost_cache=self._plan_remaining_cost_cache
            new_score,new_additive=candidate._remaining_route_score()
            shared=min(old_additive,new_additive)
        scale=max(0.,abs(old_score)-shared)
        required=max(self.PATH_SWITCH_MIN_ABS_GAIN,scale*self.PATH_SWITCH_MIN_REL_GAIN)
        return new_score >= old_score-required

    def _remaining_route_score(self):
        """One current-map objective for all retained/refined physical polylines.

        Historical graph totals remain plan_cost. Terrain and the existing grid,
        wall and clearance preferences are integrated at exact 30 mm boundaries;
        existing angular/open-turn terms are charged once per physical bend.
        """
        key=self._plan_graph_signature
        cached=self._plan_remaining_cost_cache
        if cached is None or cached[0] != key:
            grid=self._path_cost_grid(self._plan_normal_clear,self._plan_wall_dist)
            terrain=np.where(self.floor==self.FLOOR_SWAMP,self.PATH_SWAMP_COST,1.)
            cached=key,grid,grid-terrain
            self._plan_remaining_cost_cache=cached
        _,grid,additive=cached
        points=self._plan_display_points
        if points is None:
            points=list(self.plan_raw_points) if self.plan_raw_points else list(self.plan_corner_path) if self.plan_corner_path else [(x+.5,y+.5) for x,y in self.plan_path]
        points=[tuple(p) for i,p in enumerate(points) if i==0 or tuple(p)!=tuple(points[i-1])]
        score=offset=0.
        for a,b in zip(points,points[1:]):
            d=np.subtract(b,a);length=float(np.linalg.norm(d));cuts=[0.,1.]
            for axis in (0,1):
                if d[axis] != 0:
                    lo,hi=sorted((a[axis],b[axis]))
                    cuts.extend((v-a[axis])/d[axis] for v in range(math.floor(lo)+1,math.ceil(hi)))
            cuts=np.unique(cuts);mid=(cuts[:-1]+cuts[1:])/2
            xy=np.asarray(a)+mid[:,None]*d
            x,y=np.floor(xy).astype(int).T
            if np.any((x<0)|(x>=self.n)|(y<0)|(y>=self.n)):return float("inf"),0.
            weights=np.diff(cuts)*length
            score+=float(weights@grid[y,x]);offset+=float(weights@additive[y,x])
        for a,b,c in zip(points,points[1:],points[2:]):
            u,v=np.subtract(b,a),np.subtract(c,b)
            angle=math.acos(float(np.clip(np.dot(u,v)/(np.linalg.norm(u)*np.linalg.norm(v)),-1.,1.)))
            fraction=angle/(math.pi/2)
            score+=self.PATH_TURN_COST*fraction
            if angle >= math.pi/2-1e-6:
                score+=self.PATH_RIGHT_TURN_EXTRA_COST*fraction
                step=min(1.,self.PATH_FINE_GRID_MM/(self.NAV_GRID_MM*float(np.linalg.norm(v))))
                first=np.asarray(b)+step*v
                bx,by=np.clip(np.floor(b).astype(int),0,self.n-1)
                cx,cy=np.clip(np.floor(first).astype(int),0,self.n-1)
                if self._plan_normal_clear[by,bx] and self._plan_normal_clear[cy,cx]:
                    score+=self.PATH_OPEN_RIGHT_TURN_EXTRA_COST
        return score,offset

    def _retain_previous_if_stable(self, previous, previous_proof=None, current_proof=None):
        if previous.get("requested_corner", self.plan_requested_corner) != self.plan_requested_corner:
            return False
        if self._reject_invalid_plan_endpoints():
            return False
        retained=copy.copy(self)
        if previous_proof is not None:
            retained._plan_tail_proof=previous_proof
        context=lambda obj:(obj._plan_graph_signature,obj.current_world_mm,
            obj.current_yaw_deg,obj.last_update_t,obj._current_pose_for_planning,
            obj.current_nav,obj._plan_request_id,obj.plan_destination,
            obj.plan_destination_corner,obj.plan_destination_click)
        tag=context(self)
        if self.current_world_mm is None:retained._restore_plan(previous)
        elif not retained._rebase_plan(previous):return False
        if not self._should_keep_previous_plan(previous,tuple(self.current_nav),
                self.plan_destination,prepared=(previous,tag,retained),current_proof=current_proof):
            return False
        if context(self) != tag:
            return False  # A changed packet/goal/map cannot publish an old rebase.
        self._restore_plan(retained._snapshot_plan())
        self._plan_display_points=retained._plan_display_points
        self._plan_tail_proof=retained._plan_tail_proof
        self.plan_status="ok (kept stable path)"
        return True

    def _previous_plan_still_valid(self, previous, goal):
        if self.current_world_mm is not None:
            if (previous["destination"] != self.plan_destination
                    or previous["destination_corner"] != self.plan_destination_corner):
                return False
            return copy.copy(self)._rebase_plan(previous)
        if previous.get("raw_points"):
            return (previous["destination"] == self.plan_destination
                    and self._path_turns_clear(previous["raw_points"])
                    and all(self._raw_segment_clear(a, b) for a, b in zip(
                        previous["raw_points"], previous["raw_points"][1:])))
        points = previous["corner_path"] or previous["path"]
        if len(points) == 1 and self.current_world_mm is not None:
            anchor = points[0] if previous["corner_path"] else (points[0][0]+.5, points[0][1]+.5)
            source = tuple(v/self.NAV_GRID_MM+self.c for v in self.current_world_mm)
            if (not self._raw_segment_clear(source,anchor)
                    or not self._path_turns_clear([source,anchor])):
                return False
        if (previous["corner_path"] and self.plan_destination_corner is not None
                and previous["corner_path"][-1] == self.plan_destination_corner):
            pass
        elif previous["path"][-1] != goal and self.plan_destination is not None:
            if previous["path"][-1] != self.plan_destination:
                return False
        if self._plan_tight_clear is None:
            return True
        for gx, gy in previous["path"]:
            if not self.in_bounds(gx, gy) or not self._plan_tight_clear[gy, gx]:
                return False
        if previous["corner_path"] and self._plan_corner_clear is not None:
            if not self._path_turns_clear(previous["corner_path"]):
                return False
            for ex, ey in previous["corner_path"]:
                if not (0 <= ex <= self.n and 0 <= ey <= self.n):
                    return False
                if self._plan_corner_clear[ey, ex] < 1:
                    return False
            if not all(self._corner_segment_clear(a, b) for a, b in zip(
                    previous["corner_path"], previous["corner_path"][1:])):
                return False
        elif previous["path"]:
            points = [(x + 0.5, y + 0.5) for x, y in previous["path"]]
            if (not self._path_turns_clear(points)
                    or not all(self._physical_segment_clear(a, b) for a, b in zip(points, points[1:]))):
                return False
        return True

    def _stable_plan_score(self, plan):
        if plan["cost"] is None:
            return float("inf")
        score = float(plan["cost"])
        if self._plan_clearance_deficit is not None:
            score += self.PATH_CLEARANCE_DEFICIT_PENALTY * sum(
                int(self._plan_clearance_deficit[gy, gx])
                for gx, gy in plan["path"]
                if self.in_bounds(gx, gy))
        if self._plan_corner_clear is not None:
            for ex, ey in plan["corner_path"]:
                if 0 <= ex <= self.n and 0 <= ey <= self.n:
                    deficit = max(0, self.PATH_CLEARANCE_TARGET_CELLS
                                  - int(self._plan_corner_clear[ey, ex]))
                    score += self.PATH_CLEARANCE_DEFICIT_PENALTY * deficit
        return score

    @staticmethod
    def _distance_to_path_cells(cell, path):
        gx, gy = cell
        return min(max(abs(gx - px), abs(gy - py)) for px, py in path)

    @staticmethod
    def _points_checksum(points):
        h = 1469598103934665603
        for x, y in points:
            for value in (int(round(x*1000)), int(round(y*1000))):
                h ^= value & 0xffffffff
                h = (h * 1099511628211) & 0xffffffffffffffff
        return f"{len(points)}:{h:016x}"

    def _nearest_nav_corner_edge(self, gx, gy):
        return (max(0, min(self.n, int(math.floor(gx + 1.0)))),
                max(0, min(self.n, int(math.floor(gy + 1.0)))))

    def _corner_adjacent_cell(self, corner, allowed=None, preferred=None):
        ex, ey = corner
        candidates = []
        for gx in (ex - 1, ex):
            for gy in (ey - 1, ey):
                if not self.in_bounds(gx, gy):
                    continue
                if allowed is not None and not allowed[gy, gx]:
                    continue
                px, py = preferred if preferred is not None else (gx, gy)
                ownership = (int(self.cell_state[gy, gx] == self.CELL_UNKNOWN
                                 or self.floor[gy, gx] == self.FLOOR_HOLE)
                             if preferred is None and allowed is None else 0)
                candidates.append((ownership + abs(gx - px) + abs(gy - py), gx, gy))
        if not candidates:
            return None
        _, gx, gy = min(candidates)
        return gx, gy

    def _safe_plan_corners(self, corner, allowed=None, preferred=None):
        """Nearest known, physically valid30mm endpoints within one tile.

        Exact distance wins; equal-distance choices prefer the start-anchored
        60mm lattice. Reachability is proved by the actual route query, never
        inferred from a cell's colour or a coarse clearance approximation.
        """
        ex0, ey0 = corner
        radius = self.PATH_DESTINATION_ADJUST_CELLS
        options = [(ex, ey)
            for ex in range(max(0, int(ex0)-radius), min(self.n, int(ex0)+radius)+1)
            for ey in range(max(0, int(ey0)-radius), min(self.n, int(ey0)+radius)+1)
            if (ex-ex0)**2+(ey-ey0)**2 <= radius**2]
        options.sort(key=lambda p: ((p[0]-ex0)**2+(p[1]-ey0)**2,
            (p[0]-self.c)%2 != 0 or (p[1]-self.c)%2 != 0, p))
        for point in options:
            self._check_plan_cancelled()
            if (self._corner_adjacent_cell(point, allowed=allowed, preferred=preferred) is not None
                    and self._raw_segment_clear(point, point)):
                yield point

    def _nearest_safe_plan_corner(self, corner, allowed=None, preferred=None):
        return next(self._safe_plan_corners(corner, allowed, preferred), None)

    def _query_plan_graph(self, key, graph, source_states, goal_states=None, node_xy=None):
        """Cache exact graph searches; target large queries instead of all goals.

        Small graphs keep SciPy's all-goal search. On larger heading graphs an
        indexed heap visits only states needed for this goal, with bounded
        state storage and cancellation per expansion. Its octile potential uses
        the actual minimum cardinal/diagonal edge weights, capped to a metric;
        no edge, cost or physical gate of the graph is changed.
        """
        states = tuple(map(int,source_states))
        targeted = goal_states is not None and graph.shape[0] > self.n*self.n
        targets = tuple(map(int,goal_states)) if targeted else None
        previous = self._plan_search_cache.get(key)
        same_graph = previous is not None and previous[0] is graph
        if same_graph and previous[1] == states and previous[4] == targets:
            return previous[2],previous[3]
        if not targeted:
            self._check_plan_cancelled()
            dist,pred,_ = scipy_dijkstra(graph,directed=True,indices=source_states,
                min_only=True,return_predecessors=True)
            self._check_plan_cancelled()
            metric = None
        else:
            metric = previous[5] if same_graph else None
            if metric is None:
                straight = across = math.inf
                for begin in range(0,graph.nnz,65536):
                    self._check_plan_cancelled()
                    end = min(begin+65536,graph.nnz)
                    diagonal = graph.indices[begin:end] % 8 >= 4
                    values = graph.data[begin:end]
                    straight = min(straight,float(values[~diagonal].min(initial=np.inf)))
                    across = min(across,float(values[diagonal].min(initial=np.inf)))
                straight = min(straight,across)
                if not math.isfinite(straight):
                    straight = 0.
                across = min(across,2*straight)
                metric = straight,across
            straight,across = metric
            xs,ys = node_xy
            goal_node = targets[0]//8
            potential = np.empty(len(xs),dtype=np.float64)
            for begin in range(0,len(xs),4096):
                self._check_plan_cancelled()
                end = min(begin+4096,len(xs))
                dx,dy = abs(xs[begin:end]-xs[goal_node]),abs(ys[begin:end]-ys[goal_node])
                potential[begin:end] = np.minimum(dx,dy)*across+abs(dx-dy)*straight
            count = graph.shape[0]
            dist = np.full(count,np.inf,dtype=np.float64)
            pred = np.full(count,-9999,dtype=np.int32)
            heap = np.empty(count,dtype=np.int32)
            where = np.full(count,-1,dtype=np.int32)
            size = expansions = 0
            def priority(state):
                h = float(potential[int(state)//8])
                return dist[state]+h,h,int(state)
            def push(state):
                nonlocal size
                i = int(where[state])
                if i == -2:
                    return
                if i < 0:
                    i = size;size += 1
                while i:
                    parent = (i-1)//2
                    if priority(heap[parent]) <= priority(state):
                        break
                    heap[i] = heap[parent];where[heap[i]] = i;i = parent
                heap[i] = state;where[state] = i
            def pop():
                nonlocal size
                state = int(heap[0]);size -= 1;where[state] = -2
                if size:
                    last = int(heap[size]);i = 0
                    while 2*i+1 < size:
                        child = 2*i+1
                        if child+1 < size and priority(heap[child+1]) < priority(heap[child]):
                            child += 1
                        if priority(last) <= priority(heap[child]):
                            break
                        heap[i] = heap[child];where[heap[i]] = i;i = child
                    heap[i] = last;where[last] = i
                return state
            for state in states:
                dist[state] = 0.;push(state)
            while size:
                self._check_plan_cancelled()
                state = pop();expansions += 1
                if state in targets:
                    break
                for edge in range(int(graph.indptr[state]),int(graph.indptr[state+1])):
                    dst = int(graph.indices[edge])
                    if where[dst] == -2:
                        continue
                    candidate = dist[state]+float(graph.data[edge])
                    if candidate < dist[dst]:
                        dist[dst] = candidate;pred[dst] = state;push(dst)
            self._plan_coarse_search_stats = dict(possible_states=count,
                expansions=expansions,state_storage_bytes=count*20,targeted=True)
        dist.flags.writeable = pred.flags.writeable = False
        self._plan_search_cache[key] = graph,states,dist,pred,targets,metric
        return dist,pred

    def _plan_corner_path_with_scipy(self, start, goal, walkable, normal_clear,
                                     tight_clear, wall_dist, min_corner_clear=None):
        self._check_plan_cancelled()
        if self._plan_corner_clear is None:
            return False
        if min_corner_clear is None:
            min_corner_clear = self.PATH_CLEARANCE_TARGET_CELLS
        start_corner = self._best_start_corner_for_cell(start, min_corner_clear)
        goal_corner = self._nearest_safe_plan_corner(
            self.plan_destination_corner or self._path_cell_corner_point(*goal),
            allowed=walkable & tight_clear,
            preferred=goal)
        if start_corner is None or goal_corner is None:
            return False
        self.plan_destination_corner = goal_corner
        snapped_goal = self._corner_adjacent_cell(
            goal_corner, allowed=walkable & tight_clear, preferred=goal)
        if snapped_goal is not None:
            self.plan_destination = snapped_goal

        cache_key = ("corner", min_corner_clear)
        endpoints = (start_corner, goal_corner)
        cached = self._plan_graph_cache.get(cache_key)
        if cached is None or (cached[0] is not None and cached[0] != endpoints):
            n = self.n
            corner_y, corner_x = np.indices((n+1, n+1))
            adjacent_known = np.zeros((n + 1, n + 1), dtype=bool)
            known_floor = walkable & tight_clear
            adjacent_known[:-1, :-1] |= known_floor
            adjacent_known[1:, :-1] |= known_floor
            adjacent_known[:-1, 1:] |= known_floor
            adjacent_known[1:, 1:] |= known_floor

            allowed = ((self._plan_corner_clear >= min_corner_clear)
                       & adjacent_known)
            # Restore stationary body-fit nodes before graph construction,
            # querying only known candidates in bounded physical batches.
            known_y,known_x = np.nonzero(allowed)
            for first in range(0,len(known_x),512):
                self._check_plan_cancelled()
                ky,kx = known_y[first:first+512],known_x[first:first+512]
                allowed[ky,kx] = self._physical_segment_mask(kx,ky,kx,ky)
            sx, sy = start_corner
            gx, gy = goal_corner
            # If both endpoints are already ordinaryvalid nodes, this graph
            # is independent of this click/pose and can serve the next request.
            static_endpoints = bool(allowed[sy,sx] and allowed[gy,gx])
            allowed[sy, sx] = True
            allowed[gy, gx] = True
            ys, xs = np.nonzero(allowed)
            node_count = int(xs.size)
            if node_count == 0:
                return False

            node = np.full((n + 1, n + 1), -1, dtype=np.int32)
            node[ys, xs] = np.arange(node_count, dtype=np.int32)
            start_node = int(node[sy, sx])
            goal_node = int(node[gy, gx])
            if start_node < 0 or goal_node < 0:
                return False

            edge_rows = []
            edge_cols = []
            edge_costs = []
            edge_dirs = []

            def add_corner_edges(src, dst, clear, step_cost, direction, extra=None):
                src_nodes = node[src]
                dst_nodes = node[dst]
                m = clear & (src_nodes >= 0) & (dst_nodes >= 0)
                if not np.any(m):
                    return
                costs = np.full(int(np.count_nonzero(m)), np.float32(step_cost), dtype=np.float32)
                if extra is not None:
                    costs += extra[m].astype(np.float32, copy=False)
                edge_rows.append(src_nodes[m])
                edge_cols.append(dst_nodes[m])
                edge_costs.append(costs)
                edge_dirs.append(np.full(costs.shape, direction, dtype=np.int8))

            target = min_corner_clear
            h_clear = self._plan_h_lane_clear >= target
            v_clear = self._plan_v_lane_clear >= target
            # Known cells beside the endpoints do not prove the intervening lane is
            # seen. Require known safe floor along each edge, including frontier gaps.
            h_known = np.zeros(self.wall_h.shape, dtype=bool)
            h_known[:-1] |= known_floor
            h_known[1:] |= known_floor
            v_known = np.zeros(self.wall_v.shape, dtype=bool)
            v_known[:, :-1] |= known_floor
            v_known[:, 1:] |= known_floor
            h_clear &= h_known
            v_clear &= v_known
            h_def_cost = self._clearance_deficit_from_distance(
                self._plan_h_lane_clear).astype(np.float32) * np.float32(
                    self.PATH_CLEARANCE_DEFICIT_PENALTY)
            v_def_cost = self._clearance_deficit_from_distance(
                self._plan_v_lane_clear).astype(np.float32) * np.float32(
                    self.PATH_CLEARANCE_DEFICIT_PENALTY)
            # Swamp is traversable terrain with a cost, not a wall or a pit. Apply
            # the same terrain penalty to the corner graph and the fallback graph.
            swamp = self.floor == self.FLOOR_SWAMP
            h_swamp = np.zeros(self.wall_h.shape, dtype=bool)
            h_swamp[:-1] |= swamp
            h_swamp[1:] |= swamp
            v_swamp = np.zeros(self.wall_v.shape, dtype=bool)
            v_swamp[:, :-1] |= swamp
            v_swamp[:, 1:] |= swamp
            h_def_cost += h_swamp * (self.PATH_SWAMP_COST-1)
            v_def_cost += v_swamp * (self.PATH_SWAMP_COST-1)
            add_corner_edges((slice(None), slice(0, n)),
                             (slice(None), slice(1, n + 1)),
                             h_clear & ~self.wall_h, 1.0, 0, h_def_cost)
            add_corner_edges((slice(None), slice(1, n + 1)),
                             (slice(None), slice(0, n)),
                             h_clear & ~self.wall_h, 1.0, 1, h_def_cost)
            add_corner_edges((slice(0, n), slice(None)),
                             (slice(1, n + 1), slice(None)),
                             v_clear & ~self.wall_v, 1.0, 2, v_def_cost)
            add_corner_edges((slice(1, n + 1), slice(None)),
                             (slice(0, n), slice(None)),
                             v_clear & ~self.wall_v, 1.0, 3, v_def_cost)

            corner_def = self._clearance_deficit_from_distance(self._plan_corner_clear)
            diag_cost = np.maximum(corner_def[:-1, :-1], corner_def[1:, 1:]).astype(np.float32)
            diag_cost *= np.float32(self.PATH_CLEARANCE_DEFICIT_PENALTY)
            diag_clear = ((self._plan_corner_clear[:-1, :-1] >= target)
                          & (self._plan_corner_clear[1:, 1:] >= target)
                          & known_floor)
            root2 = np.float32(math.sqrt(2.0))
            swamp_extra = swamp * ((self.PATH_SWAMP_COST-1)*root2)
            diag_cost += swamp_extra
            add_corner_edges((slice(0, n), slice(0, n)),
                             (slice(1, n + 1), slice(1, n + 1)),
                             diag_clear, root2, 4, diag_cost)
            diag_clear = ((self._plan_corner_clear[:-1, 1:] >= target)
                          & (self._plan_corner_clear[1:, :-1] >= target)
                          & known_floor)
            diag_cost = np.maximum(corner_def[:-1, 1:], corner_def[1:, :-1]).astype(np.float32)
            diag_cost *= np.float32(self.PATH_CLEARANCE_DEFICIT_PENALTY)
            diag_cost += swamp_extra
            add_corner_edges((slice(0, n), slice(1, n + 1)),
                             (slice(1, n + 1), slice(0, n)),
                             diag_clear, root2, 5, diag_cost)
            add_corner_edges((slice(1, n + 1), slice(0, n)),
                             (slice(0, n), slice(1, n + 1)),
                             diag_clear, root2, 6, diag_cost)
            diag_clear = ((self._plan_corner_clear[1:, 1:] >= target)
                          & (self._plan_corner_clear[:-1, :-1] >= target)
                          & known_floor)
            diag_cost = np.maximum(corner_def[1:, 1:], corner_def[:-1, :-1]).astype(np.float32)
            diag_cost *= np.float32(self.PATH_CLEARANCE_DEFICIT_PENALTY)
            diag_cost += swamp_extra
            add_corner_edges((slice(1, n + 1), slice(1, n + 1)),
                             (slice(0, n), slice(0, n)),
                             diag_clear, root2, 7, diag_cost)

            if not edge_rows:
                return False

            base_rows = np.concatenate(edge_rows)
            base_cols = np.concatenate(edge_cols)
            base_costs = np.concatenate(edge_costs)
            base_dirs = np.concatenate(edge_dirs)
            state_rows = []
            state_cols = []
            state_costs = []
            for prev_dir in range(8):
                self._check_plan_cancelled()
                state_rows.append(base_rows * 8 + prev_dir)
                state_cols.append(base_cols * 8 + base_dirs)
                turn = self._turn_costs_from_dir(prev_dir, base_dirs)
                open_right = self._right_turn_mask_from_dir(prev_dir, base_dirs)
                open_right_cost = open_right.astype(np.float32, copy=False)
                open_right_cost *= np.float32(self.PATH_OPEN_RIGHT_TURN_EXTRA_COST)
                state_costs.append(base_costs + turn.astype(np.float32, copy=False)
                                   + open_right_cost)

            graph = scipy_sparse.csr_matrix(
                (np.concatenate(state_costs),
                 (np.concatenate(state_rows), np.concatenate(state_cols))),
                shape=(node_count*8, node_count*8),
            )
            for array in (graph.data,graph.indices,graph.indptr):array.flags.writeable=False
            cached = (None if static_endpoints else endpoints, node, ys, xs, graph)
            self._plan_graph_cache[cache_key] = cached
        _, node, ys, xs, graph = cached
        sx, sy = start_corner
        gx, gy = goal_corner
        start_node, goal_node = int(node[sy,sx]), int(node[gy,gx])
        headings = np.flatnonzero(self._start_heading_mask(sx,sy))
        if not len(headings):
            return False
        source_states = start_node*8+headings
        goal_states = goal_node * 8 + np.arange(8, dtype=np.int32)
        dist,pred = self._query_physical_plan_graph(
            cache_key,graph,source_states,goal_states,(xs,ys),(xs,ys),observe_edges=False)
        best_goal_state = int(goal_states[np.argmin(dist[goal_states])])
        if not np.isfinite(dist[best_goal_state]):
            return False

        state_path = []
        cur = best_goal_state
        for _ in range(graph.shape[0]):
            self._check_plan_cancelled()
            state_path.append(cur)
            if cur in source_states:
                break
            cur = int(pred[cur])
            if not 0 <= cur < graph.shape[0]:
                return False
        else:
            return False
        state_path.reverse()
        corner_nodes = []
        for state in state_path:
            node_id = int(state) // 8
            if not corner_nodes or corner_nodes[-1] != node_id:
                corner_nodes.append(node_id)
        corners = [(int(xs[p]), int(ys[p])) for p in corner_nodes]
        self.plan_corner_path = self._smooth_path_corners(corners)
        if self._path_turns_clear(self.plan_corner_path):
            # Publish the already proven graph route before optional shortcuts
            # and other graph comparisons refine it in this same worker.
            self.plan_path = self._cells_from_corner_path(
                self.plan_corner_path,walkable & tight_clear,start=start)
            self.plan_cost = float(dist[best_goal_state])
            self.plan_status = "ok"
            self._annotate_plan(self.plan_path)
            self._offer_plan_preview()
        # Graph legs were proved above. Smoothing only retains those legs or
        # substitutes a leg after its identical physical capsule succeeds.
        # This worker's map/pose cannot change across this same-call handoff.
        self.plan_corner_path = self._shortcut_visited_corner_path(
            self.plan_corner_path,physical_legs_proven=True)
        if not self._path_turns_clear(self.plan_corner_path):
            self.plan_corner_path = []
            return False
        self.plan_path = self._cells_from_corner_path(
            self.plan_corner_path, walkable & tight_clear, start=start)
        self.plan_cost = float(dist[best_goal_state])
        self.plan_status = "ok"
        self._annotate_plan(self.plan_path)
        return True

    def _best_start_corner_for_cell(self, cell, min_corner_clear):
        gx, gy = cell
        best = None
        for ex in (gx, gx + 1):
            for ey in (gy, gy + 1):
                if not (0 <= ex <= self.n and 0 <= ey <= self.n):
                    continue
                if self._corner_on_wall(ex, ey):
                    continue
                clear = 255
                if self._plan_corner_clear is not None:
                    clear = int(self._plan_corner_clear[ey, ex])
                    if clear < min_corner_clear:
                        continue
                position = (gx+0.5, gy+0.5)
                if self.current_world_mm is not None:
                    position = tuple(v/self.NAV_GRID_MM+self.c for v in self.current_world_mm)
                if not self._physical_segment_clear(position, (ex, ey)):
                    continue
                grid_bonus = 0
                if (ex - self.c) % 4 in (1, 2) or (ey - self.c) % 4 in (1, 2):
                    grid_bonus = 2
                elif (ex - self.c) % 2 == 0 or (ey - self.c) % 2 == 0:
                    grid_bonus = 1
                score = (clear, grid_bonus, -abs(ex - (gx + 0.5)) - abs(ey - (gy + 0.5)))
                if best is None or score > best[0]:
                    best = (score, (ex, ey))
        return best[1] if best is not None else None

    def _cells_from_corner_path(self, corners, allowed, start=None):
        cells = []
        if start is not None and self.in_bounds(*start) and allowed[start[1], start[0]]:
            cells.append(start)
        for a, b in zip(corners, corners[1:]):
            # Smoothing can join many graph edges. Keep every crossed cell for
            # validity checks and floor costs, not just the first cell of a leg.
            steps = max(abs(b[0]-a[0]), abs(b[1]-a[1]))
            for k in range(steps):
                x = a[0] + (k+0.5)*(b[0]-a[0])/steps
                y = a[1] + (k+0.5)*(b[1]-a[1])/steps
                candidates = [(int(math.floor(x)), int(math.floor(y)))]
                if a[0] == b[0]:
                    candidates.append((int(x)-1, int(math.floor(y))))
                elif a[1] == b[1]:
                    candidates.append((int(math.floor(x)), int(y)-1))
                valid = [c for c in candidates if self.in_bounds(*c) and allowed[c[1], c[0]]]
                if valid and (not cells or cells[-1] != valid[0]):
                    cells.append(valid[0])
        if not cells and corners:
            adjacent = self._corner_adjacent_cell(corners[0], allowed=allowed)
            if adjacent is not None:
                cells.append(adjacent)
        return cells

    def _fine_relaxed_goal_distances(self, model, goal, cost_grid, cardinal, diagonal, prepare_only=False):
        """An admissible reverse-cost potential, with bounded C graph work.

        Uniform observed costs use their exact float32 octile lower bound.
        Other small domains retain the orientation-free fine graph. Larger
        domains contract every known 30 mm cell to a group: free internal
        movement and one destination-cardinal cost per crossing relax every
        actual fine edge. The C graph has at most the 30 mm world's node count;
        actual physical gates, heading costs and goal semantics stay unchanged.
        """
        cached = model.get('relaxed_goal')
        if cached is not None and cached[0] == int(goal):
            return cached[1]
        xx,yy = model['xx'],model['yy']
        count = len(xx)
        mode = model.get('relaxed_mode')
        if mode is None:
            uniform = None
            minimum = np.float32(np.inf)
            same = True
            for begin in range(0,count,4096):
                self._check_plan_cancelled()
                end = min(begin+4096,count)
                values = cost_grid[model['cy'][yy[begin:end]],model['cx'][xx[begin:end]]]
                if uniform is None:
                    uniform = values[0]
                minimum = min(minimum,values.min())
                same = same and bool(np.all(values == uniform))
            model['relaxed_min_cost'] = minimum
            if same:
                mode = 'uniform'
                model['relaxed_uniform_cost'] = uniform
            if mode is None:
                mode = 'fine' if count <= self.n*self.n else 'contracted'
            model['relaxed_mode'] = mode
        if ('body' in model and mode != 'uniform'
                and self._plan_obstacle_tree is not None):
            # The real fine search already rejects a node whose stationary
            # body does not fit. Compute those SAME immutable proofs in batches
            # before relaxing the graph, rather than guiding through walls.
            if not model.get('relaxed_body_checked',False):
                # If every nonminimum-cost node is physically unusable, all
                # feasible fine nodes have the same base cost. Prove only that
                # exceptional set, then use the existing uniform relaxation;
                # testing the whole minimum-cost world would add no guidance.
                minimum = model['relaxed_min_cost']
                for begin in range(0,count,4096):
                    self._check_plan_cancelled()
                    end = min(begin+4096,count)
                    ids = np.arange(begin,end)
                    values = cost_grid[model['cy'][yy[ids]],model['cx'][xx[ids]]]
                    high = ids[values > minimum]
                    unknown = high[model['body'][high] < 0]
                    if len(unknown):
                        x = model['xc'][xx[unknown]]/self.NAV_GRID_MM+self.c
                        y = model['yc'][yy[unknown]]/self.NAV_GRID_MM+self.c
                        model['body'][unknown] = self._physical_segment_mask(x,y,x,y)
                    if np.any(model['body'][high] > 0):
                        break
                else:
                    model['relaxed_mode'] = 'uniform'
                    model['relaxed_uniform_cost'] = minimum
                    return self._fine_relaxed_goal_distances(
                        model,goal,cost_grid,cardinal,diagonal,prepare_only=prepare_only)
                fitted = int(np.count_nonzero(model['body'] > 0))
                minimum_fits = False
                for begin in range(0,count,4096):
                    self._check_plan_cancelled()
                    end = min(begin+4096,count)
                    ids = np.flatnonzero(model['body'][begin:end] < 0)+begin
                    if len(ids):
                        x = model['xc'][xx[ids]]/self.NAV_GRID_MM+self.c
                        y = model['yc'][yy[ids]]/self.NAV_GRID_MM+self.c
                        fits = self._physical_segment_mask(x,y,x,y)
                        model['body'][ids] = fits
                        fitted += int(np.count_nonzero(fits))
                    good = np.flatnonzero(model['body'][begin:end] > 0)+begin
                    values = cost_grid[model['cy'][yy[good]],model['cx'][xx[good]]]
                    minimum_fits = minimum_fits or bool(np.any(values == minimum))
                    if mode == 'contracted' and fitted > self.n*self.n and minimum_fits:
                        # A higher-cost fit was proved above. With the original
                        # minimum retained and more than the existing fine-graph
                        # budget fitting, full compression MUST select the same
                        # contracted graph and octile bound. Stop redundant body
                        # work; untested nodes keep their lazy exact proof path.
                        model['relaxed_body_contract_unchanged'] = True
                        break
                if not model.get('relaxed_body_contract_unchanged',False):
                    kept = np.flatnonzero(model['body'] > 0).astype(np.int32)
                    if len(kept) < count:
                        bx,by = xx[kept],yy[kept]
                        body_node = np.full(model['node'].shape,-1,dtype=np.int32)
                        body_node[by,bx] = np.arange(len(kept),dtype=np.int32)
                        child = dict(xx=bx,yy=by,node=body_node,cx=model['cx'],cy=model['cy'])
                        model['relaxed_body_model'] = kept,child
                model['relaxed_body_checked'] = True
            body_model = model.get('relaxed_body_model')
            if body_model is not None:
                kept,child = body_model
                body_goal = int(np.searchsorted(kept,int(goal)))
                # nearest() already proved the requested fine goal fits.
                if body_goal >= len(kept) or kept[body_goal] != int(goal):
                    raise RuntimeError('fine goal contradicts its exact body proof')
                if prepare_only:
                    return None
                potential = self._fine_relaxed_goal_distances(
                    child,body_goal,cost_grid,cardinal,diagonal)
                distances = np.full(count,np.inf,dtype=np.float64)
                distances[kept] = potential
                distances.flags.writeable = False
                model['relaxed_goal'] = (int(goal),distances)
                return distances
        # Native SciPy needs the same body proofs but never consumes this
        # goal potential. Indexed A* keeps the default and builds it below.
        if prepare_only:
            return None
        if mode == 'uniform':
            cost = model['relaxed_uniform_cost']
            straight = float(np.float32(cost*np.float32(cardinal)))
            across = min(float(np.float32(cost*np.float32(diagonal))),2*straight)
            distances = np.empty(count,dtype=np.float64)
            for begin in range(0,count,4096):
                self._check_plan_cancelled()
                end = min(begin+4096,count)
                dx = np.abs(xx[begin:end]-xx[int(goal)])
                dy = np.abs(yy[begin:end]-yy[int(goal)])
                distances[begin:end] = np.minimum(dx,dy)*across+np.abs(dx-dy)*straight
        else:
            layout = model
            if mode == 'contracted':
                layout = model.get('relaxed_layout')
                if layout is None:
                    gy,gx = np.nonzero((self.cell_state != self.CELL_UNKNOWN)
                                      & (self.floor != self.FLOOR_HOLE))
                    group = np.full(self.floor.shape,-1,dtype=np.int32)
                    group[gy,gx] = np.arange(len(gx),dtype=np.int32)
                    layout = dict(xx=gx,yy=gy,node=group,
                                  cx=np.arange(self.n),cy=np.arange(self.n))
                    model['relaxed_layout'] = layout
            graph_goal = int(goal) if mode == 'fine' else int(layout['node'][
                model['cy'][yy[int(goal)]],model['cx'][xx[int(goal)]]])
            native = (self._query_relaxed_grid(layout, graph_goal, cost_grid,
                cardinal, diagonal, mode) if getattr(NavigationMapper,
                '_compiled_relaxed_step', None) is not None else None)
            if native is not None:
                potential, exponent = native
                model['relaxed_quantum_exponent'] = exponent
            else:
                graph = model.get('relaxed_reverse_graph')
                if graph is None:
                    lx,ly,node = layout['xx'],layout['yy'],layout['node']
                    groups = len(lx)
                    vectors = np.asarray(self.PATH_DIR_VECS,dtype=np.int32)
                    steps = np.asarray([cardinal]*8 if mode == 'contracted'
                                       else [cardinal]*4+[diagonal]*4,dtype=np.float32)
                    def adjacent(begin,end):
                        nx = lx[begin:end,None]+vectors[:,0]
                        ny = ly[begin:end,None]+vectors[:,1]
                        valid = (nx>=0)&(ny>=0)&(nx<node.shape[1])&(ny<node.shape[0])
                        neighbours = node[np.clip(ny,0,node.shape[0]-1),
                                          np.clip(nx,0,node.shape[1]-1)].copy()
                        neighbours[~valid] = -1
                        return neighbours
                    indptr = np.zeros(groups+1,dtype=np.int32)
                    for begin in range(0,groups,4096):
                        self._check_plan_cancelled()
                        end = min(begin+4096,groups)
                        indptr[begin+1:end+1] = np.count_nonzero(adjacent(begin,end)>=0,axis=1)
                    np.cumsum(indptr,out=indptr)
                    indices = np.empty(int(indptr[-1]),dtype=np.int32)
                    data = np.empty(len(indices),dtype=np.float64)
                    for begin in range(0,groups,4096):
                        self._check_plan_cancelled()
                        end = min(begin+4096,groups)
                        neighbours = adjacent(begin,end)
                        valid = neighbours>=0
                        slots = (indptr[begin:end,None]
                            +np.cumsum(valid,axis=1,dtype=np.int32)-1)[valid]
                        indices[slots] = neighbours[valid]
                        row_cost = cost_grid[layout['cy'][ly[begin:end]],layout['cx'][lx[begin:end]]]
                        data[slots] = np.multiply(row_cost[:,None],steps,dtype=np.float32)[valid]
                    self._check_plan_cancelled()
                    graph = scipy_sparse.csr_matrix((data,indices,indptr),
                                                    shape=(groups,groups),copy=False)
                    for array in (graph.data,graph.indices,graph.indptr):
                        array.flags.writeable = False
                    model['relaxed_reverse_graph'] = graph
                self._check_plan_cancelled()
                # Rounding only this heuristic downward makes every reverse sum an
                # exact float64 multiple below 2**53 quanta. The real edge weights
                # remain untouched, including swamp, diagonal and turning costs.
                max_base = float(graph.data.max(initial=0.))
                exponent = math.frexp(max_base*graph.shape[0])[1]-53
                lower_data = np.empty_like(graph.data)
                for begin in range(0,len(lower_data),65536):
                    self._check_plan_cancelled()
                    end = min(begin+65536,len(lower_data))
                    lower_data[begin:end] = np.ldexp(
                        np.floor(np.ldexp(graph.data[begin:end],-exponent)),exponent)
                lower = scipy_sparse.csr_matrix((lower_data,graph.indices,graph.indptr),
                                                 shape=graph.shape,copy=False)
                graph_goal = int(goal) if mode == 'fine' else int(layout['node'][
                    model['cy'][yy[int(goal)]],model['cx'][xx[int(goal)]]])
                potential = scipy_dijkstra(lower,directed=True,indices=graph_goal,
                                           return_predecessors=False)
                self._check_plan_cancelled()
                model['relaxed_quantum_exponent'] = exponent
            if mode == 'fine':
                distances = potential
            else:
                distances = np.empty(count,dtype=np.float64)
                for begin in range(0,count,4096):
                    self._check_plan_cancelled()
                    end = min(begin+4096,count)
                    group_ids = layout['node'][model['cy'][yy[begin:end]],model['cx'][xx[begin:end]]]
                    # Max of consistent lower bounds is consistent. Keep the
                    # global octile bound so contraction cannot weaken it.
                    cost = model['relaxed_min_cost']
                    straight = float(np.float32(cost*np.float32(cardinal)))
                    across = min(float(np.float32(cost*np.float32(diagonal))),2*straight)
                    dx = np.abs(xx[begin:end]-xx[int(goal)])
                    dy = np.abs(yy[begin:end]-yy[int(goal)])
                    octile = np.minimum(dx,dy)*across+np.abs(dx-dy)*straight
                    distances[begin:end] = np.maximum(potential[group_ids],octile)
        self._check_plan_cancelled()
        distances.flags.writeable = False
        model['relaxed_goal'] = (int(goal),distances)
        return distances



    @staticmethod
    def _sampled_square_distances(bitmap, xquery, yquery):
        height, width = bitmap.shape
        infinity = 1 << 50
        horizontal = np.full((height, len(xquery)), infinity, dtype=np.int64)
        for y in range(height):
            last = -1
            i = 0
            for x in range(width):
                if bitmap[y, x]:
                    while i < len(xquery) and xquery[i] <= x:
                        query = xquery[i]
                        value = (x-query)*(x-query)
                        if last >= 0:
                            value = min(value, (query-last)*(query-last))
                        horizontal[y, i] = value
                        i += 1
                    last = x
                    if i == len(xquery):
                        break
            while i < len(xquery):
                if last >= 0:
                    horizontal[y, i] = (xquery[i]-last)*(xquery[i]-last)
                i += 1
        output = np.full((len(yquery), len(xquery)), infinity, dtype=np.int64)
        centres = np.empty(height, dtype=np.int64)
        starts = np.empty(height, dtype=np.int64)
        for x in range(len(xquery)):
            k = -1
            for q in range(height):
                value = horizontal[q, x]
                if value >= infinity:
                    continue
                boundary = -infinity
                while k >= 0:
                    p = centres[k]
                    # First integer y at which q is STRICTLY better than p.
                    boundary = (value+q*q-horizontal[p, x]-p*p)//(2*(q-p))+1
                    if boundary > starts[k]:
                        break
                    k -= 1
                k += 1
                centres[k] = q
                starts[k] = -infinity if k == 0 else boundary
            if k < 0:
                continue
            at = 0
            for i in range(len(yquery)):
                y = yquery[i]
                while at < k and starts[at+1] <= y:
                    at += 1
                q = centres[at]
                output[i, x] = horizontal[q, x]+(y-q)*(y-q)
        return output

    def _prepare_implicit_body(self, model):
        if np.all(model['body'] >= 0):
            return
        tree = self._plan_obstacle_tree
        if tree is None or tree.n == 0:
            model['body'][:] = 1
            return
        if self._rectangle_oracle(tree) is not None:
            xx,yy = model['xx'],model['yy']
            x,y = model['xc'][xx]/self.NAV_GRID_MM+self.c,model['yc'][yy]/self.NAV_GRID_MM+self.c
            model['body'][:] = self._disk_segment_mask(x,y,x,y,self.ROBOT_RADIUS_MM,tree)
            model['implicit_body_field'] = dict(backend='rectangle_bvh',nodes=len(xx))
            return
        grid = int(self._plan_grid_mm)
        if grid != self._plan_grid_mm or not 1 <= grid <= 10:
            return
        self._check_plan_cancelled()
        low = tree.data-grid/2
        rounded = np.rint(low)
        xc, yc = model['xc'], model['yc']
        if (not np.array_equal(low, rounded)
                or not np.array_equal(xc, np.rint(xc))
                or not np.array_equal(yc, np.rint(yc))):
            return
        bound = self._planning_mapper.c*self._planning_mapper.GRID_MM
        if not np.isfinite(rounded).all() or np.any(abs(rounded) > bound+grid):
            return
        pad = math.ceil(self.ROBOT_RADIUS_MM)+1
        origin = rounded.min(axis=0).astype(np.int64)-pad
        end = rounded.max(axis=0).astype(np.int64)+pad+grid
        dimensions = end-origin+1
        bound = self._planning_mapper.c*self._planning_mapper.GRID_MM
        if np.any(dimensions > 2*bound+2*pad+3):
            return  # Unsupported outside-world material keeps the original oracle.
        bitmap = np.zeros((int(dimensions[1]), int(dimensions[0])), dtype=bool)
        vertices = rounded.astype(np.int64)-origin
        for dx in range(grid+1):
            for dy in range(grid+1):
                bitmap[vertices[:,1]+dy, vertices[:,0]+dx] = True
        ix = np.flatnonzero((xc >= origin[0]) & (xc <= end[0]))
        iy = np.flatnonzero((yc >= origin[1]) & (yc <= end[1]))
        if not len(ix) or not len(iy):
            model['body'][:] = 1
            return
        self._check_plan_cancelled()
        distances = NavigationMapper._compiled_square_distances(bitmap,
            (xc[ix]-origin[0]).astype(np.int64), (yc[iy]-origin[1]).astype(np.int64))
        self._check_plan_cancelled()
        xx, yy = model['xx'], model['yy']
        inside = (xx >= ix[0]) & (xx <= ix[-1]) & (yy >= iy[0]) & (yy <= iy[-1])
        result = np.ones(len(xx), dtype=np.int8)
        ids = np.flatnonzero(inside)
        squared = distances[yy[ids]-iy[0], xx[ids]-ix[0]]
        # Actual clearance calls round-trip through NAV coordinates. Use their
        # measured displacement and 1-Lipschitz distance bounds; uncertainty falls
        # back to the existing square/capsule predicate, especially exact tangency.
        ax, ay = xc[xx[ids]]/self.NAV_GRID_MM+self.c, yc[yy[ids]]/self.NAV_GRID_MM+self.c
        error = np.hypot((ax-self.c)*self.NAV_GRID_MM-xc[xx[ids]],
                         (ay-self.c)*self.NAV_GRID_MM-yc[yy[ids]])+1e-8
        distance = np.sqrt(squared.astype(np.float64))
        radius = math.sqrt(self.ROBOT_RADIUS_MM**2-1e-8)
        clear = distance-error >= radius
        blocked = distance+error < radius
        result[ids[blocked]] = 0
        pending = ~(clear | blocked)
        if np.any(pending):
            result[ids[pending]] = self._physical_segment_mask(
                ax[pending], ay[pending], ax[pending], ay[pending])
        model['body'][:] = result
        model['implicit_body_field'] = dict(vertices_shape=bitmap.shape,
            sampled_shape=distances.shape, exact_oracle_queries=int(pending.sum()),
            temporary_bytes=bitmap.nbytes+bitmap.shape[0]*len(ix)*8+distances.nbytes)

    @staticmethod
    def _implicit_fine_step(xx, yy, node, observed, body, edge_known, edge_clear,
            turn_known, turn_clear, heuristic, costs, normal, vectors, turn, right,
            steps, open_extra, goal, distance, predecessor, heap, where, size, budget):
        def less(a, b):
            ha, hb = heuristic[a//8], heuristic[b//8]
            return (distance[a]+ha, ha, a) < (distance[b]+hb, hb, b)
        expanded = 0
        while size and expanded < budget:
            state = heap[0]
            size -= 1
            where[state] = -2
            if size:
                last = heap[size]
                i = 0
                while i*2+1 < size:
                    child = i*2+1
                    if child+1 < size and less(heap[child+1], heap[child]):
                        child += 1
                    if not less(heap[child], last):
                        break
                    heap[i] = heap[child]
                    where[heap[i]] = i
                    i = child
                heap[i] = last
                where[last] = i
            owner, previous = state//8, state%8
            expanded += 1
            if body[owner] == 0:
                continue
            if owner == goal:
                return state, size, expanded
            for direction in range(8):
                x = xx[owner]+vectors[direction, 0]
                y = yy[owner]+vectors[direction, 1]
                if x < 0 or y < 0 or y >= node.shape[0] or x >= node.shape[1]:
                    continue
                neighbour = node[y, x]
                if neighbour < 0 or body[neighbour] == 0:
                    continue
                if direction >= 4 and not (observed[yy[owner], x] and observed[y, xx[owner]]):
                    continue
                bit = 1 << direction
                if edge_known[owner] & bit and not edge_clear[owner] & bit:
                    continue
                if turn_known[owner, previous] & bit and not turn_clear[owner, previous] & bit:
                    continue
                following = neighbour*8+direction
                if where[following] == -2:
                    continue
                base = np.float32(costs[neighbour]*steps[direction])
                extra = np.float32(open_extra if right[previous, direction]
                                   and normal[owner] and normal[neighbour] else 0.)
                weight = np.float32(np.float32(base+turn[previous, direction])+extra)
                new = distance[state]+float(weight)
                if new >= distance[following]:
                    continue
                distance[following] = new
                predecessor[following] = state
                i = where[following]
                if i < 0:
                    i = size
                    size += 1
                while i:
                    parent = (i-1)//2
                    if not less(following, heap[parent]):
                        break
                    heap[i] = heap[parent]
                    where[heap[i]] = i
                    i = parent
                heap[i] = following
                where[following] = i
        return (-2 if size == 0 else -1), size, expanded

    @staticmethod
    def _relaxed_grid_maximum(xx, yy, node, costs, vectors, steps):
        maximum = 0.0
        for owner in range(len(xx)):
            for direction in range(8):
                x = xx[owner]+vectors[direction, 0]
                y = yy[owner]+vectors[direction, 1]
                if 0 <= x < node.shape[1] and 0 <= y < node.shape[0] and node[y, x] >= 0:
                    maximum = max(maximum, float(np.float32(costs[owner]*steps[direction])))
        return maximum

    @staticmethod
    def _relaxed_grid_step(xx, yy, node, costs, vectors, steps, exponent,
            distance, heap, where, size, budget):
        def less(a, b):
            return (distance[a], a) < (distance[b], b)
        expanded = 0
        while size and expanded < budget:
            owner = heap[0]
            size -= 1
            where[owner] = -2
            if size:
                last = heap[size]
                i = 0
                while i*2+1 < size:
                    child = i*2+1
                    if child+1 < size and less(heap[child+1], heap[child]):
                        child += 1
                    if not less(heap[child], last):
                        break
                    heap[i] = heap[child]
                    where[heap[i]] = i
                    i = child
                heap[i] = last
                where[last] = i
            expanded += 1
            for direction in range(8):
                x = xx[owner]+vectors[direction, 0]
                y = yy[owner]+vectors[direction, 1]
                if x < 0 or y < 0 or y >= node.shape[0] or x >= node.shape[1]:
                    continue
                neighbour = node[y, x]
                if neighbour < 0 or where[neighbour] == -2:
                    continue
                # This is the reverse relaxation: the destination cost of the
                # forward edge belongs to the current reverse-search owner.
                base = float(np.float32(costs[owner]*steps[direction]))
                weight = math.ldexp(math.floor(math.ldexp(base, -exponent)), exponent)
                new = distance[owner]+weight
                if new >= distance[neighbour]:
                    continue
                distance[neighbour] = new
                i = where[neighbour]
                if i < 0:
                    i = size
                    size += 1
                while i:
                    parent = (i-1)//2
                    if not less(neighbour, heap[parent]):
                        break
                    heap[i] = heap[parent]
                    where[heap[i]] = i
                    i = parent
                heap[i] = neighbour
                where[neighbour] = i
        return size

    def _query_relaxed_grid(self, layout, goal, cost_grid, cardinal, diagonal, mode):
        cached = layout.get('implicit_relaxed_layout')
        if cached is None:
            def readonly(array):
                result = np.ascontiguousarray(array).view()
                result.flags.writeable = False
                return result
            xx, yy = layout['xx'], layout['yy']
            costs = cost_grid[layout['cy'][yy], layout['cx'][xx]]
            if not np.isfinite(costs).all() or np.any(costs < 0):
                return None
            args = tuple(map(readonly, (xx, yy, layout['node'], costs,
                np.asarray(self.PATH_DIR_VECS, dtype=np.int64),
                np.asarray([cardinal]*8 if mode == 'contracted' else
                           [cardinal]*4+[diagonal]*4, dtype=np.float32))))
            self._check_plan_cancelled()
            maximum = NavigationMapper._compiled_relaxed_maximum(*args)
            if not math.isfinite(maximum):
                return None
            exponent = math.frexp(maximum*len(xx))[1]-53
            cached = args, exponent
            layout['implicit_relaxed_layout'] = cached
        args, exponent = cached
        count = len(layout['xx'])
        distance = np.full(count, np.inf, dtype=np.float64)
        heap = np.empty(count, dtype=np.int32)
        where = np.full(count, -1, dtype=np.int32)
        heap[0] = goal
        where[goal] = 0
        distance[goal] = 0.0
        size = 1
        while size:
            self._check_plan_cancelled()
            size = NavigationMapper._compiled_relaxed_step(*args, exponent,
                distance, heap, where, size, 4096)
        self._check_plan_cancelled()
        return distance, exponent

    @staticmethod
    def _scan_occupied_centres(logodds, threshold, centre, grid, phase):
        count = 0
        for y in range(logodds.shape[0]):
            for x in range(logodds.shape[1]):
                count += logodds[y, x] >= threshold
        points = np.empty((count, 2), dtype=np.float64)
        i = 0
        for y in range(logodds.shape[0]):
            for x in range(logodds.shape[1]):
                if logodds[y, x] >= threshold:
                    points[i, 0] = (x-centre)*grid+phase
                    points[i, 1] = (y-centre)*grid+phase
                    i += 1
        return points

    @staticmethod
    def _rectangle_raster(points, grid, bound):
        if not math.isfinite(grid) or grid < 1 or grid > 10 or (grid != math.floor(grid)):
            return (False, np.empty((0, 0), np.bool_), 0, 0)
        side = int(grid)
        if not len(points):
            return (True, np.empty((0, 0), np.bool_), 0, 0)
        minx = miny = math.inf
        maxx = maxy = -math.inf
        for i in range(len(points)):
            x = points[i, 0] - grid / 2
            y = points[i, 1] - grid / 2
            if x+grid/2 != points[i,0] or y+grid/2 != points[i,1]:
                return (False, np.empty((0, 0), np.bool_), 0, 0)
            if not math.isfinite(x) or not math.isfinite(y) or abs(x) > bound + grid or (abs(y) > bound + grid) or (x != math.floor(x)) or (y != math.floor(y)):
                return (False, np.empty((0, 0), np.bool_), 0, 0)
            minx = min(minx, x)
            maxx = max(maxx, x)
            miny = min(miny, y)
            maxy = max(maxy, y)
        x0 = int(minx)
        y0 = int(miny)
        width = int(maxx - minx) + side
        height = int(maxy - miny) + side
        if width > 2 * bound + 2 * grid + 1 or height > 2 * bound + 2 * grid + 1:
            return (False, np.empty((0, 0), np.bool_), 0, 0)
        mask = np.zeros((height, width), np.bool_)
        for i in range(len(points)):
            x = int(points[i, 0] - grid / 2) - x0
            y = int(points[i, 1] - grid / 2) - y0
            for dy in range(side):
                for dx in range(side):
                    mask[y + dy, x + dx] = True
        return (True, mask, x0, y0)

    @staticmethod
    def _rectangle_runs(mask):
        (height, width) = mask.shape
        runs = 0
        for y in range(height):
            before = False
            for x in range(width):
                now = mask[y, x]
                if now and (not before):
                    runs += 1
                before = now
        boxes = np.empty((runs, 4), np.int64)
        maximum = (width + 1) // 2
        pl = np.empty(maximum, np.int64)
        pr = np.empty(maximum, np.int64)
        pi = np.empty(maximum, np.int64)
        cl = np.empty(maximum, np.int64)
        cr = np.empty(maximum, np.int64)
        ci = np.empty(maximum, np.int64)
        previous = 0
        count = 0
        for y in range(height):
            current = 0
            x = 0
            while x < width:
                if not mask[y, x]:
                    x += 1
                    continue
                left = x
                x += 1
                while x < width and mask[y, x]:
                    x += 1
                cl[current] = left
                cr[current] = x
                current += 1
            p = 0
            for c in range(current):
                while p < previous and pl[p] < cl[c]:
                    p += 1
                if p < previous and pl[p] == cl[c] and (pr[p] == cr[c]):
                    index = pi[p]
                    boxes[index, 3] = y + 1
                else:
                    index = count
                    count += 1
                    boxes[index, 0] = cl[c]
                    boxes[index, 1] = y
                    boxes[index, 2] = cr[c]
                    boxes[index, 3] = y + 1
                ci[c] = index
            (pl, cl) = (cl, pl)
            (pr, cr) = (cr, pr)
            (pi, ci) = (ci, pi)
            previous = current
        return (boxes[:count].copy(), runs)

    @staticmethod
    def _rectangle_bvh(boxes):
        count = len(boxes)
        bounds = np.empty((2*count-1, 4), dtype=np.float64)
        children = np.full((2*count-1, 2), -1, dtype=np.int32)
        indices = np.arange(count, dtype=np.int64)
        stack = np.empty((2*count, 3), dtype=np.int64)
        stack[0] = (0, 0, count)
        size, used = 1, 1
        while size:
            size -= 1
            owner, first, last = stack[size]
            lowx, lowy, highx, highy = np.inf, np.inf, -np.inf, -np.inf
            minx, miny, maxx, maxy = np.inf, np.inf, -np.inf, -np.inf
            for j in range(first, last):
                b = boxes[indices[j]]
                lowx, lowy = min(lowx,b[0]), min(lowy,b[1])
                highx, highy = max(highx,b[2]), max(highy,b[3])
                minx, maxx = min(minx,b[0]+b[2]), max(maxx,b[0]+b[2])
                miny, maxy = min(miny,b[1]+b[3]), max(maxy,b[1]+b[3])
            bounds[owner] = (lowx,lowy,highx,highy)
            if last-first == 1:
                continue
            axis = 0 if maxx-minx >= maxy-miny else 1
            local = indices[first:last].copy()
            order = np.argsort(boxes[local,axis]+boxes[local,axis+2])
            indices[first:last] = local[order]
            middle = (first+last)//2
            children[owner] = (used,used+1)
            stack[size] = (used,first,middle)
            stack[size+1] = (used+1,middle,last)
            size += 2
            used += 2
        return bounds, children

    @staticmethod
    def _rectangle_capsules(pa, pb, radius, bounds, children):
        answer = np.ones(len(pa),dtype=np.int8)
        stack = np.empty(64,dtype=np.int32)
        threshold = radius*radius-1e-8
        u = np.finfo(np.float64).eps
        # eps is twice unit roundoff. This combined bound includes the
        # original square candidate/projection arithmetic, witness displacement,
        # support projections and squared comparisons (fewer than128 operations).
        # Clamped parameters and nonexpansive max/min bound absolute point error.
        gamma = 256*u/(1-256*u)
        extent = np.sum(np.abs(bounds[0]))
        for owner in range(len(pa)):
            ax,ay,bx,by = pa[owner,0],pa[owner,1],pb[owner,0],pb[owner,1]
            vx,vy = bx-ax,by-ay
            # Preserve the generic slab oracle's numerical parallel branch.
            if ((vx != 0. and abs(vx) <= 1e-12) or (vy != 0. and abs(vy) <= 1e-12)):
                answer[owner] = -1
                continue
            scale = abs(ax)+abs(ay)+abs(bx)+abs(by)+radius+extent+1.0
            error = gamma*scale*scale
            if not math.isfinite(error):
                answer[owner] = -1
                continue
            size = 1
            stack[0] = 0
            while size:
                size -= 1
                node = stack[size]
                lx,ly,hx,hy = bounds[node]
                dx = max(lx-max(ax,bx),min(ax,bx)-hx,0.)
                dy = max(ly-max(ay,by),min(ay,by)-hy,0.)
                if dx*dx+dy*dy > threshold+error:
                    continue
                if children[node,0] >= 0:
                    stack[size] = children[node,0]
                    stack[size+1] = children[node,1]
                    size += 2
                    continue
                # Approximate closest witnesses only SELECT a direction/t.
                # Every chosen finite t in[0,1] is a valid segment witness;
                # every chosen direction is a valid support-plane normal.
                qx,qy = ax,ay
                dx,dy = max(lx-ax,ax-hx,0.),max(ly-ay,ay-hy,0.)
                best = dx*dx+dy*dy
                dx,dy = max(lx-bx,bx-hx,0.),max(ly-by,by-hy,0.)
                if dx*dx+dy*dy < best:
                    best,qx,qy = dx*dx+dy*dy,bx,by
                denominator = max(vx*vx+vy*vy,1e-30)
                for cx,cy in ((lx,ly),(lx,hy),(hx,ly),(hx,hy)):
                    t = min(1.,max(0.,((cx-ax)*vx+(cy-ay)*vy)/denominator))
                    x,y = ax+t*vx,ay+t*vy
                    dx,dy = max(lx-x,x-hx,0.),max(ly-y,y-hy,0.)
                    squared = dx*dx+dy*dy
                    if squared < best:
                        best,qx,qy = squared,x,y
                # This is an UPPER bound from an actual segment witness,
                # including its forward-rounding displacement in error.
                if best < threshold-error:
                    answer[owner] = 0
                    break
                nx,ny = qx-min(hx,max(lx,qx)),qy-min(hy,max(ly,qy))
                magnitude = max(abs(nx),abs(ny))
                separated = False
                if magnitude > 0. and math.isfinite(magnitude):
                    nx,ny = nx/magnitude,ny/magnitude
                    minimum = nx*(lx if nx>=0 else hx)+ny*(ly if ny>=0 else hy)
                    maximum = nx*(hx if nx>=0 else lx)+ny*(hy if ny>=0 else ly)
                    a,b = nx*ax+ny*ay,nx*bx+ny*by
                    gap = max(minimum-max(a,b),min(a,b)-maximum,0.)
                    # Arbitrary-normal separation supplies a LOWER bound.
                    # gamma256*S² covers support projections, both squared
                    # comparisons and witness error; no optimality of q/n is used.
                    separated = gap>0. and gap*gap > threshold*(nx*nx+ny*ny)+error
                if not separated:
                    answer[owner] = -1
        return answer

    def _warm_implicit_fine(self):
        # Compilation is a one-time controller-construction cost, never postponed
        # to the user's first click. A missing optional backend retains40 unchanged.
        cls = NavigationMapper
        if hasattr(cls, '_compiled_implicit_fine'):
            return cls._compiled_implicit_fine
        try:
            import numba
        except ImportError:
            cls._compiled_implicit_fine = None
            return None
        kernel = numba.njit(nogil=True, cache=False)(cls._implicit_fine_step)
        def readonly(array):
            array.flags.writeable = False
            return array
        integers = readonly(np.empty(0, dtype=np.int64))
        grid = readonly(np.empty((0, 0), dtype=np.int32))
        observed = readonly(np.empty((0, 0), dtype=bool))
        body = readonly(np.empty(0, dtype=np.int8))
        bits = readonly(np.empty(0, dtype=np.uint8))
        pairs = readonly(np.empty((0, 8), dtype=np.uint8))
        heuristic = readonly(np.empty(0, dtype=np.float64))
        costs = readonly(np.empty(0, dtype=np.float32))
        normal = readonly(np.empty(0, dtype=bool))
        vectors = readonly(np.asarray(self.PATH_DIR_VECS, dtype=np.int64))
        turns = readonly(np.asarray([self._turn_costs_from_dir(i, np.arange(8)) for i in range(8)]))
        right = readonly(np.asarray([self._right_turn_mask_from_dir(i, np.arange(8)) for i in range(8)]))
        steps = readonly(np.zeros(8, dtype=np.float32))
        kernel(integers, integers, grid, observed, body, bits, bits, pairs, pairs,
            heuristic, costs, normal, vectors, turns, right, steps,
            float(self.PATH_OPEN_RIGHT_TURN_EXTRA_COST), 0,
            np.empty(0, dtype=np.float64), np.empty(0, dtype=np.int32),
            np.empty(0, dtype=np.int32), np.empty(0, dtype=np.int32), 0, 0)
        field = numba.njit(nogil=True, cache=False)(cls._sampled_square_distances)
        field(np.zeros((1,1),dtype=bool), np.zeros(1,dtype=np.int64), np.zeros(1,dtype=np.int64))
        occupied = numba.njit(nogil=True, cache=False)(cls._scan_occupied_centres)
        occupied(np.zeros((1,1), dtype=np.int16), 1, 0, 2, 0.0)
        maximum = numba.njit(nogil=True, cache=False)(cls._relaxed_grid_maximum)
        relaxed = numba.njit(nogil=True, cache=False)(cls._relaxed_grid_step)
        args = (integers, integers, grid, costs, vectors, steps)
        maximum(*args)
        relaxed(*args, -53, np.empty(0, dtype=np.float64),
            np.empty(0, dtype=np.int32), np.empty(0, dtype=np.int32), 0, 0)
        raster = numba.njit(nogil=True, cache=False)(cls._rectangle_raster)
        runs = numba.njit(nogil=True, cache=False)(cls._rectangle_runs)
        hierarchy = numba.njit(nogil=True, cache=False)(cls._rectangle_bvh)
        points = numba.njit(nogil=True, cache=False)(cls._rectangle_capsules)
        raster(readonly(np.empty((0,2),dtype=np.float64)),1.0,3000.0)
        runs(np.zeros((1,1),dtype=bool))
        bounds, children = hierarchy(np.array([[0.,0.,1.,1.]]))
        bounds.flags.writeable = children.flags.writeable = False
        points(np.empty((0,2)),np.empty((0,2)),37.0,bounds,children)
        cls._compiled_rectangle_raster = raster
        cls._compiled_rectangle_runs = runs
        cls._compiled_rectangle_bvh = hierarchy
        cls._compiled_rectangle_capsules = points
        cls._compiled_relaxed_maximum = maximum
        cls._compiled_relaxed_step = relaxed
        cls._compiled_occupied_centres = occupied
        cls._compiled_square_distances = field
        cls._compiled_implicit_fine = kernel
        return kernel

    def _query_implicit_fine(self, model, source, goal, headings, cost_grid, normal_clear):
        kernel = self._warm_implicit_fine()
        if kernel is None:
            return None
        xx, yy, xc, yc = (model[k] for k in ('xx', 'yy', 'xc', 'yc'))
        count = len(xx)
        scale = self.PATH_FINE_GRID_MM/self.NAV_GRID_MM
        cardinal, diagonal = float(np.float32(scale)), float(np.float32(math.sqrt(2)*scale))
        # Keep the existing exact body-filtered guidance: an optimistic body-
        # free field repeatedly routes through the same forbidden wall band.
        self._prepare_implicit_body(model)
        h = self._fine_relaxed_goal_distances(model, goal, cost_grid, cardinal, diagonal)
        costs = cost_grid[model['cy'][yy], model['cx'][xx]]
        normal = normal_clear[model['cy'][yy], model['cx'][xx]]
        vectors = np.asarray(self.PATH_DIR_VECS, dtype=np.int64)
        turns = np.asarray([self._turn_costs_from_dir(i, np.arange(8)) for i in range(8)])
        right = np.asarray([self._right_turn_mask_from_dir(i, np.arange(8)) for i in range(8)])
        steps = np.asarray([cardinal]*4+[diagonal]*4, dtype=np.float32)
        def readonly(array):
            view = np.ascontiguousarray(array).view()
            view.flags.writeable = False
            return view
        args = tuple(map(readonly, (xx, yy, model['node'], model['observed'], model['body'],
            model['edge_known'], model['edge_clear'], model['turn_known'], model['turn_clear'],
            h, costs, normal, vectors, turns, right, steps)))
        starts = np.sort(source*8+np.asarray(headings, dtype=np.int32))
        expansions = 0
        def positions(ids):
            return xc[xx[ids]]/self.NAV_GRID_MM+self.c, yc[yy[ids]]/self.NAV_GRID_MM+self.c
        for attempt in range(8):
            self._check_plan_cancelled()
            distance = np.full(count*8, np.inf, dtype=np.float64)
            pred = np.full(count*8, -1, dtype=np.int32)
            heap = np.empty(count*8, dtype=np.int32)
            where = np.full(count*8, -1, dtype=np.int32)
            size = len(starts)
            heap[:size] = starts
            where[starts] = np.arange(size)
            distance[starts] = 0.
            best = -1
            while best == -1:
                self._check_plan_cancelled()
                best, size, expanded = kernel(*args,
                    float(self.PATH_OPEN_RIGHT_TURN_EXTRA_COST), int(goal),
                    distance, pred, heap, where, int(size), 4096)
                expansions += expanded
            self._check_plan_cancelled()
            if best < 0:
                # A failed optimistic graph certifies physical unreachability.
                model['implicit_stats'] = dict(attempts=attempt+1, expansions=expansions, exhausted=True)
                return (), None
            states = []
            current = int(best)
            for _ in range(count*8):
                self._check_plan_cancelled()
                states.append(current)
                if current in starts:
                    break
                current = int(pred[current])
                if current < 0:
                    raise RuntimeError('implicit fine predecessor is missing')
            else:
                raise RuntimeError('implicit fine predecessor cycle')
            states = np.asarray(states[::-1], dtype=np.int32)
            nodes = states//8
            unknown = np.unique(nodes[model['body'][nodes] < 0])
            if len(unknown):
                x, y = positions(unknown)
                model['body'][unknown] = self._physical_segment_mask(x, y, x, y)
            if np.any(model['body'][nodes] == 0):
                continue
            if len(states) > 1:
                edges = np.unique(np.column_stack((nodes[:-1], states[1:]%8)), axis=0)
                unknown = edges[(model['edge_known'][edges[:,0]] & (1<<edges[:,1])) == 0]
                if len(unknown):
                    owners, dirs = unknown.T
                    dest = model['node'][yy[owners]+vectors[dirs,1], xx[owners]+vectors[dirs,0]]
                    ax, ay = positions(owners)
                    bx, by = positions(dest)
                    clear = self._physical_segment_mask(ax, ay, bx, by)
                    for owner, direction, ok in zip(owners, dirs, clear):
                        model['edge_known'][owner] |= np.uint8(1<<int(direction))
                        if ok:
                            model['edge_clear'][owner] |= np.uint8(1<<int(direction))
                if np.any((model['edge_clear'][edges[:,0]] & (1<<edges[:,1])) == 0):
                    continue
                bends = np.unique(np.column_stack((nodes[:-1], states[:-1]%8, states[1:]%8)), axis=0)
                unknown = bends[(model['turn_known'][bends[:,0],bends[:,1]] & (1<<bends[:,2])) == 0]
                if len(unknown):
                    owners, before, after = unknown.T
                    x, y = positions(owners)
                    clear = self._turn_mask_from_dir(x, y, before, after)
                    for owner, previous, direction, ok in zip(owners, before, after, clear):
                        model['turn_known'][owner,previous] |= np.uint8(1<<int(direction))
                        if ok:
                            model['turn_clear'][owner,previous] |= np.uint8(1<<int(direction))
                if np.any((model['turn_clear'][bends[:,0],bends[:,1]] & (1<<bends[:,2])) == 0):
                    continue
            self._check_plan_cancelled()
            model['implicit_stats'] = dict(attempts=attempt+1, expansions=expansions,
                body_proved=int(np.count_nonzero(model['body'] >= 0)),
                possible_nodes=count, possible_states=count*8, constructed_heading_edges=0)
            return tuple(map(int, nodes)), float(distance[best])
        model['implicit_stats'] = dict(attempts=8, expansions=expansions, fallback=True)
        return None

    def _plan_fine_path(self, start, goal, normal_clear, wall_dist, requested_corner):
        """Native LazySP on the unchanged fine heading graph.

        At most n*n fully body-proved fine nodes form a CSR with at most
        64*n*n transitions. Only candidate paths need moving/turning proofs;
        invalid transitions are removed before any route is returned. Larger
        or partially body-proved models retain the original indexed A* below.
        Source, target, costs, observation and final sweep gates are unchanged.
        """
        self._check_plan_cancelled()
        mapper = self._planning_mapper
        key = ("fine", self.PATH_FINE_GRID_MM)
        model = self._plan_graph_cache.get(key)
        if model is None:
            ky,kx = np.nonzero((self.cell_state != self.CELL_UNKNOWN)
                               & (self.floor != self.FLOOR_HOLE))
            if not len(kx):
                self._set_plan_failed("no known safe cells")
                return
            grid = self.PATH_FINE_GRID_MM
            bound = mapper.c*mapper.GRID_MM
            def coordinates(lo,hi):
                low = max(-bound,(lo-self.c)*self.NAV_GRID_MM)
                high = min(bound,(hi+1-self.c)*self.NAV_GRID_MM)
                return np.arange(math.ceil(low/grid)*grid, high+grid, grid)
            xc,yc = coordinates(kx.min(),kx.max()),coordinates(ky.min(),ky.max())
            rx = np.rint(xc/mapper.GRID_MM).astype(int)+mapper.c
            ry = np.rint(yc/mapper.GRID_MM).astype(int)+mapper.c
            cx = np.clip(np.floor(xc/self.NAV_GRID_MM).astype(int)+self.c,0,self.n-1)
            cy = np.clip(np.floor(yc/self.NAV_GRID_MM).astype(int)+self.c,0,self.n-1)
            observed = ((self.cell_state != self.CELL_UNKNOWN)
                        & (self.floor != self.FLOOR_HOLE))[cy[:,None],cx[None,:]]
            observed &= mapper.logodds[np.ix_(ry,rx)] <= mapper.FREE_T
            yy,xx = np.nonzero(observed)
            count = len(xx)
            node = np.full(observed.shape,-1,dtype=np.int32)
            node[yy,xx] = np.arange(count,dtype=np.int32)
            component,_ = scipy_ndimage.label(observed)
            model = dict(xc=xc,yc=yc,cx=cx,cy=cy,xx=xx,yy=yy,node=node,observed=observed,
                component=component,body=np.full(count,-1,dtype=np.int8),
                edge_known=np.zeros(count,dtype=np.uint8),edge_clear=np.zeros(count,dtype=np.uint8),
                turn_known=np.zeros((count,8),dtype=np.uint8),turn_clear=np.zeros((count,8),dtype=np.uint8),
                full_turn=np.full(count,-1,dtype=np.int8))
            self._plan_graph_cache[key] = model
        xc,yc,cx,cy,xx,yy,node,observed,component = (model[k] for k in
            ("xc","yc","cx","cy","xx","yy","node","observed","component"))
        count = len(xx)
        if getattr(NavigationMapper, '_compiled_square_distances', None) is not None:
            # The successful fine search needs these same immutable body proofs.
            # Prepare once before endpoint selection, avoiding its duplicate KD
            # body queries; endpoint connectors and heading gates remain exact.
            self._prepare_implicit_body(model)
        def positions(ids):
            return xc[xx[ids]]/self.NAV_GRID_MM+self.c, yc[yy[ids]]/self.NAV_GRID_MM+self.c
        def bodies(ids):
            unknown = ids[model['body'][ids] < 0]
            if len(unknown):
                x,y = positions(unknown)
                model['body'][unknown] = self._physical_segment_mask(x,y,x,y)
            return model['body'][ids] > 0
        source = (tuple(v/self.NAV_GRID_MM+self.c for v in self.current_world_mm)
                  if self.current_world_mm is not None else (start[0]+.5,start[1]+.5))
        target = requested_corner if requested_corner is not None else (goal[0]+.5,goal[1]+.5)
        def nearest(point, reverse=False):
            ix = np.flatnonzero(abs(xc/self.NAV_GRID_MM+self.c-point[0]) <= 1)
            iy = np.flatnonzero(abs(yc/self.NAV_GRID_MM+self.c-point[1]) <= 1)
            ids = node[np.ix_(iy,ix)].ravel()
            ids = ids[ids >= 0]
            ids = ids[bodies(ids)]
            px,py = positions(ids)
            order = np.lexsort((ids,(px-point[0])**2+(py-point[1])**2))
            for j in order:
                pos = float(px[j]),float(py[j])
                a,b = (pos,point) if reverse else (point,pos)
                if self._raw_segment_clear(a,b):
                    return int(ids[j])
            return None
        sn,gn = nearest(source),nearest(target,reverse=True)
        if sn is None or gn is None:
            self._set_plan_failed("no observed fine-grid connection to start or destination")
            return
        if component[yy[sn],xx[sn]] != component[yy[gn],xx[gn]]:
            self._set_plan_failed("no path through known safe cells")
            return
        if sn == gn:
            route = [sn]
            best_cost = 0.
        else:
            sx,sy = positions(np.array([sn]))
            headings = np.flatnonzero(self._start_heading_mask(float(sx[0]),float(sy[0])))
            if not len(headings):
                self._set_plan_failed("no safe initial heading")
                return
            search_key = sn,gn,tuple(map(int,headings))
            # Fixed-node proofs are geometric except the existing source-only
            # departure exception. Retire them on every escape-context change,
            # including the source-match numerical boundary and attestation.
            escape = self._initial_escape
            proof_key = (None if escape is None else (tuple(escape['source']),
                escape['excluded_indices'].tobytes(), id(escape['remaining']),
                self.current_yaw_deg, self._current_pose_for_planning))
            if model.get('implicit_proof_key') != proof_key:
                model['edge_known'].fill(0)
                model['edge_clear'].fill(0)
                model['turn_known'].fill(0)
                model['turn_clear'].fill(0)
                model.pop('search', None)
            model['implicit_proof_key'] = proof_key
            previous_search = model.get('search')
            if previous_search is not None and previous_search[0] == search_key:
                if previous_search[1] is None:
                    self._set_plan_failed("no path through known safe cells")
                    self._plan_fine_search_stats = dict(possible_nodes=count,possible_states=count*8,
                        expansions=0,state_storage_bytes=0,cache_hit=True)
                    return
                route,best_cost = previous_search[1],previous_search[2]
                self._plan_fine_search_stats = dict(possible_nodes=count,possible_states=count*8,
                    expansions=0,state_storage_bytes=0,cache_hit=True)
            else:
                implicit = self._query_implicit_fine(model, sn, gn, headings,
                    self._path_cost_grid(normal_clear, wall_dist), normal_clear)
                if implicit is not None:
                    route, best_cost = implicit
                    self._plan_fine_search_stats = dict(model['implicit_stats'], implicit=True)
                    if not route:
                        model['search'] = search_key, None, None
                        self._set_plan_failed('no path through known safe cells')
                        return
                    model['search'] = search_key, tuple(route), best_cost
                else:
                    scale = self.PATH_FINE_GRID_MM/self.NAV_GRID_MM
                    cardinal,diagonal = float(np.float32(scale)),float(np.float32(math.sqrt(2)*scale))
                    cost_grid = self._path_cost_grid(normal_clear,wall_dist)
                    # Preserve the existing bounded body-proof work/heuristic cache.
                    # Large uniform/partially proved maps retain the indexed search;
                    # never construct a world-sized heading CSR.
                    self._fine_relaxed_goal_distances(
                        model,gn,cost_grid,cardinal,diagonal,prepare_only=True)
                    budget = self.n*self.n
                    if count > budget and np.any(model['body'] < 0):
                        return self._plan_fine_path_indexed(start,goal,normal_clear,wall_dist,requested_corner)
                    if count <= budget:
                        for first in range(0,count,4096):
                            self._check_plan_cancelled()
                            bodies(np.arange(first,min(first+4096,count)))
                    kept = np.flatnonzero(model['body'] > 0).astype(np.int32)
                    if len(kept) > budget:
                        return self._plan_fine_path_indexed(start,goal,normal_clear,wall_dist,requested_corner)
                    cache = model.get('native_fine_graph')
                    graph_cached = cache is not None
                    if cache is None:
                        nx,ny = xx[kept],yy[kept]
                        node_native = np.full(node.shape,-1,dtype=np.int32)
                        node_native[ny,nx] = np.arange(len(kept),dtype=np.int32)
                        vectors = np.asarray(self.PATH_DIR_VECS,dtype=np.int32)
                        turn = np.array([self._turn_costs_from_dir(i,np.arange(8)) for i in range(8)])
                        right = np.array([self._right_turn_mask_from_dir(i,np.arange(8)) for i in range(8)])
                        steps = np.asarray([cardinal]*4+[diagonal]*4,dtype=np.float32)
                        def adjacent(first,last):
                            tx = nx[first:last,None]+vectors[:,0]
                            ty = ny[first:last,None]+vectors[:,1]
                            valid = (tx>=0)&(ty>=0)&(tx<node.shape[1])&(ty<node.shape[0])
                            safe_x=np.clip(tx,0,node.shape[1]-1)
                            safe_y=np.clip(ty,0,node.shape[0]-1)
                            neighbours=node_native[safe_y,safe_x].copy()
                            valid &= neighbours>=0
                            for direction in (4,5,6,7):
                                valid[:,direction] &= (observed[ny[first:last],safe_x[:,direction]]
                                    & observed[safe_y[:,direction],nx[first:last]])
                            neighbours[~valid]=-1
                            return tx,ty,neighbours,valid
                        states=len(kept)*8
                        indptr=np.zeros(states+1,dtype=np.int32)
                        for first in range(0,len(kept),4096):
                            self._check_plan_cancelled();last=min(first+4096,len(kept))
                            _,_,_,valid=adjacent(first,last)
                            indptr[first*8+1:last*8+1]=np.repeat(np.count_nonzero(valid,axis=1),8)
                        np.cumsum(indptr,out=indptr)
                        indices=np.empty(int(indptr[-1]),dtype=np.int32)
                        data=np.empty(len(indices),dtype=np.float32)
                        for first in range(0,len(kept),4096):
                            self._check_plan_cancelled();last=min(first+4096,len(kept))
                            tx,ty,neighbours,valid=adjacent(first,last)
                            safe_x=np.clip(tx,0,node.shape[1]-1);safe_y=np.clip(ty,0,node.shape[0]-1)
                            base=np.multiply(cost_grid[cy[safe_y],cx[safe_x]],steps[None,:],dtype=np.float32)
                            extra=(right[None,:,:]
                                & normal_clear[cy[ny[first:last]],cx[nx[first:last]]][:,None,None]
                                & normal_clear[cy[safe_y],cx[safe_x]][:,None,:]).astype(np.float32)
                            extra*=np.float32(self.PATH_OPEN_RIGHT_TURN_EXTRA_COST)
                            weights=np.float32(np.float32(base[:,None,:]+turn[None,:,:])+extra)
                            mask=np.broadcast_to(valid[:,None,:],weights.shape)
                            row_start=indptr[first*8:last*8].reshape(-1,8,1)
                            slots=(row_start+np.cumsum(mask,axis=2,dtype=np.int32)-1)[mask]
                            columns=np.broadcast_to(neighbours[:,None,:]*8+np.arange(8)[None,None,:],mask.shape)
                            indices[slots]=columns[mask];data[slots]=weights[mask]
                        self._check_plan_cancelled()
                        graph=scipy_sparse.csr_matrix((data,indices,indptr),shape=(states,states),copy=False)
                        for array in(graph.data,graph.indices,graph.indptr):array.flags.writeable=False
                        old_to_new=np.full(count,-1,dtype=np.int32);old_to_new[kept]=np.arange(len(kept),dtype=np.int32)
                        px,py=positions(kept)
                        cache=dict(graph=graph,old_to_new=old_to_new,kept=kept,xy=(nx,ny),positions=(px,py))
                        for array in (kept,old_to_new,nx,ny,px,py):array.flags.writeable=False
                        model['native_fine_graph']=cache
                    graph=cache['graph'];old_to_new=cache['old_to_new']
                    native_sn,native_gn=int(old_to_new[sn]),int(old_to_new[gn])
                    if native_sn < 0 or native_gn < 0:
                        raise RuntimeError('native fine endpoint contradicts exact body proof')
                    sources=native_sn*8+headings;goals=native_gn*8+np.arange(8,dtype=np.int32)
                    native_key=('fine_native',self.PATH_FINE_GRID_MM)
                    dist,pred=self._query_physical_plan_graph(native_key,graph,sources,goals,
                        cache['xy'],cache['positions'],native=True,observe_edges=False)
                    best=int(goals[np.argmin(dist[goals])])
                    self._plan_fine_search_stats=dict(possible_nodes=count,possible_states=count*8,
                        expansions=0,native=True,body_valid_nodes=len(kept),heading_states=graph.shape[0],
                        graph_edges=graph.nnz,graph_bytes=graph.data.nbytes+graph.indices.nbytes+graph.indptr.nbytes,
                        native_graph_cache_hit=graph_cached,physical=dict(self._plan_coarse_physical_stats))
                    if not np.isfinite(dist[best]):
                        model['search']=search_key,None,None
                        self._set_plan_failed('no path through known safe cells')
                        return
                    best_cost=float(dist[best]);route=[];state=best;source_states=set(map(int,sources))
                    for _ in range(graph.shape[0]):
                        self._check_plan_cancelled();route.append(int(cache['kept'][state//8]))
                        if state in source_states:break
                        state=int(pred[state])
                        if not 0 <= state < graph.shape[0]:
                            self._set_plan_failed('path reconstruction failed');return
                    else:
                        self._set_plan_failed('path predecessor cycle');return
                    route.reverse()
                    model['search']=search_key,tuple(route),best_cost
        px,py = positions(np.asarray(route,dtype=int))
        points = [source]+list(zip(px.tolist(),py.tolist()))+[target]
        simplified=[]
        for point in points:
            if simplified and point == simplified[-1]:continue
            if (len(simplified)>=2 and self._path_step_dir(simplified[-2],simplified[-1]) == self._path_step_dir(simplified[-1],point)
                    and abs((simplified[-1][0]-simplified[-2][0])*(point[1]-simplified[-1][1])
                            -(simplified[-1][1]-simplified[-2][1])*(point[0]-simplified[-1][0])) < 1e-10):
                simplified[-1]=point
            else:simplified.append(point)
        if (not self._raw_polyline_clear(simplified)
                or not self._path_turns_clear(simplified)):
            self._set_plan_failed("fine path failed final observation or clearance check")
            return
        self.plan_path=[]
        for x,y in points:
            cell=int(math.floor(x)),int(math.floor(y))
            if not self.plan_path or self.plan_path[-1]!=cell:self.plan_path.append(cell)
        self.plan_destination=goal
        self.plan_destination_corner=requested_corner
        self.plan_corner_path=[]
        self.plan_raw_points=simplified
        self._plan_source_point=source
        self._plan_source_prefix=(tuple(source),)
        self.plan_cost=best_cost
        self.plan_status="ok"
        self._annotate_plan(self.plan_path)
        for annotation in self.plan_annotations:
            x,y=annotation['cell']
            annotation['corner']=min(simplified,key=lambda q:(q[0]-x-.5)**2+(q[1]-y-.5)**2)

    def _plan_path_with_scipy(self, start, goal, walkable, normal_clear,
                              tight_clear, wall_dist, fine=False, requested_corner=None):
        self._check_plan_cancelled()
        if fine:
            known_y,known_x = np.nonzero((self.cell_state != self.CELL_UNKNOWN)
                                         & (self.floor != self.FLOOR_HOLE))
            possible = (((int(known_x.max())-int(known_x.min())+1)*self.NAV_GRID_MM//self.PATH_FINE_GRID_MM+1)
                        *((int(known_y.max())-int(known_y.min())+1)*self.NAV_GRID_MM//self.PATH_FINE_GRID_MM+1)
                        if len(known_x) else 0)
            if possible > self.PATH_FINE_EAGER_MAX_NODES:
                return self._plan_fine_path(start,goal,normal_clear,wall_dist,requested_corner)
        mapper = self._planning_mapper
        cache_key = ("fine", self.PATH_FINE_GRID_MM) if fine else ("cell",)
        cached = self._plan_graph_cache.get(cache_key)
        if cached is None:
            cost_grid = self._path_cost_grid(normal_clear, wall_dist)
            allowed = walkable & tight_clear
            cell_y, cell_x = np.indices(allowed.shape, dtype=float)
            cell_x += 0.5
            cell_y += 0.5
            if fine:
                known_y, known_x = np.nonzero((self.cell_state != self.CELL_UNKNOWN)
                                              & (self.floor != self.FLOOR_HOLE))
                if not len(known_x):
                    self._set_plan_failed("no known safe cells")
                    return
                bound = mapper.c*mapper.GRID_MM
                low = max(-bound, (min(known_x.min(),known_y.min())-self.c)*self.NAV_GRID_MM)
                high = min(bound, (max(known_x.max(),known_y.max())+1-self.c)*self.NAV_GRID_MM)
                coords = np.arange(math.ceil(low/self.PATH_FINE_GRID_MM)*self.PATH_FINE_GRID_MM,
                    high+self.PATH_FINE_GRID_MM, self.PATH_FINE_GRID_MM)
                raw = np.rint(coords/mapper.GRID_MM).astype(int)+mapper.c
                coarse = np.clip(np.floor(coords/self.NAV_GRID_MM).astype(int)+self.c, 0, self.n-1)
                allowed = ((self.cell_state != self.CELL_UNKNOWN)
                           & (self.floor != self.FLOOR_HOLE))[coarse[:, None], coarse[None, :]]
                allowed &= mapper.logodds[np.ix_(raw, raw)] <= mapper.FREE_T
                cell_y, cell_x = np.meshgrid(coords/self.NAV_GRID_MM+self.c,
                                            coords/self.NAV_GRID_MM+self.c, indexing="ij")
                cost_grid = cost_grid[coarse[:, None], coarse[None, :]]
                normal_clear = normal_clear[coarse[:, None], coarse[None, :]]
            if fine:
                # Diagonal neighbours need observed floor, not clearance for an
                # additional off-path chassis. add_edges checks the exact swept
                # body/front footprint along the diagonal itself.
                diagonal_observed = allowed.copy()
            if fine:
                allowed[allowed] = self._physical_segment_mask(
                    cell_x[allowed], cell_y[allowed], cell_x[allowed], cell_y[allowed])
            else:
                known_y,known_x = np.nonzero(allowed)
                for first in range(0,len(known_x),512):
                    self._check_plan_cancelled()
                    ky,kx = known_y[first:first+512],known_x[first:first+512]
                    allowed[ky,kx] = self._physical_segment_mask(
                        cell_x[ky,kx],cell_y[ky,kx],cell_x[ky,kx],cell_y[ky,kx])
            ys, xs = np.nonzero(allowed)
            node_count = int(xs.size)
            if node_count == 0:
                self._set_plan_failed("no known safe cells")
                return
            node = np.full(allowed.shape, -1, dtype=np.int32)
            node[ys, xs] = np.arange(node_count, dtype=np.int32)
            edge_rows = []
            edge_cols = []
            edge_costs = []
            edge_dirs = []

            def add_edges(src, dst, clear, step_cost, direction, extra_cost=None):
                src_nodes = node[src]
                dst_nodes = node[dst]
                m = clear & (src_nodes >= 0) & (dst_nodes >= 0)
                if not np.any(m):
                    return
                if fine:
                    m[m] = self._physical_segment_mask(
                        cell_x[src][m], cell_y[src][m], cell_x[dst][m], cell_y[dst][m])
                    if not np.any(m):
                        return
                dst_ids = dst_nodes[m]
                edge_rows.append(src_nodes[m])
                edge_cols.append(dst_ids)
                costs = (cost_grid[dst][m] * step_cost).astype(np.float32, copy=False)
                if extra_cost is not None:
                    costs = costs + extra_cost[m].astype(np.float32, copy=False)
                edge_costs.append(costs)
                edge_dirs.append(np.full(dst_ids.shape, direction, dtype=np.int8))

            n = allowed.shape[0]
            wall_v = np.zeros((n, n+1), bool) if fine else self.wall_v
            wall_h = np.zeros((n+1, n), bool) if fine else self.wall_h
            step_scale = self.PATH_FINE_GRID_MM/self.NAV_GRID_MM if fine else 1.0
            add_edges((slice(None), slice(0, n - 1)),
                      (slice(None), slice(1, n)),
                      ~wall_v[:, 1:n], step_scale, 0)
            add_edges((slice(None), slice(1, n)),
                      (slice(None), slice(0, n - 1)),
                      ~wall_v[:, 1:n], step_scale, 1)
            add_edges((slice(0, n - 1), slice(None)),
                      (slice(1, n), slice(None)),
                      ~wall_h[1:n, :], step_scale, 2)
            add_edges((slice(1, n), slice(None)),
                      (slice(0, n - 1), slice(None)),
                      ~wall_h[1:n, :], step_scale, 3)

            root2 = np.float32(math.sqrt(2.0)*step_scale)
            diagonal_corner_clear = (self._plan_corner_clear[1:n, 1:n]
                                     if self._plan_corner_clear is not None and not fine
                                     else np.full((n - 1, n - 1), 255, dtype=np.uint8))
            if fine:
                diagonal_corner_clear = np.where(
                    diagonal_observed[:-1, 1:] & diagonal_observed[1:, :-1]
                    & diagonal_observed[:-1, :-1] & diagonal_observed[1:, 1:], 255, 0)
            diagonal_corner_deficit = self._clearance_deficit_from_distance(diagonal_corner_clear)
            diagonal_corner_cost = diagonal_corner_deficit.astype(np.float32) * np.float32(
                self.PATH_CLEARANCE_DEFICIT_PENALTY)
            add_edges((slice(0, n - 1), slice(0, n - 1)),
                      (slice(1, n), slice(1, n)),
                      (~wall_v[0:n - 1, 1:n]
                       & ~wall_h[1:n, 0:n - 1]
                       & ~wall_h[1:n, 1:n]
                       & ~wall_v[1:n, 1:n]
                       & (diagonal_corner_clear >= self.PATH_CLEARANCE_TARGET_CELLS)),
                      root2, 4, diagonal_corner_cost)
            add_edges((slice(0, n - 1), slice(1, n)),
                      (slice(1, n), slice(0, n - 1)),
                      (~wall_v[0:n - 1, 1:n]
                       & ~wall_h[1:n, 1:n]
                       & ~wall_h[1:n, 0:n - 1]
                       & ~wall_v[1:n, 1:n]
                       & (diagonal_corner_clear >= self.PATH_CLEARANCE_TARGET_CELLS)),
                      root2, 5, diagonal_corner_cost)
            add_edges((slice(1, n), slice(0, n - 1)),
                      (slice(0, n - 1), slice(1, n)),
                      (~wall_v[1:n, 1:n]
                       & ~wall_h[1:n, 0:n - 1]
                       & ~wall_h[1:n, 1:n]
                       & ~wall_v[0:n - 1, 1:n]
                       & (diagonal_corner_clear >= self.PATH_CLEARANCE_TARGET_CELLS)),
                      root2, 6, diagonal_corner_cost)
            add_edges((slice(1, n), slice(1, n)),
                      (slice(0, n - 1), slice(0, n - 1)),
                      (~wall_v[1:n, 1:n]
                       & ~wall_h[1:n, 1:n]
                       & ~wall_h[1:n, 0:n - 1]
                       & ~wall_v[0:n - 1, 1:n]
                       & (diagonal_corner_clear >= self.PATH_CLEARANCE_TARGET_CELLS)),
                      root2, 7, diagonal_corner_cost)

            base_rows = np.concatenate(edge_rows) if edge_rows else np.empty(0,dtype=np.int32)
            base_cols = np.concatenate(edge_cols) if edge_cols else np.empty(0,dtype=np.int32)
            base_costs = np.concatenate(edge_costs) if edge_costs else np.empty(0,dtype=np.float32)
            base_dirs = np.concatenate(edge_dirs) if edge_dirs else np.empty(0,dtype=np.int8)
            node_normal = normal_clear[ys, xs]
            node_x, node_y = cell_x[ys, xs], cell_y[ys, xs]
            if fine:
                fully_clear = self._disk_segment_mask(node_x, node_y, node_x, node_y,
                    self.FRONT_SENSOR_OFFSET_MM+self.FRONT_SENSOR_RADIUS_MM,
                    self._plan_material_tree)
            state_rows = []
            state_cols = []
            state_costs = []
            for prev_dir in range(8):
                self._check_plan_cancelled()
                if fine:
                    turn_clear = fully_clear[base_rows].copy()
                    narrow = ~turn_clear
                    turn_clear[narrow] = self._turn_mask_from_dir(node_x[base_rows[narrow]],
                        node_y[base_rows[narrow]], prev_dir, base_dirs[narrow])
                    state_rows.append((base_rows * 8 + prev_dir)[turn_clear])
                    state_cols.append((base_cols * 8 + base_dirs)[turn_clear])
                else:
                    state_rows.append(base_rows * 8 + prev_dir)
                    state_cols.append(base_cols * 8 + base_dirs)
                turn = self._turn_costs_from_dir(prev_dir, base_dirs)
                open_right_turn = (self._right_turn_mask_from_dir(prev_dir, base_dirs)
                                   & node_normal[base_rows]
                                   & node_normal[base_cols])
                open_right_cost = open_right_turn.astype(np.float32, copy=False)
                open_right_cost *= np.float32(self.PATH_OPEN_RIGHT_TURN_EXTRA_COST)
                if fine:
                    state_costs.append((base_costs + turn.astype(np.float32, copy=False)
                                       + open_right_cost)[turn_clear])
                else:
                    state_costs.append(base_costs + turn.astype(np.float32, copy=False)
                                       + open_right_cost)

            graph = scipy_sparse.csr_matrix(
                (np.concatenate(state_costs),
                 (np.concatenate(state_rows), np.concatenate(state_cols))),
                shape=(node_count*8, node_count*8),
            )
            if not fine:
                for array in (graph.data,graph.indices,graph.indptr):array.flags.writeable=False
            cached = (node, ys, xs, cell_x, cell_y, graph)
            self._plan_graph_cache[cache_key] = cached
        node, ys, xs, cell_x, cell_y, graph = cached
        node_count = len(xs)
        sx, sy = start
        gx, gy = goal
        if fine:
            source = (tuple(v/self.NAV_GRID_MM+self.c for v in self.current_world_mm)
                      if self.current_world_mm is not None else (sx+0.5, sy+0.5))
            target = requested_corner if requested_corner is not None else (gx+0.5, gy+0.5)
            def nearest(point, reverse=False):
                near = ((abs(cell_x[ys, xs]-point[0]) <= 1)
                        & (abs(cell_y[ys, xs]-point[1]) <= 1))
                candidates = np.flatnonzero(near)
                candidates = sorted(candidates, key=lambda i:
                    (cell_x[ys[i], xs[i]]-point[0])**2+(cell_y[ys[i], xs[i]]-point[1])**2)
                for i in candidates:
                    position = (cell_x[ys[i], xs[i]], cell_y[ys[i], xs[i]])
                    a,b = (position,point) if reverse else (point,position)
                    if self._raw_segment_clear(a,b):
                        return int(xs[i]), int(ys[i])
                return None
            fine_start, fine_goal = nearest(source), nearest(target, reverse=True)
            if fine_start is None or fine_goal is None:
                self._set_plan_failed("no observed fine-grid connection to start or destination")
                return
            sx, sy = fine_start
            gx, gy = fine_goal
        start_node, goal_node = int(node[sy, sx]), int(node[gy, gx])
        if start_node < 0 or goal_node < 0:
            self._set_plan_failed("start or destination has no robot clearance")
            return
        if start_node == goal_node:
            if not fine and (not self._physical_segment_clear(
                    (cell_x[sy,sx],cell_y[sy,sx]),(cell_x[sy,sx],cell_y[sy,sx]))
                    or (mapper is not None and not self._observed_segment_clear(
                        (cell_x[sy,sx],cell_y[sy,sx]),(cell_x[sy,sx],cell_y[sy,sx])))):
                self._set_plan_failed("start or destination has no robot clearance")
                return
            self.plan_path = [start]
            if fine:
                self.plan_raw_points = [source, (float(cell_x[sy, sx]), float(cell_y[sy, sx])), target]
                self._plan_source_point = source
                self._plan_source_prefix=(tuple(source),)
                if (not self._path_turns_clear(self.plan_raw_points)
                        or not self._raw_polyline_clear(self.plan_raw_points)):
                    self._set_plan_failed("short fine path failed swept footprint clearance")
                    return
            self.plan_cost = 0.0
            self.plan_status = "ok"
            self._annotate_plan(self.plan_path)
            return
        headings = np.flatnonzero(self._start_heading_mask(cell_x[sy,sx],cell_y[sy,sx]))
        if not len(headings):
            self._set_plan_failed("no safe initial heading")
            return
        source_states = start_node*8+headings
        goal_states = goal_node * 8 + np.arange(8, dtype=np.int32)
        if fine:
            dist,pred = self._query_plan_graph(cache_key,graph,source_states,goal_states,(xs,ys))
        else:
            dist,pred = self._query_physical_plan_graph(cache_key,graph,source_states,
                goal_states,(xs,ys),(cell_x[ys,xs],cell_y[ys,xs]),observe_edges=False)
        best_goal_state = int(goal_states[np.argmin(dist[goal_states])])
        if not np.isfinite(dist[best_goal_state]):
            self._set_plan_failed("no path through known safe cells")
            return

        state_path = []
        cur = best_goal_state
        for _ in range(graph.shape[0]):
            self._check_plan_cancelled()
            state_path.append(cur)
            if cur in source_states:
                break
            cur = int(pred[cur])
            if not 0 <= cur < graph.shape[0]:
                self._set_plan_failed("path reconstruction failed")
                return
        else:
            self._set_plan_failed("path predecessor cycle")
            return
        state_path.reverse()
        node_path = []
        for state in state_path:
            node_id = int(state) // 8
            if not node_path or node_path[-1] != node_id:
                node_path.append(node_id)
        path = [(int(xs[p]), int(ys[p])) for p in node_path]
        self.plan_path = path
        self.plan_corner_path = []
        self.plan_raw_points = []
        self._plan_source_point = None
        self._plan_source_prefix=()
        self.plan_cost = float(dist[best_goal_state])
        self.plan_status = "ok"
        if fine:
            points = [source]+[(float(cell_x[y, x]), float(cell_y[y, x])) for x, y in path]+[target]
            simplified = []
            for point in points:
                if simplified and point == simplified[-1]:
                    continue
                if (len(simplified) >= 2
                        and self._path_step_dir(simplified[-2], simplified[-1]) == self._path_step_dir(simplified[-1], point)
                        and abs((simplified[-1][0]-simplified[-2][0])*(point[1]-simplified[-1][1])
                                -(simplified[-1][1]-simplified[-2][1])*(point[0]-simplified[-1][0])) < 1e-10):
                    simplified[-1] = point
                else:
                    simplified.append(point)
            if (not self._raw_polyline_clear(simplified)
                    or not self._path_turns_clear(simplified)):
                self._set_plan_failed("fine path failed final observation or clearance check")
                return
            coarse_path = []
            for x, y in points:
                cell = (int(math.floor(x)), int(math.floor(y)))
                if not coarse_path or cell != coarse_path[-1]:
                    coarse_path.append(cell)
            self.plan_path = coarse_path
            self.plan_destination = goal
            self.plan_destination_corner = requested_corner
            self.plan_raw_points = simplified
            self._plan_source_point = source
            self._plan_source_prefix=(tuple(source),)
            self._annotate_plan(coarse_path)
            for annotation in self.plan_annotations:
                x, y = annotation["cell"]
                annotation["corner"] = min(simplified, key=lambda p: (p[0]-x-0.5)**2+(p[1]-y-0.5)**2)
        else:
            if not self._path_turns_clear([(x+.5,y+.5) for x,y in path]):
                self._set_plan_failed("cell path failed initial or turning clearance check")
                return
            self._annotate_plan(path)

    def _turn_costs_from_dir(self, prev_dir, new_dirs):
        pv = self.PATH_DIR_VECS[prev_dir]
        plen = math.hypot(pv[0], pv[1])
        out = np.zeros(new_dirs.shape, dtype=np.float32)
        for nd in np.unique(new_dirs):
            nv = self.PATH_DIR_VECS[int(nd)]
            nlen = math.hypot(nv[0], nv[1])
            dot = max(-1.0, min(1.0, (pv[0] * nv[0] + pv[1] * nv[1]) / (plen * nlen)))
            angle = math.acos(dot)
            right_turn = angle / (math.pi / 2.0)
            cost = self.PATH_TURN_COST * right_turn
            if angle >= math.pi / 2.0 - 1e-6:
                cost += self.PATH_RIGHT_TURN_EXTRA_COST * right_turn
            out[new_dirs == nd] = np.float32(cost)
        return out

    def _right_turn_mask_from_dir(self, prev_dir, new_dirs):
        pv = self.PATH_DIR_VECS[prev_dir]
        plen = math.hypot(pv[0], pv[1])
        out = np.zeros(new_dirs.shape, dtype=bool)
        for nd in np.unique(new_dirs):
            nv = self.PATH_DIR_VECS[int(nd)]
            nlen = math.hypot(nv[0], nv[1])
            dot = max(-1.0, min(1.0, (pv[0] * nv[0] + pv[1] * nv[1]) / (plen * nlen)))
            out[new_dirs == nd] = math.acos(dot) >= math.pi / 2.0 - 1e-6
        return out

    def _path_cost_grid(self, normal_clear, wall_dist):
        cost = np.ones((self.n, self.n), dtype=np.float32)
        cost[self.floor == self.FLOOR_SWAMP] = np.float32(self.PATH_SWAMP_COST)
        clearance_deficit = self._path_clearance_deficit_grid(wall_dist)
        cost += clearance_deficit.astype(np.float32) * np.float32(
            self.PATH_CLEARANCE_DEFICIT_PENALTY)
        grid_rank = self._path_grid_rank()
        unvisited = self.cell_state == self.CELL_UNVISITED
        cost[unvisited & (grid_rank == 1)] += np.float32(self.PATH_60_GRID_OFF_PENALTY)
        cost[unvisited & (grid_rank == 2)] += np.float32(self.PATH_30_GRID_OFF_PENALTY)
        near_wall = unvisited & (wall_dist <= self.PATH_GATEWAY_LOOKUP_CELLS)
        cost[near_wall & (wall_dist == 1)] += np.float32(self.PATH_WALL_FOLLOW_NEAR_PENALTY)
        cost[near_wall & (wall_dist >= 3)] += np.float32(self.PATH_WALL_FOLLOW_FAR_PENALTY)
        return cost

    def _path_clearance_deficit_grid(self, wall_dist):
        target = np.uint8(self.PATH_CLEARANCE_TARGET_CELLS)
        nearest = wall_dist.copy()
        if self._plan_hole_dist is not None:
            nearest = np.minimum(nearest, self._plan_hole_dist)
        best = nearest.copy()
        if self._plan_v_lane_clear is not None:
            best = np.maximum(best, self._plan_v_lane_clear[:, :-1])
            best = np.maximum(best, self._plan_v_lane_clear[:, 1:])
        if self._plan_h_lane_clear is not None:
            best = np.maximum(best, self._plan_h_lane_clear[:-1, :])
            best = np.maximum(best, self._plan_h_lane_clear[1:, :])
        return self._clearance_deficit_from_distance(best)

    def _clearance_deficit_from_distance(self, dist):
        target = np.uint8(self.PATH_CLEARANCE_TARGET_CELLS)
        capped = np.minimum(dist, target).astype(np.int16, copy=False)
        deficit = int(target) - capped
        deficit[deficit < 0] = 0
        return deficit.astype(np.uint8, copy=False)

    def _set_plan_failed(self, reason):
        self._plan_preview_owner = None
        self._plan_tail_proof = self._plan_display_proof = None
        self._plan_display_points = None
        self.plan_path = []
        self.plan_corner_path = []
        self.plan_raw_points = []
        self._plan_source_point = None
        self._plan_source_prefix=()
        self.plan_waypoints = []
        self.plan_annotations = []
        self.plan_cost = None
        self.plan_status = reason

    _PLAN_GEOMETRY_FIELDS = (
        "_plan_wall_dist", "_plan_hole_dist", "_plan_swamp_dist",
        "_plan_v_lane_clear", "_plan_h_lane_clear", "_plan_corner_clear",
        "_plan_clearance_deficit", "_plan_normal_clear", "_plan_tight_clear",
        "_plan_obstacle_tree", "_plan_material_tree", "_plan_grid_mm")

    def _planning_maps(self, prepare_escape=True):
        mapper = self._planning_mapper
        material, self._plan_grid_mm = ((self._planning_material_snapshot
            if self._planning_material_snapshot is not None else mapper.physical_material())
            if mapper is not None else (np.empty((0,2)),10))
        key = (self._planning_input_signature if self._planning_input_signature is not None
               else self._planning_signature(material,self._plan_grid_mm))
        if key != self._plan_graph_signature:
            self._plan_graph_cache.clear()
            self._plan_search_cache.clear()
            self._plan_graph_signature = key
        cached = self._plan_geometry_cache
        if cached is not None and cached[0] == key:
            for name,value in zip(self._PLAN_GEOMETRY_FIELDS,cached[2]):
                setattr(self,name,value)
            # Geometry is immutable, but the precise initial footprint belongs
            # to this packet's current body pose and must never be reused.
            if prepare_escape:
                self._prepare_initial_escape()
            return cached[1]
        if mapper is not None:
            self._plan_material_tree = scipy_cKDTree(material, leafsize=64, compact_nodes=False, balanced_tree=False) if len(material) else None
            # Floor cells cover 30 mm edge-based regions; raw 10 mm occupancy cells
            # are centre-based. Subdivide the actual floor regions directly so
            # a pit footprint cannot shift by half a raw cell on either axis.
            py, px = np.nonzero(self.floor == self.FLOOR_HOLE)
            offset = np.arange(self._plan_grid_mm/2, self.NAV_GRID_MM, self._plan_grid_mm)
            ox, oy = np.meshgrid(offset, offset)
            origins = np.column_stack(((px-self.c)*self.NAV_GRID_MM,
                                       (py-self.c)*self.NAV_GRID_MM))
            pits = (origins[:,None,:]+np.column_stack((ox.ravel(),oy.ravel()))).reshape(-1,2)
            obstacle = np.vstack([material, pits])
            self._plan_obstacle_tree = scipy_cKDTree(obstacle, leafsize=64, compact_nodes=False, balanced_tree=False) if len(obstacle) else None
        walkable = ((self.cell_state != self.CELL_UNKNOWN)
                    & (self.floor != self.FLOOR_HOLE))
        wall_dist = self._wall_distance_cells(self.PATH_GATEWAY_LOOKUP_CELLS)
        hole_dist = self._floor_distance_cells(
            self.floor == self.FLOOR_HOLE, self.PATH_GATEWAY_LOOKUP_CELLS)
        swamp_dist = self._floor_distance_cells(
            self.floor == self.FLOOR_SWAMP, self.PATH_GATEWAY_LOOKUP_CELLS)
        self._plan_hole_dist = hole_dist
        self._plan_swamp_dist = swamp_dist
        floor_dist = hole_dist
        self._plan_corner_clear = self._corner_clearance_map(floor_dist)
        tight_clear = walkable & (wall_dist >= 1) & (hole_dist >= 1)
        self._plan_v_lane_clear, self._plan_h_lane_clear = self._edge_clearance_maps(
            floor_dist)
        self._plan_clearance_deficit = self._path_clearance_deficit_grid(wall_dist)
        normal_clear = tight_clear & (self._plan_clearance_deficit == 0)
        self._plan_wall_dist = wall_dist
        self._plan_normal_clear = normal_clear
        self._plan_tight_clear = tight_clear
        maps = (walkable,normal_clear,tight_clear,wall_dist)
        context = tuple(getattr(self,name) for name in self._PLAN_GEOMETRY_FIELDS)
        for value in maps+context:
            if isinstance(value,np.ndarray):
                value.flags.writeable = False
            elif isinstance(value,scipy_cKDTree):
                value.data.flags.writeable = False
        self._plan_geometry_cache = (key,maps,context)
        if prepare_escape:
            self._prepare_initial_escape()
        return maps

    def _rectangle_oracle(self, tree):
        cls = NavigationMapper
        if tree is not None and tree.data.flags.writeable:
            self._plan_rectangle_cache = tuple(row for row in
                getattr(self,'_plan_rectangle_cache',()) if row[0] is not tree)
            return None
        if (getattr(cls, '_compiled_rectangle_raster', None) is None
                or self._planning_mapper is None or tree is None or tree.n < 128
                or tree.data.flags.writeable):
            return None
        context = self._plan_geometry_cache
        if context is None or not any(value is tree for value in context[2]):
            return None  # Only this immutable map's owned geometry may be cached.
        grid = self._plan_grid_mm
        cache = getattr(self, '_plan_rectangle_cache', ())
        for saved_tree, saved_grid, oracle in cache:
            if saved_tree is tree and saved_grid == grid:
                return oracle
        oracle = None
        for saved_tree, saved_grid, saved_oracle in cache:
            if (saved_grid == grid and saved_tree.n == tree.n
                    and np.array_equal(saved_tree.data, tree.data)):
                oracle = saved_oracle
                break
        if oracle is None:
            self._check_plan_cancelled()
            bound = self._planning_mapper.c*self._planning_mapper.GRID_MM
            ok, mask, x0, y0 = cls._compiled_rectangle_raster(tree.data,float(grid),float(bound))
            self._check_plan_cancelled()
            if ok and mask.size:
                boxes, _ = cls._compiled_rectangle_runs(mask)
                boxes = boxes.astype(np.float64)
                boxes[:,0] += x0; boxes[:,2] += x0
                boxes[:,1] += y0; boxes[:,3] += y0
                bounds, children = cls._compiled_rectangle_bvh(boxes)
                self._check_plan_cancelled()
                bounds.flags.writeable = children.flags.writeable = False
                oracle = bounds, children
        # Two immutable roles at most; workers share no mutable cache container.
        self._plan_rectangle_cache = ((tree,grid,oracle),)+tuple(
            row for row in cache if row[0] is not tree)[:1]
        return oracle

    def _disk_segment_mask(self, ax, ay, bx, by, radius_mm, tree, precise=False, excluded=None):
        if (excluded is not None or not isinstance(radius_mm,(float,np.float64))
                or not math.isfinite(radius_mm) or radius_mm < 0):
            return self._disk_segment_mask_generic(ax,ay,bx,by,radius_mm,tree,precise,excluded)
        oracle = self._rectangle_oracle(tree)
        if (oracle is None or radius_mm > 2*self._planning_mapper.c*self._planning_mapper.GRID_MM+1):
            return self._disk_segment_mask_generic(ax,ay,bx,by,radius_mm,tree,precise,excluded)
        self._check_plan_cancelled()
        ax,ay,bx,by = np.broadcast_arrays(ax,ay,bx,by)
        pa = np.column_stack(((ax.ravel()-self.c)*self.NAV_GRID_MM,
            (ay.ravel()-self.c)*self.NAV_GRID_MM))
        pb = np.column_stack(((bx.ravel()-self.c)*self.NAV_GRID_MM,
            (by.ravel()-self.c)*self.NAV_GRID_MM))
        if pa.dtype == np.int64 and pb.dtype == np.int64:
            # With |coordinate|<=2**24, differences, their integer products
            # and their two-term sum are exactly representable in float64.
            # Thus the old integer vector/denominator arithmetic is preserved.
            if (np.max(np.abs(pa.astype(np.float64)),initial=0.) > 2**24
                    or np.max(np.abs(pb.astype(np.float64)),initial=0.) > 2**24):
                return self._disk_segment_mask_generic(ax,ay,bx,by,radius_mm,tree,precise,excluded)
            pa,pb = pa.astype(np.float64),pb.astype(np.float64)
        elif pa.dtype != np.float64 or pb.dtype != np.float64:
            return self._disk_segment_mask_generic(ax,ay,bx,by,radius_mm,tree,precise,excluded)
        pa,pb = np.ascontiguousarray(pa),np.ascontiguousarray(pb)
        if not np.isfinite(pa).all() or not np.isfinite(pb).all():
            return self._disk_segment_mask_generic(ax,ay,bx,by,radius_mm,tree,precise,excluded)
        bound = self._planning_mapper.c*self._planning_mapper.GRID_MM
        result = np.all((abs(pa)<=bound)&(abs(pb)<=bound),axis=1)
        for first in range(0,len(pa),4096):
            self._check_plan_cancelled()
            last = min(first+4096,len(pa))
            status = NavigationMapper._compiled_rectangle_capsules(pa[first:last],pb[first:last],float(radius_mm),*oracle)
            result[first:last] &= status != 0
            unknown = np.flatnonzero((status < 0)&result[first:last])+first
            if len(unknown):
                result[unknown] = self._disk_segment_mask_generic(
                    ax.ravel()[unknown],ay.ravel()[unknown],bx.ravel()[unknown],by.ravel()[unknown],
                    radius_mm,tree,precise)
        return result.reshape(ax.shape)

    def _disk_segment_mask_generic(self, ax, ay, bx, by, radius_mm, tree, precise=False, excluded=None):
        """Exact swept-disk clearance against occupied grid squares.

        The closest pair lies at a segment endpoint, at a rectangle corner,
        or at an intersection. This avoids inflating both endpoints to cover
        sampling gaps, which falsely trapped a robot next to a wall even when
        its next movement led away from that wall.
        """
        self._check_plan_cancelled()
        ax,ay,bx,by=np.broadcast_arrays(ax,ay,bx,by)
        result=np.ones(ax.shape,dtype=bool)
        if self._planning_mapper is None or not result.size:return result
        mapper=self._planning_mapper
        pa=np.column_stack(((ax.ravel()-self.c)*self.NAV_GRID_MM,
                            (ay.ravel()-self.c)*self.NAV_GRID_MM))
        point = bool(np.all(ax == bx) and np.all(ay == by))
        pb = pa if point else np.column_stack(((bx.ravel()-self.c)*self.NAV_GRID_MM,
                                                (by.ravel()-self.c)*self.NAV_GRID_MM))
        bound=mapper.c*mapper.GRID_MM
        result &= np.all((abs(pa)<=bound)&(abs(pb)<=bound),axis=1).reshape(ax.shape)
        if tree is None or not tree.n:return result
        vector = None if point else pb-pa
        length = np.zeros(len(pa)) if point else np.linalg.norm(vector,axis=1)
        half=getattr(self, '_plan_grid_mm', mapper.GRID_MM)/2
        midpoint = pa if point else (pa+pb)/2
        if excluded is None:
            distance,nearest=tree.query(midpoint)
            delta=np.maximum(abs(midpoint-tree.data[nearest])-half,0)
            # A midpoint collision proves a swept collision; the nearest-centre
            # bound can also prove a clear segment without enumerating squares.
            collision=np.sum(delta*delta,axis=1)<radius_mm**2-1e-8
            result.ravel()[collision]=False
            pending=np.flatnonzero(result.ravel() & ~(
                distance>radius_mm+half*math.sqrt(2)+length/2+1e-8))
        else:
            # Query-local initial-body exclusions cannot use an excluded
            # nearest square as collision proof. Test every remaining square
            # in the same exact capsule broadphase instead of rebuilding a tree.
            pending=np.flatnonzero(result.ravel())
        if not pending.size:return result
        if not point and excluded is None:
            # Nearest-centre distance is 1-Lipschitz. Every point of a uniform
            # subsegment lies within half its length of its midpoint. Thus ALL
            # these strict bounds prove the whole capsule misses every square;
            # an inconclusive sample still takes the original exact fallback.
            # If the midpoint itself misses the circumcircle bound, the
            # Lipschitz sample certificate cannot succeed; keep exact testing.
            long = pending[(length[pending] > mapper.GRID_MM/2)
                           & (distance[pending] > radius_mm+half*math.sqrt(2)+1e-8)]
            proved = []
            for begin in range(0, len(long), 32):
                self._check_plan_cancelled()
                ids = long[begin:begin+32]
                counts = np.ceil(length[ids]/(mapper.GRID_MM/2)).astype(int)
                starts = np.r_[0, np.cumsum(counts)]
                owners = np.repeat(np.arange(len(ids)), counts)
                fraction = (np.arange(starts[-1])-starts[owners]+.5)/counts[owners]
                samples = pa[ids[owners]]+fraction[:, None]*vector[ids[owners]]
                distances = tree.query(samples)[0]
                safe = distances > (radius_mm+half*math.sqrt(2)
                                    +length[ids[owners]]/(2*counts[owners])+1e-8)
                proved.extend(ids[np.logical_and.reduceat(safe, starts[:-1])])
            if proved:
                pending = pending[~np.isin(pending, proved)]
            if not pending.size:return result
        pa,pb = pa[pending],pb[pending]
        if not point:
            vector = vector[pending]
        nearby=tree.query_ball_point(midpoint[pending],
            radius_mm+half*math.sqrt(2)+length[pending]/2)
        counts=np.fromiter(map(len,nearby),dtype=int,count=len(pa))
        owners=np.repeat(np.arange(len(pa)),counts)
        if not owners.size:return result
        indices=np.concatenate([v for v in nearby if v]).astype(int)
        if excluded is not None:
            keep=~np.isin(indices,excluded)
            owners,indices=owners[keep],indices[keep]
            if not len(indices):return result
        centres=tree.data[indices]
        if point:
            # For EXACT a==b the point-to-square endpoint distance is the
            # complete swept distance; corner/slab work cannot improve it.
            delta = np.maximum(abs(pa[owners]-centres)-half,0)
            minimum = np.sum(delta*delta,axis=1)
            result.ravel()[pending[owners[minimum < radius_mm**2-1e-8]]] = False
            return result
        start,end,v=pa[owners],pb[owners],vector[owners]
        d0=np.maximum(abs(start-centres)-half,0)
        d1=np.maximum(abs(end-centres)-half,0)
        minimum=np.minimum(np.sum(d0*d0,axis=1),np.sum(d1*d1,axis=1))
        denominator=np.maximum(np.sum(v*v,axis=1),1e-30)
        for dx,dy in ((-half,-half),(-half,half),(half,-half),(half,half)):
            corner=centres+np.array([dx,dy])
            t=np.clip(np.sum((corner-start)*v,axis=1)/denominator,0,1)
            delta=corner-(start+t[:,None]*v)
            minimum=np.minimum(minimum,np.sum(delta*delta,axis=1))
        low,high=centres-half,centres+half
        moving=abs(v)>1e-12
        enter=np.full(v.shape,-np.inf);leave=np.full(v.shape,np.inf)
        np.divide(low-start,v,out=enter,where=moving)
        np.divide(high-start,v,out=leave,where=moving)
        parallel_outside=np.any(~moving&((start<low)|(start>high)),axis=1)
        first=np.maximum(0,np.max(np.minimum(enter,leave),axis=1))
        last=np.minimum(1,np.min(np.maximum(enter,leave),axis=1))
        minimum[(first<=last)&~parallel_outside]=0
        result.ravel()[pending[owners[minimum < radius_mm**2-1e-8]]]=False
        return result

    def _physical_segment_mask(self, ax, ay, bx, by, precise=False):
        ax, ay, bx, by = np.broadcast_arrays(ax, ay, bx, by)
        result = self._disk_segment_mask(ax, ay, bx, by, self.ROBOT_RADIUS_MM,
                                         self._plan_obstacle_tree, precise)
        escape = self._initial_escape
        if escape is not None:
            candidate=(~result)&self._initial_escape_source_mask(ax,ay)&(np.hypot(bx-ax,by-ay)>1e-9)
            for index in np.flatnonzero(candidate):
                a=(float(ax.ravel()[index]),float(ay.ravel()[index]))
                b=(float(bx.ravel()[index]),float(by.ravel()[index]))
                if (self._initial_escape_direction_clear(a,b)
                        and self._disk_segment_mask(*b,*b,self.ROBOT_RADIUS_MM,self._plan_obstacle_tree)
                        and self._disk_segment_mask(*a,*b,self.ROBOT_RADIUS_MM,escape['remaining'],
                            excluded=escape['excluded_indices'])
                        and self._observed_segment_clear(a,b)):
                    result.ravel()[index]=True
        length = np.hypot(bx-ax, by-ay)
        moving = result & (length > 1e-9)
        if np.any(moving):
            scale = self.FRONT_SENSOR_OFFSET_MM/self.NAV_GRID_MM/length[moving]
            ox, oy = (bx-ax)[moving]*scale, (by-ay)[moving]*scale
            # The raised IR sensor can overhang a pit; only physical wall and
            # obstacle material can collide with its bounding sphere.
            result[moving] &= self._disk_segment_mask(
                ax[moving]+ox, ay[moving]+oy, bx[moving]+ox, by[moving]+oy,
                self.FRONT_SENSOR_RADIUS_MM, self._plan_material_tree, precise)
        return result

    def _prepare_initial_escape(self):
        """A non-worsening departure from current conservative grid overlap.

        The circular chassis already occupies its current footprint. Only an
        initial connector may omit overlapping material squares, and only when
        distance to EVERY point in each omitted square is nondecreasing. This
        is not a claim that the current overlap is free, nor an inter-beam
        inference. Graph nodes, destination clearance, pits, unknown floor and
        the original forward-sensor material/sweep checks remain unchanged.
        """
        self._initial_escape = None
        mapper = self._planning_mapper
        if (mapper is None or self.current_world_mm is None
                or self._current_pose_for_planning != (self.current_world_mm,self.current_yaw_deg,self.last_update_t)
                or self._plan_material_tree is None or self._plan_obstacle_tree is None):
            return
        source=np.asarray(self.current_world_mm,dtype=float)
        obstacle=self._plan_obstacle_tree.data
        half=getattr(self, '_plan_grid_mm', mapper.GRID_MM)/2
        nearby=np.asarray(self._plan_obstacle_tree.query_ball_point(
            source,self.ROBOT_RADIUS_MM+half*math.sqrt(2)),dtype=np.intp)
        overlap=nearby[np.sum(np.maximum(abs(obstacle[nearby]-source)-half,0)**2,axis=1)
            < self.ROBOT_RADIUS_MM**2-1e-8]
        overlap.sort()
        if not len(overlap) or np.any(np.all(abs(obstacle[overlap]-source)<=half,axis=1)):
            return  # no nonzero direction can move away from a containing square
        gx=np.clip(np.floor(obstacle[overlap,0]/self.NAV_GRID_MM).astype(int)+self.c,0,self.n-1)
        gy=np.clip(np.floor(obstacle[overlap,1]/self.NAV_GRID_MM).astype(int)+self.c,0,self.n-1)
        if np.any(self.floor[gy,gx]==self.FLOOR_HOLE):
            return
        self._initial_escape=dict(source=source/self.NAV_GRID_MM+self.c,
            excluded=obstacle[overlap],excluded_indices=overlap,
            remaining=self._plan_obstacle_tree)

    def _initial_escape_source_mask(self, x, y):
        if self._initial_escape is None:
            return np.zeros(np.broadcast(x,y).shape,dtype=bool)
        source=self._initial_escape['source']
        return (abs(x-source[0])<=1e-8)&(abs(y-source[1])<=1e-8)

    def _initial_escape_direction_clear(self, a, b):
        """All-corner half-space test; valid for every interior point and t>=0.

        For d=b-a and any material point q in an excluded square, the squared
        distance derivative is 2*d.(a-q)+2*t*|d|^2. Its first term is nonnegative
        exactly when it is nonnegative at all four square vertices. Maximizing
        d.q over the square gives the compact support-function expression below.
        """
        escape=self._initial_escape
        if escape is None or not self._initial_escape_source_mask(*a):return False
        start=(np.asarray(a)-self.c)*self.NAV_GRID_MM
        delta=(np.asarray(b)-np.asarray(a))*self.NAV_GRID_MM
        if np.dot(delta,delta)<=1e-16:return False
        half=getattr(self, '_plan_grid_mm', self._planning_mapper.GRID_MM)/2
        return bool(np.all(np.sum((start-escape['excluded'])*delta,axis=1)
            >= half*np.sum(abs(delta))-1e-8))

    def _heading_turn_mask(self, x, y, before, after):
        x,y,before,after=np.broadcast_arrays(x,y,before,after)
        result=self._heading_turn_mask_model(x,y,before,after,self._plan_obstacle_tree)
        escape=self._initial_escape
        if escape is not None:
            candidate=(~result)&self._initial_escape_source_mask(x,y)
            for index in np.flatnonzero(candidate):
                source=(float(x.ravel()[index]),float(y.ravel()[index]))
                if self._observed_segment_clear(source,source):
                    result.ravel()[index]=self._heading_turn_mask_model(*source,
                        float(before.ravel()[index]),float(after.ravel()[index]),escape['remaining'],
                        excluded=escape['excluded_indices'])
        return result

    def _heading_turn_mask_model(self, x, y, before, after, obstacle_tree, excluded=None,
                                 body_clear=None, full_turn_clear=None):
        """Swept forward sensor during the shorter in-place rotation.

        Angles use atan2(map_y,map_x). At a half-turn either direction is legal.
        Most nodes have full 40 mm clearance and need no angular refinement.
        """
        x, y, before, after = np.broadcast_arrays(x, y, before, after)
        # Optional proofs come only from this fine model's exact stationary
        # body/full-turn arrays. Generic/current-source callers still compute
        # both masks, including their source-only exclusion semantics.
        result = (self._disk_segment_mask(x,y,x,y,self.ROBOT_RADIUS_MM,
                    obstacle_tree,excluded=excluded) if body_clear is None else
                  np.broadcast_to(body_clear,x.shape).copy())
        if self._planning_mapper is None or self._plan_material_tree is None:
            return result
        full = (self._disk_segment_mask(x,y,x,y,
                    self.FRONT_SENSOR_OFFSET_MM+self.FRONT_SENSOR_RADIUS_MM,
                    self._plan_material_tree) if full_turn_clear is None else
                np.broadcast_to(full_turn_clear,x.shape))
        narrow = result & ~full
        if not np.any(narrow):
            return result
        nx, ny, angle = x[narrow], y[narrow], before[narrow]
        delta = (after[narrow]-angle+math.pi) % (2*math.pi)-math.pi
        radius = self.FRONT_SENSOR_OFFSET_MM
        # Each rotation keeps the original scalar sampling and half-gap
        # inflation. Another vertex must not change its geometric predicate.
        steps = np.maximum(1,np.ceil(abs(delta)*radius).astype(np.intp))
        margin = abs(delta)*radius/(2*steps)
        sensor_radius = self.FRONT_SENSOR_RADIUS_MM+margin
        grid = getattr(self, '_plan_grid_mm', self._planning_mapper.GRID_MM)
        bound = self._planning_mapper.c*self._planning_mapper.GRID_MM
        cx,cy = (nx-self.c)*self.NAV_GRID_MM,(ny-self.c)*self.NAV_GRID_MM
        def sweep(change):
            clear = np.ones(len(nx),dtype=bool)
            # Equal sample counts share the exact scalar np.linspace. The
            # existing512-entry bound also bounds angular temporary arrays.
            for count in np.unique(steps):
                group = np.flatnonzero(steps == count)
                samples = np.linspace(0,1,int(count)+1)
                for begin in range(0,len(group),512):
                    self._check_plan_cancelled()
                    ids = group[begin:begin+512]
                    a = angle[ids,None]+change[ids,None]*samples
                    px = cx[ids,None]+radius*np.cos(a)
                    py = cy[ids,None]+radius*np.sin(a)
                    clear[ids] = np.all((abs(px)<=bound)&(abs(py)<=bound),axis=1)
                    nearby=self._plan_material_tree.query_ball_point(
                        np.column_stack((cx[ids],cy[ids])),
                        radius+sensor_radius[ids]+grid/math.sqrt(2))
                    sizes=np.fromiter(map(len,nearby),dtype=int,count=len(ids))
                    owners=np.repeat(np.arange(len(ids)),sizes)
                    if not owners.size:continue
                    indices=np.concatenate([v for v in nearby if v]).astype(int)
                    obstacle=self._plan_material_tree.data[indices]
                    dx=np.maximum(abs(px[owners]-obstacle[:,0,None])-grid/2,0)
                    dy=np.maximum(abs(py[owners]-obstacle[:,1,None])-grid/2,0)
                    unsafe=np.any(dx*dx+dy*dy < sensor_radius[ids[owners],None]**2,axis=1)
                    clear[ids[owners[unsafe]]]=False
            return clear
        clear = sweep(delta)
        half_turn = np.isclose(abs(delta), math.pi) & ~clear
        if np.any(half_turn):
            clear |= half_turn & sweep(-delta)
        result[narrow] &= clear
        return result

    def _turn_mask_from_dir(self, x, y, previous, following):
        vectors = np.asarray(self.PATH_DIR_VECS)
        angles = np.arctan2(vectors[:, 1], vectors[:, 0])
        return self._heading_turn_mask(x, y, angles[previous], angles[following])

    def _start_heading_mask(self, x, y):
        vectors = np.asarray(self.PATH_DIR_VECS)
        angles = np.arctan2(vectors[:, 1], vectors[:, 0])
        if self.current_world_mm is None or self.current_yaw_deg is None:
            return np.ones(8, dtype=bool)  # offline unoriented graph request
        source = np.asarray(self.current_world_mm)/self.NAV_GRID_MM+self.c
        before = math.pi/2-math.radians(OccupancyMapper.YAW_OFFSET_DEG
                                       + OccupancyMapper.YAW_SIGN*self.current_yaw_deg)
        if math.hypot(x-source[0], y-source[1]) > 1e-9:
            connector = math.atan2(y-source[1], x-source[0])
            if (not self._physical_segment_clear(source,(x,y))
                    or not self._heading_turn_mask(*source,before,connector)):
                return np.zeros(8, dtype=bool)
            before = connector
        return self._heading_turn_mask(x,y,before,angles)

    def _path_turns_clear(self, points):
        points = np.asarray(points, dtype=float)
        if len(points) > 1:
            points = points[np.r_[True, np.any(np.diff(points, axis=0) != 0, axis=1)]]
        if len(points) < 2 or self._planning_mapper is None:
            return True
        if self.current_world_mm is not None:
            source = np.asarray(self.current_world_mm)/self.NAV_GRID_MM+self.c
            if np.linalg.norm(source-points[0]) > 1e-9:
                if not self._physical_segment_clear(source,points[0]):
                    return False
                points = np.vstack((source,points))
        direction = np.diff(points, axis=0)
        angles = np.arctan2(direction[:, 1], direction[:, 0])
        if self.current_yaw_deg is not None:
            before = math.pi/2-math.radians(OccupancyMapper.YAW_OFFSET_DEG
                                           + OccupancyMapper.YAW_SIGN*self.current_yaw_deg)
            if (len(points) > 2 and math.isfinite(before)
                    and np.all((points >= 0) & (points <= self.n))):
                # Each turn retains its exact independent scalar predicate.
                # Invalid inputs keep the original source-first error/exit path.
                return bool(np.all(self._heading_turn_mask(
                    points[:-1, 0], points[:-1, 1],
                    np.r_[before, angles[:-1]], angles)))
            if not self._heading_turn_mask(*points[0],before,angles[0]):
                return False
        if len(points) > 2 and not np.all(self._heading_turn_mask(
                points[1:-1, 0], points[1:-1, 1], angles[:-1], angles[1:])):
            return False
        return True

    def _physical_segment_clear(self, a, b):
        return bool(self._physical_segment_mask(*a, *b))

    def _raw_polyline_clear(self, points):
        """Same current collision/observation gates with bounded leg batches."""
        if len(points) < 3:
            return all(self._raw_segment_clear(a,b) for a,b in zip(points,points[1:]))
        if self._planning_mapper is None:
            return False
        for first in range(0,len(points)-1,512):
            self._check_plan_cancelled()
            last=min(first+512,len(points)-1)
            p=np.asarray(points[first:last+1],dtype=float)
            a,b=p[:-1],p[1:]
            if np.any(np.all(a==b,axis=1)):
                # Preserve the scalar exact-point branch in mixed batches.
                if not all(self._raw_segment_clear(x,y) for x,y in zip(a,b)):
                    return False
            elif (not np.all(self._physical_segment_mask(a[:,0],a[:,1],b[:,0],b[:,1]))
                    or not all(self._observed_segment_clear(x,y) for x,y in zip(a,b))):
                return False
        self._check_plan_cancelled()
        return True

    def _raw_segment_clear(self, a, b):
        mapper = self._planning_mapper
        if mapper is None or not self._physical_segment_clear(a, b):
            return False
        return self._observed_segment_clear(a,b)

    def _observed_segment_clear(self, a, b):
        mapper = self._planning_mapper
        if mapper is None:return False
        steps = max(1, math.ceil(math.dist(a, b)*self.NAV_GRID_MM/(mapper.GRID_MM/2)))
        points = np.linspace(a, b, steps+1)
        world = (points-self.c)*self.NAV_GRID_MM
        raw = np.rint(world/mapper.GRID_MM).astype(int)+mapper.c
        nav = np.floor(points).astype(int)
        if (np.any(raw < 0) or np.any(raw >= mapper.n)
                or np.any(nav < 0) or np.any(nav >= self.n)):
            return False
        return bool(np.all((mapper.logodds[raw[:, 1], raw[:, 0]] <= mapper.FREE_T)
                           & (self.cell_state[nav[:, 1], nav[:, 0]] != self.CELL_UNKNOWN)
                           & (self.floor[nav[:, 1], nav[:, 0]] != self.FLOOR_HOLE)))

    def _corner_segment_clear(self, a, b, physical_proven=False):
        # The fallback graph can draw half-integer cell centres. They are not
        # corner lanes; retain its already validated centre path in that case.
        if any(int(v) != v for v in (*a, *b)):
            return False
        a, b = tuple(map(int, a)), tuple(map(int, b))
        dx, dy = b[0]-a[0], b[1]-a[1]
        steps = max(abs(dx), abs(dy))
        if not steps or (dx and dy and abs(dx) != abs(dy)):
            return False
        # Preserve prompt cancellation before walking the cheap lane gates.
        self._check_plan_cancelled()
        sx, sy = int(np.sign(dx)), int(np.sign(dy))
        for k in range(steps+1):
            ex, ey = a[0]+k*sx, a[1]+k*sy
            if not (0 <= ex <= self.n and 0 <= ey <= self.n):
                return False
            if self._plan_corner_clear is not None and self._plan_corner_clear[ey, ex] < 1:
                return False
            if self._corner_on_wall(ex, ey):
                return False
            if k == steps:
                break
            gx, gy = min(ex, ex+sx), min(ey, ey+sy)
            if dx and dy:
                if (not self.in_bounds(gx, gy) or self.cell_state[gy, gx] == self.CELL_UNKNOWN
                        or self.floor[gy, gx] == self.FLOOR_HOLE):
                    return False
            elif dx:
                if self.wall_h[ey, gx]:
                    return False
                cells = ((gx, ey-1), (gx, ey))
                if not any(self.cell_known(*c) and self.floor[c[1], c[0]] != self.FLOOR_HOLE for c in cells):
                    return False
                if self._plan_h_lane_clear is not None and self._plan_h_lane_clear[ey, gx] < 1:
                    return False
            else:
                if self.wall_v[gy, ex]:
                    return False
                cells = ((ex-1, gy), (ex, gy))
                if not any(self.cell_known(*c) and self.floor[c[1], c[0]] != self.FLOOR_HOLE for c in cells):
                    return False
                if self._plan_v_lane_clear is not None and self._plan_v_lane_clear[gy, ex] < 1:
                    return False
        # A proved fixed-tail edge bypasses only this identical capsule query.
        return physical_proven or self._physical_segment_clear(a, b)

    def _wall_distance_cells(self, max_dist):
        wall_adjacent = np.zeros((self.n, self.n), dtype=bool)
        wall_adjacent |= self.wall_v[:, 1:]
        wall_adjacent |= self.wall_v[:, :-1]
        wall_adjacent |= self.wall_h[1:, :]
        wall_adjacent |= self.wall_h[:-1, :]

        if not np.any(wall_adjacent):
            return np.full((self.n, self.n), 255, dtype=np.uint8)
        dist = scipy_ndimage.distance_transform_cdt(
            ~wall_adjacent, metric="chessboard")
        dist = np.clip(dist, 0, 255).astype(np.uint8, copy=False)
        dist[dist > max_dist] = 255
        return dist

    def _floor_distance_cells(self, floor_mask, max_dist):
        if not np.any(floor_mask):
            return np.full((self.n, self.n), 255, dtype=np.uint8)
        dist = scipy_ndimage.distance_transform_cdt(
            ~floor_mask, metric="chessboard")
        dist = np.clip(dist, 0, 255).astype(np.uint8, copy=False)
        dist[dist > max_dist] = 255
        return dist

    def _edge_clearance_maps(self, floor_dist):
        v_wall = (scipy_ndimage.distance_transform_cdt(~self.wall_v, metric="chessboard")
                  if np.any(self.wall_v) else np.full(self.wall_v.shape, 255))
        h_wall = (scipy_ndimage.distance_transform_cdt(~self.wall_h, metric="chessboard")
                  if np.any(self.wall_h) else np.full(self.wall_h.shape, 255))
        v_clear = np.clip(v_wall, 0, 255).astype(np.uint8, copy=False)
        h_clear = np.clip(h_wall, 0, 255).astype(np.uint8, copy=False)

        v_floor = np.full((self.n, self.n + 1), 255, dtype=np.uint8)
        v_floor[:, 0] = floor_dist[:, 0]
        v_floor[:, self.n] = floor_dist[:, self.n - 1]
        v_floor[:, 1:self.n] = np.minimum(floor_dist[:, :-1], floor_dist[:, 1:])

        h_floor = np.full((self.n + 1, self.n), 255, dtype=np.uint8)
        h_floor[0, :] = floor_dist[0, :]
        h_floor[self.n, :] = floor_dist[self.n - 1, :]
        h_floor[1:self.n, :] = np.minimum(floor_dist[:-1, :], floor_dist[1:, :])

        if self._plan_corner_clear is not None:
            v_clear = np.minimum(v_clear, self._plan_corner_clear[:-1, :])
            v_clear = np.minimum(v_clear, self._plan_corner_clear[1:, :])
            h_clear = np.minimum(h_clear, self._plan_corner_clear[:, :-1])
            h_clear = np.minimum(h_clear, self._plan_corner_clear[:, 1:])

        return np.minimum(v_clear, v_floor), np.minimum(h_clear, h_floor)

    def _corner_clearance_map(self, floor_dist):
        corner_wall = np.zeros((self.n + 1, self.n + 1), dtype=bool)
        ys, xs = np.nonzero(self.wall_v)
        corner_wall[ys, xs] = True
        corner_wall[ys + 1, xs] = True
        ys, xs = np.nonzero(self.wall_h)
        corner_wall[ys, xs] = True
        corner_wall[ys, xs + 1] = True
        if np.any(corner_wall):
            wall_clear = scipy_ndimage.distance_transform_cdt(
                ~corner_wall, metric="chessboard")
            clear = np.clip(wall_clear, 0, 255).astype(np.uint8, copy=False)
        else:
            clear = np.full((self.n + 1, self.n + 1), 255, dtype=np.uint8)

        floor_clear = np.full((self.n + 1, self.n + 1), 255, dtype=np.uint8)
        floor_clear[:-1, :-1] = np.minimum(floor_clear[:-1, :-1], floor_dist)
        floor_clear[1:, :-1] = np.minimum(floor_clear[1:, :-1], floor_dist)
        floor_clear[:-1, 1:] = np.minimum(floor_clear[:-1, 1:], floor_dist)
        floor_clear[1:, 1:] = np.minimum(floor_clear[1:, 1:], floor_dist)
        return np.minimum(clear, floor_clear)

    def _near_main_grid_line(self, gx, gy):
        return ((gx - self.c) % 4 in (1, 2)
                or (gy - self.c) % 4 in (1, 2))

    def _main_grid_mask(self):
        return self._path_grid_rank() == 0

    def _path_grid_rank(self):
        idx = np.arange(self.n, dtype=np.int32)
        mod = (idx - self.c) % 4
        on_120 = (mod == 1) | (mod == 2)
        on_60 = (mod == 0) | (mod == 3)
        rank = np.full((self.n, self.n), 2, dtype=np.uint8)
        rank[on_60[None, :] | on_60[:, None]] = 1
        rank[on_120[None, :] | on_120[:, None]] = 0
        return rank

    def _annotate_plan(self, path):
        # Fine/native routes already contain the validated display geometry.
        # Raster indices identify cells only; gateway grouping must never
        # delete actual turns or place an inferred marker off that geometry.
        if self.plan_raw_points or self._plan_display_points is not None:
            corners = self._path_corner_points()
            annotations = []
            for a, b, c in zip(corners, corners[1:], corners[2:]):
                before = math.atan2(b[1]-a[1], b[0]-a[0])
                after = math.atan2(c[1]-b[1], c[0]-b[0])
                if a == b or b == c or abs(math.remainder(after-before, 2*math.pi)) <= 1e-7:
                    continue
                index, cell = self._nearest_path_index_for_corner(path, b)
                reason = self._geometric_bend_reason(a, b, c)
                annotations.append(dict(index=index, cell=cell, corner=tuple(b), reason=reason))
            self.plan_annotations = annotations
            self.plan_waypoints = ([path[0]] + [a['cell'] for a in annotations] + [path[-1]]) if path else []
            return
        annotations = []
        if len(path) >= 3:
            corners = (list(self.plan_raw_points) if self.plan_raw_points else
                       list(self.plan_corner_path) if self.plan_corner_path else
                       list(self._plan_display_points) if self._plan_display_points is not None else
                       self._path_corner_points_for(path, smooth=True))
            prev_dir = self._path_step_dir(corners[0], corners[1]) if len(corners) >= 2 else None
            for i in range(1, len(corners) - 1):
                if corners[i] == corners[i - 1]:
                    continue
                if corners[i + 1] == corners[i]:
                    continue
                next_dir = self._path_step_dir(corners[i], corners[i + 1])
                if next_dir != prev_dir:
                    path_index, cell = self._nearest_path_index_for_corner(path, corners[i])
                    annotations.append({
                        "index": path_index,
                        "cell": cell,
                        "corner": corners[i],
                        "reason": self._bend_reason(
                            path[max(0, path_index - 1)],
                            cell,
                            path[min(len(path) - 1, path_index + 1)]),
                    })
                prev_dir = next_dir

        if self._plan_normal_clear is not None and path:
            tight = [not bool(self._plan_normal_clear[y, x]) for x, y in path]
            i = 0
            while i < len(tight):
                if not tight[i]:
                    i += 1
                    continue
                j = i
                while j < len(tight) and tight[j]:
                    j += 1
                corner = self._tight_gateway_corner(path, i, j)
                cell = path[(i + j - 1) // 2]
                annotations = [
                    a for a in annotations
                    if not (i <= int(a.get("index", -1)) < j)
                ]
                if corner is not None and not self._annotation_has_corner(annotations, corner):
                    annotations.append({
                        "index": (i + j - 1) // 2,
                        "cell": cell,
                        "corner": corner,
                        "reason": "tight_gateway",
                    })
                i = j

        annotations.sort(key=lambda a: int(a.get("index", 0)))
        self.plan_annotations = annotations
        self.plan_waypoints = [path[0]] + [a["cell"] for a in annotations] + [path[-1]]

    @staticmethod
    def _annotation_has_corner(annotations, corner):
        return any(tuple(a.get("corner", ())) == tuple(corner) for a in annotations)

    def _nearest_path_index_for_corner(self, path, corner):
        best = None
        ex, ey = corner
        for i, (gx, gy) in enumerate(path):
            if self._cell_touches_corner((gx, gy), corner):
                score = 0
            else:
                cx = gx + 0.5
                cy = gy + 0.5
                score = abs(ex - cx) + abs(ey - cy)
            if best is None or score < best[0]:
                best = (score, i, (gx, gy))
        return best[1], best[2]

    def _tight_gateway_corner(self, path, start, end):
        segment = path[start:end]
        if not segment:
            return None
        before = path[start - 1] if start > 0 else segment[0]
        after = path[end] if end < len(path) else segment[-1]
        dx = after[0] - before[0]
        dy = after[1] - before[1]
        mid = segment[len(segment) // 2]
        use_exit = start == 0 and end < len(path)
        anchor = segment[-1] if use_exit else segment[0]
        anchor_corner = self._path_cell_corner_point(*anchor)
        if start == 0 or end == len(path):
            return self._nearest_open_corner(*anchor_corner)
        if abs(dy) >= abs(dx):
            corner = self._tight_gateway_between_vertical_walls(segment, dy, use_exit)
        else:
            corner = self._tight_gateway_between_horizontal_walls(segment, dx, use_exit)
        if (corner is None or self._corner_on_wall(*corner)
                or max(abs(corner[0] - anchor_corner[0]),
                       abs(corner[1] - anchor_corner[1])) > 2):
            corner = self._nearest_open_corner(*anchor_corner)
        if self._corner_on_wall(*corner):
            corner = self._nearest_open_corner(*self._path_cell_corner_point(*mid))
        return corner

    def _tight_gateway_between_vertical_walls(self, segment, dy, use_exit=False):
        xs = [gx for gx, _ in segment]
        ys = [gy for _, gy in segment]
        gx = int(round(float(np.median(xs))))
        rows = sorted(set(ys))
        left_edges = []
        right_edges = []
        for gy in rows:
            for ex in range(gx, max(-1, gx - 5), -1):
                if 0 <= gy < self.n and 0 <= ex <= self.n and self.wall_v[gy, ex]:
                    left_edges.append(ex)
                    break
            for ex in range(gx + 1, min(self.n + 1, gx + 6)):
                if 0 <= gy < self.n and self.wall_v[gy, ex]:
                    right_edges.append(ex)
                    break
        if not left_edges or not right_edges:
            return None
        left = float(np.median(left_edges))
        right = float(np.median(right_edges))
        gap = right - left
        if gap < 2.0 or gap > 4.0:
            return None
        ex = int(math.floor((left + right) / 2.0 + 0.5))
        if dy > 0:
            ey = max(ys) + 1 if use_exit else min(ys)
        elif dy < 0:
            ey = min(ys) if use_exit else max(ys) + 1
        else:
            ey = self._path_cell_corner_point(*segment[len(segment) // 2])[1]
        return max(0, min(self.n, ex)), max(0, min(self.n, ey))

    def _tight_gateway_between_horizontal_walls(self, segment, dx, use_exit=False):
        xs = [gx for gx, _ in segment]
        ys = [gy for _, gy in segment]
        gy = int(round(float(np.median(ys))))
        cols = sorted(set(xs))
        low_edges = []
        high_edges = []
        for gx in cols:
            for ey in range(gy, max(-1, gy - 5), -1):
                if 0 <= gx < self.n and 0 <= ey <= self.n and self.wall_h[ey, gx]:
                    low_edges.append(ey)
                    break
            for ey in range(gy + 1, min(self.n + 1, gy + 6)):
                if 0 <= gx < self.n and self.wall_h[ey, gx]:
                    high_edges.append(ey)
                    break
        if not low_edges or not high_edges:
            return None
        low = float(np.median(low_edges))
        high = float(np.median(high_edges))
        gap = high - low
        if gap < 2.0 or gap > 4.0:
            return None
        ey = int(math.floor((low + high) / 2.0 + 0.5))
        if dx > 0:
            ex = max(xs) + 1 if use_exit else min(xs)
        elif dx < 0:
            ex = min(xs) if use_exit else max(xs) + 1
        else:
            ex = self._path_cell_corner_point(*segment[len(segment) // 2])[0]
        return max(0, min(self.n, ex)), max(0, min(self.n, ey))

    def _corner_on_wall(self, ex, ey):
        if 0 <= ey <= self.n and 0 <= ex < self.n and self.wall_h[ey, ex]:
            return True
        if 0 <= ey <= self.n and 0 < ex <= self.n and self.wall_h[ey, ex - 1]:
            return True
        if 0 <= ey < self.n and 0 <= ex <= self.n and self.wall_v[ey, ex]:
            return True
        if 0 < ey <= self.n and 0 <= ex <= self.n and self.wall_v[ey - 1, ex]:
            return True
        return False

    def _nearest_open_corner(self, ex, ey):
        best = None
        for r in range(0, 4):
            for nx in range(max(0, ex - r), min(self.n, ex + r) + 1):
                for ny in range(max(0, ey - r), min(self.n, ey + r) + 1):
                    if max(abs(nx - ex), abs(ny - ey)) != r:
                        continue
                    if not self._corner_on_wall(nx, ny):
                        score = abs(nx - ex) + abs(ny - ey)
                        if best is None or score < best[0]:
                            best = (score, nx, ny)
            if best is not None:
                return best[1], best[2]
        return ex, ey

    @staticmethod
    def _path_step_dir(a, b):
        dx = b[0] - a[0]
        dy = b[1] - a[1]
        return (0 if dx == 0 else (1 if dx > 0 else -1),
                0 if dy == 0 else (1 if dy > 0 else -1))

    def _bend_reason(self, prev_cell, cell, next_cell):
        gx, gy = cell
        if self._plan_normal_clear is not None and not self._plan_normal_clear[gy, gx]:
            return "tight_gateway"
        if self.floor[gy, gx] == self.FLOOR_SWAMP:
            return "swamp"
        unvisited = self.cell_state[gy, gx] == self.CELL_UNVISITED
        if unvisited and self._near_main_grid_line(gx, gy):
            return "grid_preference"
        wd_prev = self._wall_dist_at(*prev_cell)
        wd = self._wall_dist_at(gx, gy)
        wd_next = self._wall_dist_at(*next_cell)
        if unvisited and wd <= 2 and (wd < wd_prev or wd < wd_next):
            return "wall_follow"
        return "turn"

    def _geometric_bend_reason(self, before, corner, after):
        """Portable fallback; the installed native backend uses measured geometry."""
        _, cell = self._nearest_path_index_for_corner(self.plan_path, corner)
        gx, gy = cell
        if self.floor[gy, gx] == self.FLOOR_SWAMP:
            return "swamp"
        if all(abs((v-self.c)/2-round((v-self.c)/2)) < 1e-8 for v in corner):
            return "grid_preference"
        return "turn"

    def _wall_dist_at(self, gx, gy):
        if self._plan_wall_dist is None or not self.in_bounds(gx, gy):
            return 255
        return int(self._plan_wall_dist[gy, gx])

    @staticmethod
    def _path_reason_color(reason):
        return {
            "tight_gateway": (1.00, 0.30, 0.05),
            "swamp": (0.45, 0.22, 0.05),
            "wall_follow": (0.10, 0.45, 1.00),
            "grid_preference": (0.10, 0.75, 0.25),
            "turn": (0.95, 0.05, 0.85),
        }.get(reason, (0.95, 0.05, 0.85))

    # ------------------------------------------------------------------ #
    #  Robot pose
    # ------------------------------------------------------------------ #
    def _mark_current_robot(self, data, mapper):
        self._current_pose_for_planning = None
        self.current_pose_source = self.current_yaw_source = None
        pos = data.get("position", {})
        if not pos:
            return
        body_x,body_y = pos.get("map_body_x_float"),pos.get("map_body_y_float")
        body_ready = (pos.get("mapping_reference") == "gps_device_xz_mm"
                      and body_x is not None and body_y is not None
                      and math.isfinite(float(body_x)) and math.isfinite(float(body_y)))
        x = body_x if body_ready else pos.get("map_x",pos.get("x"))
        y_raw = body_y if body_ready else pos.get("map_y",pos.get("y"))
        if x is None or y_raw is None or not all(math.isfinite(float(v)) for v in (x,y_raw)):
            return
        y = mapper.GPS_Y_SIGN * y_raw
        rx = x - mapper.origin_x
        ry = y - mapper.origin_y
        nav_x = rx / self.NAV_GRID_MM + self.c - 0.5
        nav_y = ry / self.NAV_GRID_MM + self.c - 0.5
        gx, gy = self.world_to_nav(rx, ry)
        self.current_nav = (gx, gy)
        self.current_nav_xy = (float(nav_x), float(nav_y))
        self.current_world_mm = (float(rx), float(ry))
        self.current_pose_source = ("map_body_x_float/map_body_y_float" if body_ready
            else ("map_x" if "map_x" in pos else "x") + "/"
                 + ("map_y" if "map_y" in pos else "y"))
        orientation = data.get("orientation", {})
        raw_yaw = orientation.get("yaw_raw")
        self.current_yaw_deg = (float(raw_yaw) if body_ready and raw_yaw is not None
            and math.isfinite(float(raw_yaw)) else orientation.get("yaw"))
        self.current_yaw_source = ("yaw_raw" if body_ready and raw_yaw is not None
            and math.isfinite(float(raw_yaw)) else
            "yaw" if self.current_yaw_deg is not None else None)
        now=data.get("time")
        if (now is not None and self.current_yaw_deg is not None
                and math.isfinite(float(now)) and math.isfinite(float(self.current_yaw_deg))):
            self._current_pose_for_planning=(self.current_world_mm,self.current_yaw_deg,float(now))

    # ------------------------------------------------------------------ #
    #  Cropping
    # ------------------------------------------------------------------ #
    def _used_bbox(self):
        used = ((self.floor != self.FLOOR_NORMAL)
                | (self.cell_state != self.CELL_UNKNOWN)
                | self.current_beam)
        if self._planning_mapper is not None:
            mapper = self._planning_mapper
            y, x = np.nonzero(mapper.occupied_geometry())
            x = np.floor((x-mapper.c)*mapper.GRID_MM/self.NAV_GRID_MM).astype(int)+self.c
            y = np.floor((y-mapper.c)*mapper.GRID_MM/self.NAV_GRID_MM).astype(int)+self.c
            inside = (x >= 0) & (x < self.n) & (y >= 0) & (y < self.n)
            used[y[inside], x[inside]] = True
        sx, sy = self.start_nav
        used[sy, sx] = True

        ys, edge_xs = np.nonzero(self.wall_v)
        m = edge_xs > 0
        used[ys[m], edge_xs[m] - 1] = True
        m = edge_xs < self.n
        used[ys[m], edge_xs[m]] = True

        edge_ys, xs = np.nonzero(self.wall_h)
        m = edge_ys > 0
        used[edge_ys[m] - 1, xs[m]] = True
        m = edge_ys < self.n
        used[edge_ys[m], xs[m]] = True

        if self.current_nav_xy is not None:
            gx, gy = int(round(self.current_nav_xy[0])), int(round(self.current_nav_xy[1]))
            if self.in_bounds(gx, gy):
                used[gy, gx] = True
        if self.plan_destination is not None:
            gx, gy = self.plan_destination
            if self.in_bounds(gx, gy):
                used[gy, gx] = True
        if self.plan_destination_corner is not None:
            ex, ey = self.plan_destination_corner
            for gx in (ex - 1, ex):
                for gy in (ey - 1, ey):
                    if self.in_bounds(gx, gy):
                        used[gy, gx] = True
        for gx, gy in self.plan_path:
            if self.in_bounds(gx, gy):
                used[gy, gx] = True
        ys, xs = np.nonzero(used)
        if xs.size == 0:
            p = self.USED_PAD_CELLS
            return self.c - p, self.c + p + 1, self.c - p, self.c + p + 1
        p = self.USED_PAD_CELLS
        x0 = max(0, int(xs.min()) - p)
        x1 = min(self.n, int(xs.max()) + p + 1)
        y0 = max(0, int(ys.min()) - p)
        y1 = min(self.n, int(ys.max()) + p + 1)
        self._last_used_bbox = (x0, x1, y0, y1)
        return x0, x1, y0, y1

    # ------------------------------------------------------------------ #
    #  Debug export
    # ------------------------------------------------------------------ #
    def navigation_image(self, measured=None):
        x0, x1, y0, y1 = self._used_bbox()
        floor = self.floor[y0:y1, x0:x1]
        state = self.cell_state[y0:y1, x0:x1]
        beam = self.current_beam[y0:y1, x0:x1]
        image = np.repeat(np.repeat(self._compose_cell_image(floor, state, beam), 6, axis=0), 6, axis=1)
        if (self.show_measured_geometry if measured is None else measured):
            self._paint_measured_geometry(image, x0, y0, 6)
        return image

    def _paint_measured_geometry(self, image, x0, y0, scale):
        """Show the same occupied squares used by swept-body path clearance.

        Small wall/object fragments need not span a 30 mm edge. Keep their
        measured footprint visible instead of inventing a full coarse wall.
        """
        mapper = self._planning_mapper
        if mapper is None:
            return image
        x = (((np.arange(image.shape[1])+.5)/scale+x0-self.c)*self.NAV_GRID_MM)
        y = (((np.arange(image.shape[0])+.5)/scale+y0-self.c)*self.NAV_GRID_MM)
        x = np.floor(x/mapper.GRID_MM+.5).astype(int)+mapper.c
        y = np.floor(y/mapper.GRID_MM+.5).astype(int)+mapper.c
        inside = ((y[:, None] >= 0) & (y[:, None] < mapper.n)
                  & (x[None, :] >= 0) & (x[None, :] < mapper.n))
        occupied = mapper.occupied_geometry()[np.ix_(np.clip(y, 0, mapper.n-1), np.clip(x, 0, mapper.n-1))]
        image[occupied & inside] = (0.18, 0.18, 0.18)
        return image

    def _compose_cell_image(self, floor, state, beam=None):
        img = np.empty((floor.shape[0], floor.shape[1], 3), dtype=np.float32)
        for code, rgb in self.CELL_STATE_RGB.items():
            img[state == code] = rgb
        # Confirmed floor identity covers its full 120 mm tile independently
        # of camera visitation. This display evidence never certifies clearance.
        identity_only = (floor != self.FLOOR_NORMAL) & (state == self.CELL_UNKNOWN)
        img[identity_only] = self.CELL_STATE_RGB[self.CELL_UNVISITED]
        for code, rgb in self.FLOOR_RGB.items():
            if code == self.FLOOR_NORMAL:
                continue
            mask = floor == code
            if np.any(mask):
                img[mask] = 0.65 * np.asarray(rgb, dtype=np.float32) + 0.35 * img[mask]
        if beam is not None and np.any(beam):
            img[beam] = self.CURRENT_BEAM_RGB
        return img

    def debug_image(self, scale=6, measured=False):
        x0, x1, y0, y1 = self._used_bbox()
        rows = max(1, y1 - y0)
        cols = max(1, x1 - x0)
        img = np.ones((rows * scale + 1, cols * scale + 1, 3), dtype=np.float32)
        floor = self.floor[y0:y1, x0:x1]
        state = self.cell_state[y0:y1, x0:x1]
        beam = self.current_beam[y0:y1, x0:x1]
        cell_img = self._compose_cell_image(floor, state, beam)
        for r in range(rows):
            for c in range(cols):
                img[r * scale:(r + 1) * scale,
                    c * scale:(c + 1) * scale, :] = cell_img[r, c]

        img[::scale, :, :] = 0.75
        img[:, ::scale, :] = 0.75
        if measured:
            self._paint_measured_geometry(img, x0, y0, scale)

        v_edges = self.wall_v[y0:y1, x0:x1 + 1]
        rs, es = np.nonzero(v_edges)
        for r, e in zip(rs, es):
            img[r * scale:(r + 1) * scale + 1, e * scale, :] = 0.0

        h_edges = self.wall_h[y0:y1 + 1, x0:x1]
        es, cs = np.nonzero(h_edges)
        for e, c in zip(es, cs):
            img[e * scale, c * scale:(c + 1) * scale + 1, :] = 0.0

        sx, sy = self.start_nav
        sx = int(round((sx - x0) * scale))
        sy = int(round((sy - y0) * scale))
        if 0 <= sx < img.shape[1] and 0 <= sy < img.shape[0]:
            img[max(0, sy - 2):min(img.shape[0], sy + 3),
                max(0, sx - 2):min(img.shape[1], sx + 3)] = (0.0, 0.45, 1.0)

        if self.current_nav_xy is not None:
            cx = int(round((self.current_nav_xy[0] - x0 + 0.5) * scale))
            cy = int(round((self.current_nav_xy[1] - y0 + 0.5) * scale))
            if 0 <= cx < img.shape[1] and 0 <= cy < img.shape[0]:
                img[max(0, cy - 2):min(img.shape[0], cy + 3),
                    max(0, cx - 2):min(img.shape[1], cx + 3)] = (1.0, 0.0, 0.0)
        return img

    def save_debug(self, base, mapper, data):
        # DEBUG ONLY: no image/grid construction or disk work when disabled.
        if not mapper.DEBUG_MAPPING_DUMP:
            return []
        x0, x1, y0, y1 = self._used_bbox()
        wx0 = (x0 - self.c) * self.NAV_GRID_MM
        wx1 = (x1 - self.c) * self.NAV_GRID_MM
        wy0 = (y0 - self.c) * self.NAV_GRID_MM
        wy1 = (y1 - self.c) * self.NAV_GRID_MM

        hx0 = max(0, int(math.floor(wx0 / mapper.GRID_MM)) + mapper.c)
        hx1 = min(mapper.n, int(math.ceil(wx1 / mapper.GRID_MM)) + mapper.c)
        hy0 = max(0, int(math.floor(wy0 / mapper.GRID_MM)) + mapper.c)
        hy1 = min(mapper.n, int(math.ceil(wy1 / mapper.GRID_MM)) + mapper.c)

        occ_crop = mapper.logodds[hy0:hy1, hx0:hx1] >= mapper.OCC_T
        free_crop = mapper.logodds[hy0:hy1, hx0:hx1] <= mapper.FREE_T
        highres_rgb = np.full(occ_crop.shape + (3,), 76, dtype=np.uint8)
        highres_rgb[free_crop] = (255, 255, 255)
        highres_rgb[occ_crop] = (0, 0, 0)
        obstacle_mask = getattr(mapper, "_confirmed_obstacle_mask", None)
        if obstacle_mask is not None and obstacle_mask.shape == mapper.logodds.shape:
            obstacle_crop = obstacle_mask[hy0:hy1, hx0:hx1]
        else:
            obstacle_crop = np.zeros_like(occ_crop, dtype=bool)
        highres_rgb[obstacle_crop] = (242, 115, 12)
        if self._plan_clearance_deficit is not None:
            plan_deficits = [
                int(self._plan_clearance_deficit[gy, gx])
                for gx, gy in self.plan_path
                if self.in_bounds(gx, gy)
            ]
            clearance_crop = self._plan_clearance_deficit[y0:y1, x0:x1]
        else:
            plan_deficits = []
            clearance_crop = np.zeros((y1 - y0, x1 - x0), dtype=np.uint8)
        path_corners = self._path_corner_points()
        if self._plan_corner_clear is not None:
            corner_clearances = [
                int(self._plan_corner_clear[int(ey), int(ex)])
                for ex, ey in path_corners
                if int(ex) == ex and int(ey) == ey and 0 <= ex <= self.n and 0 <= ey <= self.n
            ]
            corner_clear_crop = self._plan_corner_clear[y0:y1 + 1, x0:x1 + 1]
        else:
            corner_clearances = []
            corner_clear_crop = np.zeros((y1 - y0 + 1, x1 - x0 + 1), dtype=np.uint8)

        meta = {
            "time": round(float(data.get("time", 0.0)), 3),
            "nav_grid_mm": self.NAV_GRID_MM,
            "nav_shape": [int(self.n), int(self.n)],
            "nav_center_edge_index": int(self.c),
            "nav_bbox": [int(x0), int(x1), int(y0), int(y1)],
            "nav_bbox_world_mm": [float(wx0), float(wx1), float(wy0), float(wy1)],
            "highres_grid_mm": int(mapper.GRID_MM),
            "highres_shape": [int(mapper.n), int(mapper.n)],
            "highres_center_index": int(mapper.c),
            "highres_bbox": [int(hx0), int(hx1), int(hy0), int(hy1)],
            "origin_mm": [mapper.origin_x, mapper.origin_y],
            "pose_source": self.current_pose_source,
            "yaw_source": self.current_yaw_source,
            "current_nav": list(self.current_nav) if self.current_nav is not None else None,
            "current_nav_xy": list(self.current_nav_xy) if self.current_nav_xy is not None else None,
            "current_yaw_deg": self.current_yaw_deg,
            "wall_model": "zero-thickness edges",
            "wall_alias_model": "straight_highres_with_curve_only_stairs",
            "wall_source": self.last_wall_source,
            "measured_geometry_overlay": "separate _nav_measured.png; M toggles live overlay",
            "edge_band_cells": int(self.EDGE_BAND_CELLS),
            "edge_segment_cells": int(self.EDGE_SEG_CELLS),
            "edge_min_span_cells": int(self.EDGE_MIN_SPAN_CELLS),
            "curve_stair_radius_cells": int(self.CURVE_STAIR_RADIUS_CELLS),
            "curve_stair_min_arc_hits": int(OccupancyMapper.ARC_SOFT_HITS),
            "curve_stair_bends": int(self.last_curve_stairs),
            "curve_stair_skipped": int(self.last_curve_stairs_skipped),
            "wall_v_edges": int(np.count_nonzero(self.wall_v)),
            "wall_h_edges": int(np.count_nonzero(self.wall_h)),
            "floor_counts": self.floor_counts(),
            "floor_code_to_char": {str(k): v for k, v in self.FLOOR_CODE_TO_CHAR.items()},
            "floor_identity_known_cells": int(np.count_nonzero(self.floor != self.FLOOR_NORMAL)),
            "cell_state_semantics": "observation/visitation; floor identity alone does not certify free space",
            "floor_identity_display": "confirmed full tile identity uses known/unvisited base where observation is unknown",
            "cell_state_counts": self.cell_state_counts(),
            "cell_state_to_char": {str(k): v for k, v in self.CELL_STATE_TO_CHAR.items()},
            "current_beam_cells": int(self.last_current_beam_cells),
            "wall_adjacent_discovered_cells": int(self.last_wall_adjacent_discovered_cells),
            "plan_status": self.plan_status,
            "inspect_walls": self.PATH_INSPECT_WALLS,
            "path_resolution_mm": self.PATH_FINE_GRID_MM if self.plan_raw_points else self.NAV_GRID_MM,
            "plan_cost": self.plan_cost,
            "plan_destination": list(self.plan_destination) if self.plan_destination else None,
            "plan_destination_corner": (list(self.plan_destination_corner)
                                        if self.plan_destination_corner else None),
            "plan_requested_corner": (list(self.plan_requested_corner)
                                      if self.plan_requested_corner else None),
            "plan_path_cells": int(len(self.plan_path)),
            "plan_path_signature": self._points_checksum(self.plan_path),
            "plan_corner_path_signature": self._points_checksum(path_corners),
            "plan_clearance_deficit_cells": int(sum(v > 0 for v in plan_deficits)),
            "plan_clearance_deficit_max": int(max(plan_deficits) if plan_deficits else 0),
            "plan_clearance_target_cells": int(self.PATH_CLEARANCE_TARGET_CELLS),
            "plan_corner_clearance_min": int(min(corner_clearances) if corner_clearances else 0),
            "plan_corner_clearance_below_target": int(sum(
                v < self.PATH_CLEARANCE_TARGET_CELLS for v in corner_clearances)),
            "plan_waypoints": [list(p) for p in self.plan_waypoints],
            "plan_path_corners": [list(p) for p in path_corners],
            "plan_annotations": [
                {"cell": list(a["cell"]),
                 "corner": list(a.get("corner", self._path_cell_corner_point(*a["cell"]))),
                 "reason": a["reason"]}
                for a in self.plan_annotations
            ],
            "highres_occ_cells_in_crop": int(np.count_nonzero(occ_crop)),
            "highres_free_cells_in_crop": int(np.count_nonzero(free_crop)),
        }

        npz_path = base + "_nav_debug.npz"
        json_path = base + "_nav_debug.json"
        png_path = base + "_nav_debug.png"
        np.savez_compressed(
            npz_path,
            meta_json=np.array(json.dumps(meta)),
            nav_floor=self.floor[y0:y1, x0:x1],
            nav_floor_chars=np.array([
                [self.FLOOR_CODE_TO_CHAR.get(int(code), "?") for code in row]
                for row in self.floor[y0:y1, x0:x1]
            ], dtype="<U1"),
            nav_floor_identity_known=(self.floor[y0:y1, x0:x1] != self.FLOOR_NORMAL),
            nav_cell_state=self.cell_state[y0:y1, x0:x1],
            nav_cell_state_chars=np.array([
                [self.CELL_STATE_TO_CHAR.get(int(code), "?") for code in row]
                for row in self.cell_state[y0:y1, x0:x1]
            ], dtype="<U1"),
            nav_current_beam=self.current_beam[y0:y1, x0:x1],
            nav_wall_adjacent_discovered=self.wall_adjacent_discovered[y0:y1, x0:x1],
            nav_plan_path=np.array(self.plan_path, dtype=np.int16),
            nav_plan_path_corners=np.array(path_corners, dtype=np.float64),
            nav_plan_waypoints=np.array(self.plan_waypoints, dtype=np.int16),
            nav_plan_annotation_reasons=np.array(
                [a["reason"] for a in self.plan_annotations], dtype="<U16"),
            nav_clearance_deficit=clearance_crop,
            nav_corner_clearance=corner_clear_crop,
            nav_wall_v=self.wall_v[y0:y1, x0:x1 + 1],
            nav_wall_h=self.wall_h[y0:y1 + 1, x0:x1],
            nav_wall_v_counts=self.wall_v_counts[y0:y1, x0:x1 + 1],
            nav_wall_h_counts=self.wall_h_counts[y0:y1 + 1, x0:x1],
            highres_logodds=mapper.logodds[hy0:hy1, hx0:hx1],
            highres_occ=occ_crop,
            highres_free=free_crop,
            highres_obstacle=obstacle_crop,
            highres_rgb=highres_rgb,
        )
        with open(json_path, "w", encoding="utf-8") as fp:
            json.dump(meta, fp, indent=2)
        plt.imsave(png_path, np.clip(self.debug_image(), 0.0, 1.0), origin="lower")
        measured_path = base + "_nav_measured.png"
        plt.imsave(measured_path, np.clip(self.debug_image(measured=True), 0.0, 1.0), origin="lower")
        return [npz_path, json_path, png_path, measured_path]

    # ------------------------------------------------------------------ #
    #  Live visualization
    # ------------------------------------------------------------------ #
    def draw_navigation_map(self):
        image = self.navigation_image()
        x0, x1, y0, y1 = self._last_used_bbox
        width, height = x1-x0, y1-y0
        if self.fig_nav is None:
            h, w = image.shape[:2]
            fig_w = min(max(w * self.MAP_PX_PER_CELL, 420), self.MAP_FIG_MAX_W_PX) / self.MAP_FIG_DPI
            fig_h = min(max(h * self.MAP_PX_PER_CELL, 320), self.MAP_FIG_MAX_H_PX) / self.MAP_FIG_DPI
            self.fig_nav, self.ax_nav = plt.subplots(figsize=(fig_w, fig_h), dpi=self.MAP_FIG_DPI)
            self.fig_nav.subplots_adjust(left=0.02, right=0.98, top=0.98, bottom=0.02)
            self.fig_nav.canvas.manager.set_window_title("30 mm Navigation Map")
            self.nav_click_cid = self.fig_nav.canvas.mpl_connect(
                "button_press_event", self._on_nav_click)
            self.fig_nav.canvas.mpl_connect("key_press_event", self._on_nav_key)
            self.img_nav = self.ax_nav.imshow(
                image, origin="lower", interpolation="nearest",
                extent=(-0.5, width - 0.5, -0.5, height - 0.5))
        else:
            self.img_nav.set_data(image)
            self.img_nav.set_extent((-0.5, width - 0.5, -0.5, height - 0.5))
        self.ax_nav.set_xlim(-0.5, width - 0.5)
        self.ax_nav.set_ylim(-0.5, height - 0.5)
        self._style_navigation_grid(width, height)
        self._draw_wall_edges()
        self._draw_path_overlay()
        self._draw_pose_overlay()
        plt.pause(0.001)

    def _on_nav_key(self, event):
        if event.key == "m":
            self.show_measured_geometry = not self.show_measured_geometry
            self.draw_navigation_map()

    def _on_nav_click(self, event):
        if event.inaxes != self.ax_nav or event.xdata is None or event.ydata is None:
            return
        if getattr(event, "button", 1) != 1:
            return
        if self._last_used_bbox is None:
            return
        x0, x1, y0, y1 = self._last_used_bbox
        gx = x0 + int(math.floor(event.xdata + 0.5))
        gy = y0 + int(math.floor(event.ydata + 0.5))
        if not (x0 <= gx < x1 and y0 <= gy < y1 and self.in_bounds(gx, gy)):
            return
        corner = (
            max(0, min(self.n, int(math.floor(x0 + event.xdata + 1.0)))),
            max(0, min(self.n, int(math.floor(y0 + event.ydata + 1.0)))),
        )
        self.set_plan_destination_corner(corner, preferred=(gx, gy))
        self.draw_navigation_map()

    def _style_navigation_grid(self, w, h):
        self.ax_nav.set_xticks(np.arange(-0.5, w, 1.0), minor=True)
        self.ax_nav.set_yticks(np.arange(-0.5, h, 1.0), minor=True)
        self.ax_nav.grid(which="minor", color=(0.70, 0.70, 0.70), linewidth=0.25)
        self.ax_nav.tick_params(which="both", bottom=False, left=False,
                                labelbottom=False, labelleft=False)

    def _draw_wall_edges(self):
        if self._last_used_bbox is None:
            return
        x0, x1, y0, y1 = self._last_used_bbox
        segments = []

        v_edges = self.wall_v[y0:y1, x0:x1 + 1]
        rows, edges = np.nonzero(v_edges)
        for row, edge in zip(rows, edges):
            x = edge - 0.5
            segments.append(((x, row - 0.5), (x, row + 0.5)))

        h_edges = self.wall_h[y0:y1 + 1, x0:x1]
        edges, cols = np.nonzero(h_edges)
        for edge, col in zip(edges, cols):
            y = edge - 0.5
            segments.append(((col - 0.5, y), (col + 0.5, y)))

        if self.wall_collection is not None:
            self.wall_collection.remove()
            self.wall_collection = None
        self.wall_collection = LineCollection(
            segments, colors=[(0.0, 0.0, 0.0)], linewidths=1.8, zorder=4)
        self.ax_nav.add_collection(self.wall_collection)

    def _draw_pose_overlay(self):
        if self._last_used_bbox is None:
            return
        x0, _, y0, _ = self._last_used_bbox

        sx, sy = self.start_nav
        sx = sx - x0 - 0.5
        sy = sy - y0 - 0.5
        if self.start_dot is None:
            self.start_dot, = self.ax_nav.plot([sx], [sy], marker="o", markersize=4,
                                               color=(0.0, 0.45, 1.0), linestyle="None",
                                               zorder=5)
        else:
            self.start_dot.set_data([sx], [sy])

        if self.current_nav_xy is None:
            return
        cx = self.current_nav_xy[0] - x0
        cy = self.current_nav_xy[1] - y0
        radius_cells = self.ROBOT_RADIUS_MM / self.NAV_GRID_MM
        if self.robot_dot is None:
            self.robot_dot, = self.ax_nav.plot([cx], [cy], marker="o", markersize=3.5,
                                               color=(1.0, 0.0, 0.0), linestyle="None",
                                               zorder=8)
        else:
            self.robot_dot.set_data([cx], [cy])

        if self.robot_circle is None:
            self.robot_circle = plt.Circle((cx, cy), radius_cells, fill=False,
                                           edgecolor=(1.0, 0.0, 0.0),
                                           linewidth=0.8, zorder=7)
            self.ax_nav.add_patch(self.robot_circle)
        else:
            self.robot_circle.center = (cx, cy)
            self.robot_circle.radius = radius_cells

        if self.robot_arrow is not None:
            self.robot_arrow.remove()
            self.robot_arrow = None
        if self.current_yaw_deg is not None:
            bang = math.radians(OccupancyMapper.YAW_OFFSET_DEG +
                                OccupancyMapper.YAW_SIGN * self.current_yaw_deg)
            length = radius_cells * 1.55
            dx = math.sin(bang) * length
            dy = math.cos(bang) * length
            self.robot_arrow = self.ax_nav.annotate(
                "", xy=(cx + dx, cy + dy), xytext=(cx, cy),
                arrowprops={
                    "arrowstyle": "->",
                    "color": (1.0, 0.0, 0.0),
                    "linewidth": 0.8,
                    "shrinkA": 0,
                    "shrinkB": 0,
                    "mutation_scale": 7,
                },
                zorder=9,
            )

    def _draw_path_overlay(self):
        if self._last_used_bbox is None:
            return
        x0, _, y0, _ = self._last_used_bbox

        if self.path_line is not None:
            self.path_line.remove()
            self.path_line = None
        if self.destination_dot is not None:
            self.destination_dot.remove()
            self.destination_dot = None
        for artist in self.gateway_dots:
            artist.remove()
        self.gateway_dots = []

        if self.plan_destination is not None:
            if self.plan_destination_corner is not None:
                gx = self.plan_destination_corner[0] - x0 - 0.5
                gy = self.plan_destination_corner[1] - y0 - 0.5
            else:
                gx, gy = self.plan_destination
                gx = gx - x0
                gy = gy - y0
            self.destination_dot, = self.ax_nav.plot(
                [gx], [gy], marker="*", markersize=8,
                color=(0.05, 0.05, 0.05), linestyle="None", zorder=11)

        if len(self._path_corner_points()) >= 2:
            xs, ys = self._path_plot_points(x0, y0)
            self.path_line, = self.ax_nav.plot(
                xs, ys, color=self.PATH_LINE_RGB, linewidth=1.8,
                alpha=0.85, zorder=10)

        for ann in self.plan_annotations:
            gx, gy = ann["cell"]
            corner = ann.get("corner")
            if corner is None:
                px, py = self._path_cell_plot_point(gx, gy, x0, y0)
            else:
                px = corner[0] - x0 - 0.5
                py = corner[1] - y0 - 0.5
            color = self._path_reason_color(ann["reason"])
            dot, = self.ax_nav.plot(
                [px], [py], marker="o", markersize=5.5,
                color=color, markeredgecolor=(0.0, 0.0, 0.0),
                markeredgewidth=0.35, linestyle="None", zorder=12)
            self.gateway_dots.append(dot)

    def _path_plot_points(self, x0, y0):
        pts = self._path_corner_points()
        return ([ex - x0 - 0.5 for ex, _ in pts],
                [ey - y0 - 0.5 for _, ey in pts])

    def _path_cell_plot_point(self, gx, gy, x0, y0):
        ex, ey = self._path_cell_corner_point(gx, gy)
        return ex - x0 - 0.5, ey - y0 - 0.5

    def _path_corner_points(self):
        if self._plan_display_points is not None:
            return list(self._plan_display_points)
        if self.async_planning:
            # Readers never run decorative geometry/smoothing on the UI thread.
            return (list(self.plan_raw_points) if self.plan_raw_points else
                    list(self.plan_corner_path) if self.plan_corner_path else
                    [(x+.5,y+.5) for x,y in self.plan_path])
        if self.plan_raw_points:
            return list(self.plan_raw_points)
        if self.plan_corner_path:
            pts = list(self.plan_corner_path)
            if self.plan_destination_corner is not None:
                dest = tuple(self.plan_destination_corner)
                if (pts and pts[-1] != dest and self._corner_clear_enough(dest)
                        and self._corner_segment_clear(pts[-1], dest)
                        and self._path_turns_clear(pts+[dest])):
                    pts.append(dest)
            return pts
        pts = self._path_corner_points_for(self.plan_path)
        if self.plan_destination_corner is not None:
            dest = tuple(self.plan_destination_corner)
            if pts and self.plan_path and self._cell_touches_corner(self.plan_path[-1], dest):
                previous = pts[-2] if len(pts) > 1 else pts[-1]
                if (self._physical_segment_clear(previous, dest) and self._corner_clear_enough(dest)
                        and self._path_turns_clear(pts[:-1]+[dest])):
                    pts[-1] = dest
            elif (pts and pts[-1] != dest and self._corner_segment_clear(pts[-1], dest)
                    and self._path_turns_clear(pts+[dest])):
                pts.append(dest)
        return pts

    def _corner_clear_enough(self, corner):
        ex, ey = corner
        if self._plan_corner_clear is None:
            return True
        if not (0 <= ex <= self.n and 0 <= ey <= self.n):
            return False
        return self._plan_corner_clear[ey, ex] >= self.PATH_CLEARANCE_TARGET_CELLS

    @staticmethod
    def _cell_touches_corner(cell, corner):
        gx, gy = cell
        ex, ey = corner
        return gx <= ex <= gx + 1 and gy <= ey <= gy + 1

    def _path_corner_points_for(self, path, smooth=True):
        if not path:
            return []
        pts = [self._path_cell_corner_point(*path[0])]
        for prev_cell, cell in zip(path, path[1:]):
            pts.append(self._path_transition_corner(prev_cell, cell, pts[-1]))
        if smooth:
            pts = self._smooth_path_corners(pts)
        if (self._planning_mapper is not None
                and (not self._path_turns_clear(pts)
                     or any(not self._physical_segment_clear(a, b)
                        for a, b in zip(pts, pts[1:])))):
            # The fallback graph validates cell-centre segments. A decorative
            # shift to cell corners must not draw a different, colliding route.
            pts = [(x+0.5, y+0.5) for x, y in path]
        return pts

    def _smooth_path_corners(self, pts):
        original = list(pts)
        clean = []
        for pt in pts:
            if not clean or clean[-1] != pt:
                clean.append(pt)
        if len(clean) < 3:
            return clean

        changed = True
        while changed:
            self._check_plan_cancelled()
            changed = False
            out = [clean[0]]
            i = 1
            while i < len(clean) - 1:
                a = out[-1]
                b = clean[i]
                c = clean[i + 1]
                if (self._can_replace_corner_with_diagonal(a, b, c)
                        or self._can_replace_corner_with_straight(a, b, c)):
                    changed = True
                    i += 1
                    continue
                out.append(b)
                i += 1
            out.append(clean[-1])
            clean = out
        return clean if self._path_turns_clear(clean) else original

    def _shortcut_visited_corner_path(self, pts, physical_legs_proven=False):
        if len(pts) < 4:
            return pts

        # A monotone straight run has one exact minimum-cost shortcut: its
        # endpoints. Prove that whole segment once instead of rechecking every
        # pair of redundant collinear vertices (cubic work on long corridors).
        direction = np.asarray(pts[-1],dtype=float)-pts[0]
        steps = np.diff(np.asarray(pts,dtype=float),axis=0)
        collinear = (np.all(abs(steps[:,0]*direction[1]-steps[:,1]*direction[0]) < 1e-10)
                     and np.all(steps@direction >= 0))
        if (collinear and np.any(direction) and self._visited_corner_segment_clear(pts[0],pts[-1])
                and self._path_turns_clear([pts[0],pts[-1]])):
            return [pts[0],pts[-1]]
        n = len(pts)
        # For one monotone straight run, its swept capsule is the union of
        # consecutive leg capsules. Unit-leg floor/wall gates cover exactly
        # the same interval too. Reuse that proof through a prefix sum.
        ends = np.arange(n,dtype=int)
        clear = np.zeros(n-1,dtype=bool)
        for i in range(n-2,-1,-1):
            self._check_plan_cancelled()
            clear[i] = self._visited_corner_segment_clear(
                pts[i],pts[i+1],physical_proven=physical_legs_proven)
            ends[i] = i+1
            if i+2 < n:
                a = np.asarray(pts[i+1])-pts[i]
                b = np.asarray(pts[i+2])-pts[i+1]
                if abs(a[0]*b[1]-a[1]*b[0]) < 1e-10 and np.dot(a,b) > 0:
                    ends[i] = ends[i+1]
        bad = np.r_[0,np.cumsum(~clear)]
        dir_to_idx = {d: i for i, d in enumerate(self.PATH_DIR_VECS)}
        start_state = 8
        costs = [dict() for _ in range(n)]
        prev = {}
        costs[0][start_state] = 0.0
        for i in range(n - 1):
            self._check_plan_cancelled()
            # Keep the same candidate and DP orders without retaining every
            # source vertex's options at once. Geometry is immutable here.
            edge_options = []
            for j in range(i + 1, n):
                self._check_plan_cancelled()
                if (j == i+1 or (j <= ends[i] and bad[j] == bad[i])
                        or self._visited_corner_segment_clear(pts[i],pts[j])):
                    direction = self._path_step_dir(pts[i], pts[j])
                    length = math.hypot(pts[j][0] - pts[i][0], pts[j][1] - pts[i][1])
                    # Tiny bias toward longer legal shortcuts: on visited floor,
                    # fewer segments means fewer real rotations without changing
                    # the underlying explored/unexplored planning rule.
                    length -= 0.03 * float(j - i - 1)
                    edge_options.append((j, direction, length))

            if not costs[i]:
                continue
            for prev_dir, base_cost in costs[i].items():
                self._check_plan_cancelled()
                for option_start in range(0, len(edge_options), 512):
                    self._check_plan_cancelled()
                    for j, direction, length in edge_options[option_start:option_start + 512]:
                        dir_idx = dir_to_idx.get(direction)
                        if dir_idx is None:
                            continue
                        turn_cost = 0.0 if prev_dir == start_state else float(
                            self._turn_cost_between_dirs(prev_dir, dir_idx))
                        new_cost = base_cost + length + turn_cost
                        if new_cost < costs[j].get(dir_idx, float("inf")):
                            costs[j][dir_idx] = new_cost
                            prev[(j, dir_idx)] = (i, prev_dir)

        if not costs[-1]:
            return pts
        end_dir = min(costs[-1], key=lambda d: costs[-1][d])
        path = []
        state = (n - 1, end_dir)
        while state[0] != 0:
            path.append(pts[state[0]])
            state = prev[state]
        path.append(pts[0])
        path.reverse()
        return path if len(path) < len(pts) and self._path_turns_clear(path) else pts

    def _turn_cost_between_dirs(self, prev_dir, new_dir):
        pv = self.PATH_DIR_VECS[int(prev_dir)]
        nv = self.PATH_DIR_VECS[int(new_dir)]
        plen = math.hypot(pv[0], pv[1])
        nlen = math.hypot(nv[0], nv[1])
        dot = max(-1.0, min(1.0, (pv[0] * nv[0] + pv[1] * nv[1]) / (plen * nlen)))
        angle = math.acos(dot)
        cost = self.PATH_TURN_COST * (angle / (math.pi / 2.0))
        if angle >= math.pi / 2.0 - 1e-6:
            cost += self.PATH_RIGHT_TURN_EXTRA_COST * (angle / (math.pi / 2.0))
        return cost

    def _visited_corner_segment_clear(self, a, b, physical_proven=False):
        if not self._corner_segment_clear(a, b, physical_proven=True):
            return False
        dx = b[0] - a[0]
        dy = b[1] - a[1]
        if dx == 0 and dy == 0:
            return False
        if dx == 0:
            clear = self._visited_vertical_corner_segment_clear(a, b)
        elif dy == 0:
            clear = self._visited_horizontal_corner_segment_clear(a, b)
        elif abs(dx) == abs(dy):
            clear = self._visited_diagonal_corner_segment_clear(a, b)
        else:
            return False
        return clear and (physical_proven or self._physical_segment_clear(a, b))

    def _visited_horizontal_corner_segment_clear(self, a, b):
        ey = a[1]
        sx = 1 if b[0] > a[0] else -1
        for ex in range(a[0], b[0], sx):
            gx = min(ex, ex + sx)
            if not (0 <= gx < self.n and 0 <= ey <= self.n):
                return False
            if self.wall_h[ey, gx]:
                return False
            if self._plan_h_lane_clear is not None:
                if self._plan_h_lane_clear[ey, gx] < self.PATH_CLEARANCE_TARGET_CELLS:
                    return False
            if not self._visited_lane_has_floor(((gx, ey - 1), (gx, ey))):
                return False
        return True

    def _visited_vertical_corner_segment_clear(self, a, b):
        ex = a[0]
        sy = 1 if b[1] > a[1] else -1
        for ey in range(a[1], b[1], sy):
            gy = min(ey, ey + sy)
            if not (0 <= gy < self.n and 0 <= ex <= self.n):
                return False
            if self.wall_v[gy, ex]:
                return False
            if self._plan_v_lane_clear is not None:
                if self._plan_v_lane_clear[gy, ex] < self.PATH_CLEARANCE_TARGET_CELLS:
                    return False
            if not self._visited_lane_has_floor(((ex - 1, gy), (ex, gy))):
                return False
        return True

    def _visited_diagonal_corner_segment_clear(self, a, b):
        sx = 1 if b[0] > a[0] else -1
        sy = 1 if b[1] > a[1] else -1
        steps = abs(b[0] - a[0])
        for k in range(steps):
            ex = a[0] + k * sx
            ey = a[1] + k * sy
            gx = min(ex, ex + sx)
            gy = min(ey, ey + sy)
            if not self.in_bounds(gx, gy):
                return False
            if self.cell_state[gy, gx] != self.CELL_VISITED:
                return False
            if self.floor[gy, gx] == self.FLOOR_HOLE:
                return False
            if self._plan_normal_clear is not None and not self._plan_normal_clear[gy, gx]:
                return False
            if self._plan_corner_clear is not None:
                if (self._plan_corner_clear[ey, ex] < self.PATH_CLEARANCE_TARGET_CELLS
                        or self._plan_corner_clear[ey + sy, ex + sx] <
                        self.PATH_CLEARANCE_TARGET_CELLS):
                    return False
        return True

    def _visited_lane_has_floor(self, cells):
        seen = False
        for gx, gy in cells:
            if not self.in_bounds(gx, gy):
                continue
            if self.floor[gy, gx] == self.FLOOR_HOLE:
                return False
            if self.cell_state[gy, gx] == self.CELL_VISITED:
                seen = True
            elif self.cell_state[gy, gx] == self.CELL_UNKNOWN:
                return False
        return seen

    def _can_replace_corner_with_diagonal(self, a, b, c):
        ab = (b[0] - a[0], b[1] - a[1])
        bc = (c[0] - b[0], c[1] - b[1])
        ac = (c[0] - a[0], c[1] - a[1])
        if max(abs(ab[0]), abs(ab[1])) != 1:
            return False
        if max(abs(bc[0]), abs(bc[1])) != 1:
            return False
        if abs(ac[0]) != 1 or abs(ac[1]) != 1:
            return False
        if ab[0] and bc[0]:
            return False
        if ab[1] and bc[1]:
            return False
        return self._corner_diagonal_cell_clear(a, c)

    def _can_replace_corner_with_straight(self, a, b, c):
        if a[0] == c[0]:
            if abs(c[1] - a[1]) != 2 or abs(b[0] - a[0]) != 1:
                return False
            return self._corner_straight_clear(a, c)
        if a[1] == c[1]:
            if abs(c[0] - a[0]) != 2 or abs(b[1] - a[1]) != 1:
                return False
            return self._corner_straight_clear(a, c)
        return False

    def _corner_straight_clear(self, a, c):
        return (a[0] == c[0] or a[1] == c[1]) and self._corner_segment_clear(a, c)

    def _corner_diagonal_cell_clear(self, a, c):
        return self._corner_segment_clear(a, c)

    def _path_transition_corner(self, prev_cell, cell, current_corner):
        ax, ay = prev_cell
        bx, by = cell
        dx = bx - ax
        dy = by - ay
        if dx and dy:
            return max(ax, bx), max(ay, by)
        if dx:
            ex = max(ax, bx) if dx > 0 else min(ax, bx) + 1
            ey = self._best_horizontal_lane_y(ay, ax, bx, current_corner[1])
            return max(0, min(self.n, ex)), max(0, min(self.n, ey))
        if dy:
            ey = max(ay, by) if dy > 0 else min(ay, by) + 1
            ex = self._best_vertical_lane_x(ax, ay, by, current_corner[0])
            return max(0, min(self.n, ex)), max(0, min(self.n, ey))
        return current_corner

    def _best_vertical_lane_x(self, gx, gy_a, gy_b, current_x):
        gy0 = min(gy_a, gy_b)
        gy1 = max(gy_a, gy_b)
        candidates = (gx, gx + 1)
        clearances = {ex: self._vertical_lane_clearance(ex, gy0, gy1)
                      for ex in candidates}
        if current_x in candidates and clearances[current_x] >= 2:
            return current_x
        scores = [(self._vertical_lane_score(ex, gy0, gy1, current_x), ex)
                  for ex in candidates]
        best_score, best = max(scores)
        if current_x in candidates and clearances[current_x] < 2 and clearances[best] >= 2:
            return best
        for score, ex in scores:
            if ex == current_x and score > -1e8 and best_score <= score + 1.5:
                return ex
        return best

    def _best_horizontal_lane_y(self, gy, gx_a, gx_b, current_y):
        gx0 = min(gx_a, gx_b)
        gx1 = max(gx_a, gx_b)
        candidates = (gy, gy + 1)
        clearances = {ey: self._horizontal_lane_clearance(ey, gx0, gx1)
                      for ey in candidates}
        if current_y in candidates and clearances[current_y] >= 2:
            return current_y
        scores = [(self._horizontal_lane_score(ey, gx0, gx1, current_y), ey)
                  for ey in candidates]
        best_score, best = max(scores)
        if current_y in candidates and clearances[current_y] < 2 and clearances[best] >= 2:
            return best
        for score, ey in scores:
            if ey == current_y and score > -1e8 and best_score <= score + 1.5:
                return ey
        return best

    def _vertical_lane_clearance(self, ex, gy0, gy1):
        if not (0 <= ex <= self.n):
            return -1
        if self._plan_v_lane_clear is not None:
            lo = max(0, gy0)
            hi = min(self.n - 1, gy1)
            if lo > hi:
                return -1
            return int(np.min(self._plan_v_lane_clear[lo:hi + 1, ex]))
        best = 255
        for gy in range(max(0, gy0), min(self.n - 1, gy1) + 1):
            if self.wall_v[gy, ex]:
                return 0
            left = self._nearest_vertical_wall_edge(gy, ex, -1)
            right = self._nearest_vertical_wall_edge(gy, ex, 1)
            best = min(best,
                       ex - left if left is not None else 255,
                       right - ex if right is not None else 255,
                       self._lane_adjacent_floor_clearance(ex, gy, vertical=True))
        return best

    def _horizontal_lane_clearance(self, ey, gx0, gx1):
        if not (0 <= ey <= self.n):
            return -1
        if self._plan_h_lane_clear is not None:
            lo = max(0, gx0)
            hi = min(self.n - 1, gx1)
            if lo > hi:
                return -1
            return int(np.min(self._plan_h_lane_clear[ey, lo:hi + 1]))
        best = 255
        for gx in range(max(0, gx0), min(self.n - 1, gx1) + 1):
            if self.wall_h[ey, gx]:
                return 0
            low = self._nearest_horizontal_wall_edge(gx, ey, -1)
            high = self._nearest_horizontal_wall_edge(gx, ey, 1)
            best = min(best,
                       ey - low if low is not None else 255,
                       high - ey if high is not None else 255,
                       self._lane_adjacent_floor_clearance(gx, ey, vertical=False))
        return best

    def _vertical_lane_score(self, ex, gy0, gy1, current_x):
        if not (0 <= ex <= self.n):
            return -1e9
        if self._plan_v_lane_clear is not None:
            lo = max(0, gy0)
            hi = min(self.n - 1, gy1)
            if lo > hi:
                return -1e9
            clear = self._plan_v_lane_clear[lo:hi + 1, ex].astype(np.float32)
            score = float(np.sum(np.minimum(clear, 6.0)))
            score -= 0.05 * abs(ex - current_x)
            if (ex - self.c) % 4 in (1, 2):
                score += 0.35
            elif (ex - self.c) % 2 == 0:
                score += 0.12
            return score
        score = 0.0
        for gy in range(max(0, gy0), min(self.n - 1, gy1) + 1):
            if self.wall_v[gy, ex]:
                return -1e9
            left = self._nearest_vertical_wall_edge(gy, ex, -1)
            right = self._nearest_vertical_wall_edge(gy, ex, 1)
            clearance = min(ex - left if left is not None else 255,
                            right - ex if right is not None else 255,
                            self._lane_adjacent_floor_clearance(ex, gy, vertical=True))
            score += min(clearance, 6)
        score -= 0.05 * abs(ex - current_x)
        return score

    def _horizontal_lane_score(self, ey, gx0, gx1, current_y):
        if not (0 <= ey <= self.n):
            return -1e9
        if self._plan_h_lane_clear is not None:
            lo = max(0, gx0)
            hi = min(self.n - 1, gx1)
            if lo > hi:
                return -1e9
            clear = self._plan_h_lane_clear[ey, lo:hi + 1].astype(np.float32)
            score = float(np.sum(np.minimum(clear, 6.0)))
            score -= 0.05 * abs(ey - current_y)
            if (ey - self.c) % 4 in (1, 2):
                score += 0.35
            elif (ey - self.c) % 2 == 0:
                score += 0.12
            return score
        score = 0.0
        for gx in range(max(0, gx0), min(self.n - 1, gx1) + 1):
            if self.wall_h[ey, gx]:
                return -1e9
            low = self._nearest_horizontal_wall_edge(gx, ey, -1)
            high = self._nearest_horizontal_wall_edge(gx, ey, 1)
            clearance = min(ey - low if low is not None else 255,
                            high - ey if high is not None else 255,
                            self._lane_adjacent_floor_clearance(gx, ey, vertical=False))
            score += min(clearance, 6)
        score -= 0.05 * abs(ey - current_y)
        return score

    def _nearest_vertical_wall_edge(self, gy, ex, step):
        e = ex
        while 0 <= e <= self.n:
            if self.wall_v[gy, e]:
                return e
            e += step
        return None

    def _nearest_horizontal_wall_edge(self, gx, ey, step):
        e = ey
        while 0 <= e <= self.n:
            if self.wall_h[e, gx]:
                return e
            e += step
        return None

    def _lane_adjacent_floor_clearance(self, a, b, vertical):
        cells = []
        if vertical:
            ex, gy = a, b
            cells = ((ex - 1, gy), (ex, gy))
        else:
            gx, ey = a, b
            cells = ((gx, ey - 1), (gx, ey))
        best = 255
        for gx, gy in cells:
            if not self.in_bounds(gx, gy):
                continue
            if self.floor[gy, gx] == self.FLOOR_HOLE:
                return 0
            if self._plan_hole_dist is not None:
                best = min(best, int(self._plan_hole_dist[gy, gx]))
        return min(best, 6)

    def _path_cell_corner_point(self, gx, gy):
        if self.in_bounds(gx, gy) and self.cell_state[gy, gx] == self.CELL_VISITED:
            return self._nearest_30_edge_index(gx), self._nearest_30_edge_index(gy)
        return self._nearest_path_edge_index(gx), self._nearest_path_edge_index(gy)

    def _nearest_path_edge_index(self, g):
        if (g - self.c) % 4 in (1, 2):
            return self._nearest_120_edge_index(g)
        if (g - self.c) % 4 in (0, 3):
            return self._nearest_60_edge_index(g)
        return self._nearest_30_edge_index(g)

    def _nearest_120_edge_index(self, g):
        k = int(round((g + 0.5 - (self.c + 2)) / 4.0))
        return max(0, min(self.n, self.c + 2 + 4 * k))

    def _nearest_60_edge_index(self, g):
        k = int(round((g + 0.5 - (self.c + 2)) / 2.0))
        return max(0, min(self.n, self.c + 2 + 2 * k))

    def _nearest_30_edge_index(self, g):
        return max(0, min(self.n, int(round(g + 0.5))))


    # ------------------------------------------------------------------ #
    #  Status
    # ------------------------------------------------------------------ #
    def floor_counts(self):
        vals, counts = np.unique(self.floor, return_counts=True)
        out = {}
        for val, count in zip(vals, counts):
            ch = self.FLOOR_CODE_TO_CHAR.get(int(val), str(int(val)))
            if ch != "0":
                out[ch] = int(count)
        return out

    def cell_state_counts(self):
        vals, counts = np.unique(self.cell_state, return_counts=True)
        out = {}
        for val, count in zip(vals, counts):
            ch = self.CELL_STATE_TO_CHAR.get(int(val), str(int(val)))
            out[ch] = int(count)
        return out

    def summary(self):
        return {
            "grid_mm": self.NAV_GRID_MM,
            "shape": [int(self.n), int(self.n)],
            "floor_cells": int(self.n * self.n),
            "cell_state_counts": self.cell_state_counts(),
            "current_beam_cells": int(self.last_current_beam_cells),
            "wall_adjacent_discovered_cells": int(self.last_wall_adjacent_discovered_cells),
            "plan_status": self.plan_status,
            "inspect_walls": self.PATH_INSPECT_WALLS,
            "plan_cost": self.plan_cost,
            "plan_destination": ([int(self.plan_destination[0]), int(self.plan_destination[1])]
                                 if self.plan_destination is not None else None),
            "plan_destination_corner": ([int(self.plan_destination_corner[0]),
                                         int(self.plan_destination_corner[1])]
                                        if self.plan_destination_corner is not None else None),
            "plan_path_cells": int(len(self.plan_path)),
            "plan_path_signature": self._points_checksum(self.plan_path),
            "plan_corner_path_signature": self._points_checksum(
                self._path_corner_points()),
            "plan_waypoints": [[int(x), int(y)] for x, y in self.plan_waypoints],
            "special_floor_cells": int(np.count_nonzero(self.floor != self.FLOOR_NORMAL)),
            "floor_counts": self.floor_counts(),
            "wall_v_edges": int(np.count_nonzero(self.wall_v)),
            "wall_h_edges": int(np.count_nonzero(self.wall_h)),
            "wall_edges": int(np.count_nonzero(self.wall_v) +
                              np.count_nonzero(self.wall_h)),
            "wall_model": "zero-thickness edges",
            "wall_alias_model": "straight_highres_with_curve_only_stairs",
            "wall_source": self.last_wall_source,
            "edge_segment_cells": int(self.EDGE_SEG_CELLS),
            "edge_min_span_cells": int(self.EDGE_MIN_SPAN_CELLS),
            "curve_stair_radius_cells": int(self.CURVE_STAIR_RADIUS_CELLS),
            "curve_stair_min_arc_hits": int(OccupancyMapper.ARC_SOFT_HITS),
            "curve_stair_bends": int(self.last_curve_stairs),
            "curve_stair_skipped": int(self.last_curve_stairs_skipped),
            "start_nav": [int(self.start_nav[0]), int(self.start_nav[1])],
            "current_nav": ([int(self.current_nav[0]), int(self.current_nav[1])]
                            if self.current_nav is not None else None),
            "current_nav_xy": ([round(float(self.current_nav_xy[0]), 2),
                                round(float(self.current_nav_xy[1]), 2)]
                               if self.current_nav_xy is not None else None),
            "current_yaw_deg": self.current_yaw_deg,
            "pose_source": self.current_pose_source,
            "yaw_source": self.current_yaw_source,
            "used_bbox": list(self._last_used_bbox) if self._last_used_bbox else None,
        }


class MappingGPSFilter:
    """Average noisy GPS positions after transporting them to the current pose.

    GPS.getSpeedVector() supplies world velocity independently of position
    noise. Right-endpoint integration matches the measured Webots update.
    The default49-sample window preserves canonical mapping. The optional
    physical window reduces noise at the same stated confidence without a
    moving-median lag. Missing samples, invalid velocity and relocation reset the window;
    no unmeasured interval is extrapolated. This is an estimate, not a bound
    on the GPS device's Gaussian position noise.
    """
    WINDOW = 49
    # Unchanged custom_robot.proto GPS accuracy (Webots standard deviation),
    # with default noiseCorrelation=0. The transported 49-sample mean has
    # ideal per-axis sigma 2.5/sqrt(49)mm. Reusing the single-sample sigma as
    # a footprint registration allowance is 7sigma per axis, not a hard bound.
    RAW_GPS_SIGMA_MM = 2.5
    REGISTRATION_SIGMAS = 7.0
    # The shortest history whose ideal7sigma position disk reaches the
    # existing1mm physical-cell resolution. Canonical mapping remains49.
    PHYSICAL_WINDOW = math.ceil((REGISTRATION_SIGMAS*RAW_GPS_SIGMA_MM
                                 /PhysicalEvidenceGrid.PRECISE_GRID_MM)**2)

    def __init__(self, period_s, window=None):
        self.period_s = period_s
        self.window = self.WINDOW if window is None else int(window)
        if self.window < self.WINDOW:
            raise ValueError('Mapping GPS history cannot be shorter than49 samples')
        self.history = deque()
        self.total = np.zeros(2)
        self.displacement = np.zeros(2)
        self.time = None

    def reset(self):
        self.history.clear(); self.total[:] = 0
        self.displacement[:] = 0; self.time = None

    def update(self, raw_x, raw_y, velocity, now, fallback):
        raw = np.array([raw_x, raw_y], dtype=float)
        velocity = np.asarray(velocity, dtype=float)
        valid = (velocity.shape == (3,) and np.isfinite(velocity).all()
                 and np.isfinite(raw).all() and math.isfinite(now))
        if not valid:
            self.reset()
            return fallback, False
        dt = now-self.time if self.time is not None else 0
        continuous = self.time is None or abs(dt-self.period_s) <= 1e-8
        if continuous and self.time is not None:
            self.displacement += velocity[[0, 2]]*1000*dt
        previous = (self.displacement+self.total/len(self.history)
                    if self.history else raw)
        if not continuous or np.linalg.norm(raw-previous) > 60:
            self.history.clear(); self.total[:] = 0; self.displacement[:] = 0
        self.time = now
        if len(self.history) == self.window:
            self.total -= self.history.popleft()
        anchor = raw-self.displacement
        self.history.append(anchor); self.total += anchor
        return (self.displacement+self.total/len(self.history),
                len(self.history) == self.window)


class KavoshRobot:
    # Compatibility names for probe tools; normal runs use the two controls at
    # the top of this file. All diagnostic entry points obey DEBUG_ENABLED.
    DEBUG_ENABLED = globals().get("DEBUG", True)
    SHOW_MAPS = globals().get("SHOW_MAPS", True)
    CAPTURE_DIR = 'captures'
    AUTO_CAPTURE_PERIOD_S = 0.5   # while auto-capture is on, save this often
    # Manual arrow-key driving uses full motor speed, with or without Shift.
    DRIVE_SPEED = 6.28           # wheel rad/s for normal driving (motor max)
    DRIVE_TURBO = 6.28           # compatibility: Shift uses the same full speed
    DRIVE_TURN = 0.6             # turn authority (fraction of forward mix)

    def __init__(self):
        self.robot = Robot()
        self.timestep = int(self.robot.getBasicTimeStep())

        self._init_motors()
        self._init_sensors()

        self._gps_x_history = deque(maxlen=9)
        self._gps_y_history = deque(maxlen=9)
        self._gps_filtered_x = None
        self._gps_filtered_y = None
        self._gps_ema_alpha = 0.25

        # Low-lag position for mapping: a short median de-spikes the raw GPS
        # without the lag of the navigation filter, so scans taken while moving
        # land in the correct place.
        self._gps_map_hist_x = deque(maxlen=3)
        self._gps_map_hist_y = deque(maxlen=3)
        self._mapping_gps = MappingGPSFilter(self.timestep/1000.0)
        self._physical_mapping_gps = MappingGPSFilter(self.timestep/1000.0,
            window=MappingGPSFilter.PHYSICAL_WINDOW)
        self._gps_reset_pending = False

        self.keyboard = self.robot.getKeyboard()
        self.keyboard.enable(self.timestep)

        # Resume numbering after the last capture already on disk (so 's' never
        # overwrites an earlier session's point_N_* files); starts at 0 only
        # when the capture folder is empty.
        self.capture_index = self._last_capture_index() if self.DEBUG_ENABLED else 0
        self.last_saved = []

        # auto-capture: toggled with 'a', saves a numbered point every
        # AUTO_CAPTURE_PERIOD_S so a mid-motion disruption gets recorded
        self.auto_capture = False
        self._last_auto_capture_t = -1e9

        self.mapper = RelayOccupancyMapper()
        self.mapper.DEBUG_MAPPING_DUMP = self.DEBUG_ENABLED
        self.mapper._record_stages = self.DEBUG_ENABLED
        self.navigator = NavigationMapper(async_planning=True)
        self.emitter = self.robot.getDevice("emitter")
        self.vision = RescueVision(self.mapper, camera_size=(self.cam_w,self.cam_h),
            camera_fov=self.cam_fov,debug_enabled=self.DEBUG_ENABLED,async_processing=True)
        self.token_result = {"objects": []}

    def _last_capture_index(self):
        """Highest N among existing point_N_* files in CAPTURE_DIR (0 if none)."""
        last = 0
        try:
            for name in os.listdir(self.CAPTURE_DIR):
                m = re.match(r"point_(\d+)_", name)
                if m:
                    last = max(last, int(m.group(1)))
        except OSError:
            pass
        return last

    def record_debug_step(self, data, keys=(), model_update=False):
        """Record each control step and each integrated scan independently of S.

        A later correct map must not erase an earlier missed/changed feature.
        Raw scan bytes and their frame indices permit exact ordered replay;
        model events retain intermediate wall, curve, floor and room decisions.
        """
        if not self.DEBUG_ENABLED or getattr(self, "_trace_failed", False):
            return
        try:
            if not hasattr(self, "_trace_frames"):
                parent = os.path.join(self.CAPTURE_DIR, "traces")
                os.makedirs(parent, exist_ok=True)
                self.trace_dir = tempfile.mkdtemp(prefix="run_", dir=parent)
                self._trace_frames = open(os.path.join(self.trace_dir, "frames.jsonl"), "w", buffering=1)
                self._trace_events = open(os.path.join(self.trace_dir, "models.jsonl"), "w", buffering=1)
                self._trace_floors = open(os.path.join(self.trace_dir, "floor_events.jsonl"), "w", buffering=1)
                self._trace_lidar = open(os.path.join(self.trace_dir, "integrated_scans.i32"), "wb", buffering=0)
                self._trace_raw_lidar = open(os.path.join(self.trace_dir, "raw_lidar.f32"), "wb", buffering=0)
                self._trace_stages = open(os.path.join(self.trace_dir, "pipeline_stages.jsonl"), "w", buffering=1)
                self._trace_scan_count = self._trace_frame_count = 0
                self._trace_previous_model = None
                self._trace_previous_floor = None
                source = globals().get("__file__")
                digest = None
                if source and os.path.isfile(source):
                    with open(source, "rb") as stream:
                        source_bytes = stream.read()
                    digest = hashlib.sha256(source_bytes).hexdigest()
                    with open(os.path.join(self.trace_dir, "controller_source.py"), "wb") as stream:
                        stream.write(source_bytes)
                with open(os.path.join(self.trace_dir, "manifest.json"), "w") as stream:
                    json.dump({"source_sha256": digest, "sensor_period_s": self.timestep/1000,
                        "lidar_dtype": "<i4", "lidar_values_per_scan": len(data["lidar"]),
                        "lidar_hres": data.get("lidar_hres"), "lidar_fov": data.get("lidar_fov"),
                        "lidar_vfov": data.get("lidar_vfov"),
                        "lidar_projection": "r2023b-depth-texel-NUE-v2",
                        "raw_lidar_dtype": "<f4", "raw_lidar_units": "metres",
                        "initial_state_after_frame": 0,
                        "side_camera_fusion": {"surfaces": self.mapper.CAM_FUSE, "floor": self.mapper.FLOOR_SIDE_CAM_FUSE},
                        "meaning": "Every control frame, including rejected raw LiDAR scans. Byte offsets index raw_lidar.f32. Integrated scans retain the exact integer mapping inputs. Live model, floor, planner and pipeline-stage events are independent of S. Camera pixels are RAM-only and are not recorded. Numeric vision decisions and pipeline timing/counts are recorded in exploration.jsonl. Independent observer/device/world poses are not collected by this production trace."}, stream)
                self.mapper.save_raw_debug(os.path.join(self.trace_dir, "initial_state_raw.npz"))
                with open(os.path.join(self.trace_dir, "initial_state_debug.json"), "w") as stream:
                    json.dump({"debug_snapshot": self.mapper._update_debug_snapshot()}, stream)
            m, n = self.mapper, self.navigator
            integrated = m._last_pose is not None and m._last_pose["time"] == data["time"]
            scan_index = None
            if integrated:
                scan_index = self._trace_scan_count
                self._trace_lidar.write(np.asarray(data["lidar"], dtype="<i4").tobytes())
                self._trace_scan_count += 1
            raw_lidar = data.get("lidar_raw_m")
            unrounded = raw_lidar is not None
            if raw_lidar is None:
                raw_lidar = np.asarray(data["lidar"], dtype=np.float32)/1000
            raw_offset = self._trace_raw_lidar.tell()
            self._trace_raw_lidar.write(np.asarray(raw_lidar, dtype="<f4").tobytes())
            frame = {k: data[k] for k in ("time", "position", "orientation", "color", "distance", "distance_raw_m", "encoders", "gps_velocity_mps", "supervisor_relocated", "gps_filter_reset", "motor_command_radps", "motor_command_before_control_radps", "cam_w", "cam_h", "cam_fov", "lidar_hres", "lidar_layers", "lidar_fov", "lidar_vfov") if k in data}
            frame.update({"frame": self._trace_frame_count, "keys": sorted(keys),
                "sensor_period_s": self.timestep/1000, "origin_mm": [m.origin_x,m.origin_y],
                "raw_lidar_offset_bytes": raw_offset, "raw_lidar_values": len(raw_lidar),
                "raw_lidar_unrounded": unrounded,
                "scan_index": scan_index, "integrated_pose": m._last_pose if integrated else None,
                "free_space": m.last_free_space_debug if integrated else None,
                "curve_fits": m.last_curve_fit_debug if integrated else None,
                "obstacle_fits": m.last_obstacle_fit_debug if integrated else None,
                "curve_raster": m.last_curve_raster_debug if model_update else None,
                "integration_status": "integrated" if integrated else
                    "speed-gate" if m.last_speed > m.MAP_MAX_SPEED_MMPS else "motion-filter",
                "speed_mmps": m.last_speed, "speed_source": m.last_speed_source,
                "floor_fusion": m.last_color_sensor_fusion})
            if hasattr(self, "left_motor"):
                frame["motor_targets_radps"] = [self.left_motor.getVelocity(), self.right_motor.getVelocity()]
            self._trace_frames.write(json.dumps(frame)+"\n")
            self._trace_frame_count += 1
            floor_state = {"floor": {str(t):v for t,v in m._committed_floor_tiles.items()},
                "provisional_holes": sorted(m._provisional_holes), "current_room": m._current_room}
            floor_changed = floor_state != self._trace_previous_floor
            if floor_changed:
                self._trace_floors.write(json.dumps({"time": data["time"],
                    "frame": frame["frame"], "fusion": m.last_color_sensor_fusion, **floor_state})+"\n")
                self._trace_previous_floor = floor_state
            plan = {"destination": n.plan_destination,
                    "destination_corner": n.plan_destination_corner,
                    "requested_corner": n.plan_requested_corner,
                    "status": n.plan_status,
                    "points": n._path_corner_points()}
            if model_update or integrated or floor_changed or plan != getattr(self, "_trace_previous_plan", None):
                competition = m.competition_matrix()
                state = {"matrix_meta": m._matrix_meta, "matrix": ["".join(r) for r in m.report_matrix],
                    "competition_matrix": ["".join(r) for r in competition],
                    "competition_cells": competition if any(len(cell)>1 for row in competition for cell in row) else None,
                    "floor": {str(t):v for t,v in m._committed_floor_tiles.items()},
                    "rooms": {str(t):v for t,v in m._room_tiles.items()},
                    "area4_tiles": sorted(m._area4_tiles), "obstacles": sorted(m.obstacle_tiles),
                    "curve_models": sorted(m._confirmed_curve_quadrants),
                    "straight_models": sorted(m._confirmed_straight_walls),
                    "plan": plan,
                    "nav_h": np.argwhere(n.wall_h).tolist(), "nav_v": np.argwhere(n.wall_v).tolist()}
                if state != self._trace_previous_model:
                    self._trace_events.write(json.dumps({"time": data["time"], "frame": frame["frame"],
                        "matrix_rebuilt": model_update, "scan_index": scan_index, **state})+"\n")
                    self._trace_previous_model = state
                self._trace_previous_plan = plan
            if model_update:
                self._trace_stages.write(json.dumps({"time": data["time"], "frame": frame["frame"],
                    "matrix_meta": m._matrix_meta,
                    "stages": {key: ["".join(row) for row in value]
                               for key, value in m._stage_snapshots.items()}})+"\n")
        except OSError as error:
            self._trace_failed = True
            sys.stderr.write(f"Continuous mapping trace failed: {error}\n")

    def save_capture(self, data):
        """Keep diagnostic storage errors from terminating robot control."""
        if not self.DEBUG_ENABLED:
            return []
        self.last_saved = []
        self.last_capture_error = None
        previous_recording = self.mapper._record_stages
        try:
            return self._write_capture(data)
        except OSError as error:
            # DEBUG ONLY: retain partial files under this consumed point index.
            self.last_capture_error = str(error)
            sys.stderr.write(f"Capture failed: {error}; partial files retained.\n")
            return []
        finally:
            self.mapper._record_stages = previous_recording

    def _write_capture(self, data):
        """Write one numbered diagnostic capture; caller handles IO failures."""
        self.capture_index += 1
        idx = self.capture_index
        os.makedirs(self.CAPTURE_DIR, exist_ok=True)
        base = os.path.join(self.CAPTURE_DIR, f"point_{idx}")

        # record per-stage matrix snapshots while building this capture
        previous_recording = self.mapper._record_stages
        self.mapper._record_stages = True
        self.mapper.build_report_matrix(show=self.SHOW_MAPS)
        self.mapper._record_stages = previous_recording
        self.navigator.update(data, self.mapper, show=False)

        hi_path = base + "_highres.png"
        mat_path = base + "_matrix.png"
        self.mapper._save_rgb_png(hi_path, self.mapper._highres_image())
        self.mapper._save_rgb_png(mat_path, self.mapper._report_image())

        map_json = base + "_map.json"
        self.mapper.save_json(map_json)

        matrix_py = base + "_matrix.py"
        self.mapper.save_emitter_matrix_txt(
            matrix_py,
            title=f"Captured map matrix (point {idx}).",
        )

        debug_json = base + "_debug.json"
        with open(debug_json, "w", encoding="utf-8") as f:
            json.dump(self._build_debug_report(data, idx), f, indent=2)

        # raw, loss-less data for offline pipeline debugging
        raw_npz = base + "_raw.npz"
        stages_json = base + "_stages.json"
        self.mapper.save_raw_debug(raw_npz, stages_json)
        nav_debug_paths = self.navigator.save_debug(base, self.mapper, data)

        # side-camera frames + intrinsics/pose for the camera-fusion work
        cam_paths = self._save_camera_debug(base, data)

        # floor-tile colour debug (side cameras looking down at the floor)
        floor_paths = self._save_floor_debug(base, data)

        self.last_saved = [hi_path, mat_path, map_json, matrix_py,
                           debug_json, raw_npz, stages_json] + nav_debug_paths + cam_paths + floor_paths
        return self.last_saved

    def _save_camera_debug(self, base, data):
        """Camera image persistence is disabled; numeric/raw mapping traces remain."""
        return []

    def _save_floor_debug(self, base, data):
        """Camera image persistence is disabled; numeric/raw mapping traces remain."""
        return []

    def _build_debug_report(self, data, idx):
        lidar = data["lidar"]
        hres = data["lidar_hres"]
        start = 1024
        front_layer = []
        if len(lidar) >= start + hres:
            front_layer = [int(v) for v in lidar[start:start + hres]]
        return {
            "index": idx,
            "time": round(data["time"], 2),
            "gps_filtered": [data["position"]["x"], data["position"]["y"]],
            "gps_raw": [data["position"]["raw_x"], data["position"]["raw_y"]],
            "gps_map": [data["position"]["map_x"], data["position"]["map_y"]],
            "yaw": data["orientation"]["yaw"],
            "distance": data["distance"],
            "color": [data["color"]["red"], data["color"]["green"], data["color"]["blue"]],
            "settled_on_tile": self.mapper.last_settled,
            "speed_mmps": round(self.mapper.last_speed, 1),
            "speed_source": self.mapper.last_speed_source,
            "color_sensor_class": self.mapper.classify_color_sensor(
                data["color"]["red"], data["color"]["green"], data["color"]["blue"]),
            "color_sensor_fusion": self.mapper.last_color_sensor_fusion,
            "floor_commit_log": sorted(self.mapper._floor_commit_log.values(),
                                       key=lambda r: r.get("seq", 0)),
            "continuous_trace_dir": getattr(self, "trace_dir", None),
            "mapping_summary": self.mapper.summary(),
            "debug_snapshot": self.mapper.debug_snapshot,
            "front_layer_mm": front_layer,
            "cameras": {
                "left": {
                    "fov_rad": self.left_camera.getFov(),
                    "w": self.left_camera.getWidth(),
                    "h": self.left_camera.getHeight(),
                },
                "right": {
                    "fov_rad": self.right_camera.getFov(),
                    "w": self.right_camera.getWidth(),
                    "h": self.right_camera.getHeight(),
                },
            },
        }

    def _init_motors(self):
        self.left_motor = self.robot.getDevice("wheelL motor")
        self.right_motor = self.robot.getDevice("wheelR motor")

        self.left_motor.setPosition(float("inf"))
        self.right_motor.setPosition(float("inf"))

        self.motor(0, 0, 0)

    def _init_sensors(self):
        self.left_encoder = self.robot.getDevice("wheelL sensor")
        self.right_encoder = self.robot.getDevice("wheelR sensor")
        self.left_encoder.enable(self.timestep)
        self.right_encoder.enable(self.timestep)

        self.left_camera = self.robot.getDevice("camL")
        self.right_camera = self.robot.getDevice("camR")
        # Wider optics recovered native moving glyph views at the same64x64
        # pixel budget; preserve the measured read-back for both vision engines.
        cameras = (self.left_camera, self.right_camera)
        if any((c.getWidth(), c.getHeight()) != (64, 64) for c in cameras):
            raise ValueError("camL and camR must provide real64x64 camera frames")
        desired_fov = 1.2
        if all(c.getMinFov() <= desired_fov <= c.getMaxFov() for c in cameras):
            for camera in cameras:
                camera.setFov(desired_fov)
        fovs = tuple(float(c.getFov()) for c in cameras)
        if not all(math.isfinite(f) and f > 0 for f in fovs) or abs(fovs[0]-fovs[1]) > 1e-6:
            raise ValueError("camL and camR require matching valid optical calibration")
        for camera in cameras:
            camera.enable(self.timestep)
        self.cam_w = self.left_camera.getWidth()
        self.cam_h = self.left_camera.getHeight()
        self.cam_fov = fovs[0]

        self.imu = self.robot.getDevice("imu")
        self.imu.enable(self.timestep)

        self.gps = self.robot.getDevice("gps")
        self.gps.enable(self.timestep)

        self.lidar = self.robot.getDevice("lidar")
        self.lidar.enable(self.timestep)
        self.lidar_hres = self.lidar.getHorizontalResolution()
        self.lidar_layers = max(1, self.lidar.getNumberOfLayers())
        self.lidar_fov = self.lidar.getFov()
        self.lidar_vfov = self.lidar.getVerticalFov()
        lidar_size = self.lidar_hres * self.lidar_layers
        self._lidar_float = np.empty(lidar_size, dtype=np.float32)
        self._lidar_raw_m = np.empty(lidar_size, dtype=np.float32)
        self._lidar_mm = np.empty(lidar_size, dtype=np.int32)

        self.color_sensor = self.robot.getDevice("color")
        self.color_sensor.enable(self.timestep)

        self.distance_sensor = self.robot.getDevice("dist")
        self.distance_sensor.enable(self.timestep)
        self.receiver = self.robot.getDevice("receiver")
        if self.receiver is not None:
            self.receiver.enable(self.timestep)

    def drive_from_keys(self, keys, turbo=False):
        """Manual driving from held ARROW keys (Up/Down = forward/back,
        Left/Right = turn; combine for arcs). `keys` is the set of base key codes
        currently held. Returns True while any drive key is held. Speeds are kept
        full speed by default; turbo=True (Shift) retains the same speed.
        """
        fwd = (Keyboard.UP in keys) - (Keyboard.DOWN in keys)
        turn = (Keyboard.RIGHT in keys) - (Keyboard.LEFT in keys)
        if not fwd and not turn:
            self.motor(0, 0, 0)
            return False
        speed = self.DRIVE_TURBO if turbo else self.DRIVE_SPEED
        left = fwd + self.DRIVE_TURN * turn
        right = fwd - self.DRIVE_TURN * turn
        self.motor(left, right, speed)
        return True

    def motor(self, left, right, max_speed):
        max_input = max(abs(left), abs(right))

        if max_input == 0 or max_speed == 0:
            self._motor_command = [0., 0.]
            self.left_motor.setVelocity(0)
            self.right_motor.setVelocity(0)
            return

        scale = min(abs(max_speed), 6.28) / max_input
        self._motor_command = [left*scale,right*scale]
        self.left_motor.setVelocity(left * scale)
        self.right_motor.setVelocity(right * scale)

    def stop(self):
        self.motor(0, 0, 0)

    def _filter_gps(self, raw_x, raw_y):
        if self._gps_filtered_x is not None and math.hypot(
            raw_x - self._gps_filtered_x, raw_y - self._gps_filtered_y
        ) > 60:
            self._gps_x_history.clear()
            self._gps_y_history.clear()
            self._gps_filtered_x = None
            self._gps_filtered_y = None

        self._gps_x_history.append(raw_x)
        self._gps_y_history.append(raw_y)

        median_x = float(np.median(self._gps_x_history))
        median_y = float(np.median(self._gps_y_history))

        if self._gps_filtered_x is None:
            self._gps_filtered_x = median_x
            self._gps_filtered_y = median_y
        else:
            alpha = self._gps_ema_alpha
            self._gps_filtered_x += alpha * (median_x - self._gps_filtered_x)
            self._gps_filtered_y += alpha * (median_y - self._gps_filtered_y)

        return round(self._gps_filtered_x), round(self._gps_filtered_y)

    def _gps_relocation_reset(self):
        """Handle the judge's legal one-byte L relocation notification.

        Clear once more on the following sensor frame: the notification can
        arrive on either side of the GPS/LiDAR sampling cycle. No map, origin,
        confirmed feature or explicit navigation destination is discarded.
        """
        relocated = False
        receiver = getattr(self, 'receiver', None)
        while receiver is not None and receiver.getQueueLength():
            packet = receiver.getBytes()
            receiver.nextPacket()
            relocated |= packet == b'L'
        reset = relocated or getattr(self, '_gps_reset_pending', False)
        if reset:
            for history in (self._gps_x_history, self._gps_y_history,
                            self._gps_map_hist_x, self._gps_map_hist_y):
                history.clear()
            self._gps_filtered_x = self._gps_filtered_y = None
            self._mapping_gps.reset()
            physical_filter = getattr(self, '_physical_mapping_gps', None)
            if physical_filter is not None:
                physical_filter.reset()
        self._gps_reset_pending = relocated
        return relocated, reset

    def read_all_sensors(self):
        relocated, gps_filter_reset = self._gps_relocation_reset()
        gps_values = self.gps.getValues()
        raw_x_float = gps_values[0] * 1000
        raw_y_float = gps_values[2] * 1000
        speed_mmps = self.gps.getSpeed() * 1000
        filtered_x, filtered_y = self._filter_gps(raw_x_float, raw_y_float)
        raw_x = round(raw_x_float)
        raw_y = round(raw_y_float)

        self._gps_map_hist_x.append(raw_x_float)
        self._gps_map_hist_y.append(raw_y_float)
        map_x = round(float(np.median(self._gps_map_hist_x)))
        map_y = round(float(np.median(self._gps_map_hist_y)))
        now = self.robot.getTime()
        try:
            gps_velocity = list(self.gps.getSpeedVector())
        except AttributeError:
            gps_velocity = None
        mapping_xy, map_pose_ready = self._mapping_gps.update(
            raw_x_float, raw_y_float, gps_velocity, now, (map_x, map_y))
        map_x, map_y = map(round, mapping_xy)
        physical_xy, _ = self._physical_mapping_gps.update(
            raw_x_float, raw_y_float, gps_velocity, now, (map_x, map_y))
        physical_samples = len(self._physical_mapping_gps.history)
        physical_ready = map_pose_ready and physical_samples >= MappingGPSFilter.WINDOW
        physical_sigma = (MappingGPSFilter.RAW_GPS_SIGMA_MM/math.sqrt(physical_samples)
                          if physical_ready else None)

        roll, pitch, yaw = self.imu.getRollPitchYaw()
        yaw_degrees = round((math.degrees(yaw) + 360) % 360)
        body_mapping_xy = None
        if physical_ready:
            attitude = scipy_Rotation.from_euler('YZX', [yaw, pitch, roll]).as_matrix()
            gps_lever = (attitude @ np.array([0.0,18.5,0.0]))[[0,2]]
            body_mapping_xy = np.asarray(physical_xy)-gps_lever

        color_image = self.color_sensor.getImage()
        color_width = self.color_sensor.getWidth()
        red = self.color_sensor.imageGetRed(color_image, color_width, 0, 0)
        green = self.color_sensor.imageGetGreen(color_image, color_width, 0, 0)
        blue = self.color_sensor.imageGetBlue(color_image, color_width, 0, 0)

        lidar_raw = self.lidar.getRangeImage()
        if lidar_raw:
            n = len(lidar_raw)
            np.copyto(self._lidar_raw_m[:n], lidar_raw)
            np.multiply(self._lidar_raw_m[:n], 1000, out=self._lidar_float[:n])
            np.rint(self._lidar_float[:n], out=self._lidar_float[:n])
            # Invalid/open rays remain outside every hit gate without an
            # undefined float-to-int cast or a platform-dependent int sentinel.
            np.nan_to_num(self._lidar_float[:n], copy=False,
                          nan=0, posinf=1_000_000, neginf=0)
            self._lidar_mm[:n] = self._lidar_float[:n]
            lidar = self._lidar_mm[:n]
        else:
            lidar = self._lidar_mm[:0]

        return {
            "time": now,
            "gps_velocity_mps": gps_velocity,
            "supervisor_relocated": relocated,
            "gps_filter_reset": gps_filter_reset,
            "sensor_period_s": self.timestep/1000.0,
            "motor_command_radps": list(self._motor_command),
            "motor_command_before_control_radps": list(self._motor_command),
            "position": {
                "x": filtered_x,
                "y": filtered_y,
                "raw_x": raw_x,
                "raw_y": raw_y,
                "raw_x_float": raw_x_float,
                "raw_y_float": raw_y_float,
                "speed_mmps": speed_mmps,
                "raw_xyz_m": list(gps_values),
                "map_x": map_x,
                "map_y": map_y,
                "map_pose_ready": map_pose_ready,
                # Functional physical input; never reconstructed from rounded
                # pose values or the independently noisy raw GPS reading.
                "map_x_float": float(physical_xy[0]) if physical_ready else None,
                "map_y_float": float(physical_xy[1]) if physical_ready else None,
                "map_physical_pose_ready": physical_ready,
                "mapping_reference": "gps_device_xz_mm",
                "map_position_samples": physical_samples,
                "map_position_sigma_mm": physical_sigma,
                "map_registration_allowance_mm": MappingGPSFilter.REGISTRATION_SIGMAS*physical_sigma if physical_ready else None,
                "map_body_x_float": float(body_mapping_xy[0]) if body_mapping_xy is not None else None,
                "map_body_y_float": float(body_mapping_xy[1]) if body_mapping_xy is not None else None,
            },
            "orientation": {
                "roll": roll,
                "pitch": pitch,
                "yaw": yaw_degrees,
                "yaw_raw": (math.degrees(yaw)+360) % 360,
            },
            "distance": round(self.distance_sensor.getValue() * 100),
            "distance_raw_m": self.distance_sensor.getValue(),
            "color": {
                "red": red,
                "green": green,
                "blue": blue,
            },
            "encoders": {
                "left": self.left_encoder.getValue(),
                "right": self.right_encoder.getValue(),
            },
            "lidar": lidar,
            "lidar_raw_m": self._lidar_raw_m[:len(lidar)],
            "lidar_hres": self.lidar_hres,
            "lidar_layers": self.lidar_layers,
            "lidar_fov": self.lidar_fov,
            "lidar_vfov": self.lidar_vfov,
            "cameras": {
                "left": self.left_camera.getImage(),
                "right": self.right_camera.getImage(),
            },
            "cam_w": self.cam_w,
            "cam_h": self.cam_h,
            "cam_fov": self.cam_fov,
        }

    def print_sensor_data(self, data):
        if not self.DEBUG_ENABLED:
            return
        # DEBUG ONLY: bound console work by wall time, independently of the
        # full16ms sensor trace and simulation speed.
        now = time.monotonic()
        if now-getattr(self, "_last_console_wall_s", -math.inf) < 0.25:
            return
        self._last_console_wall_s = now
        scan = data["lidar"]
        hres = int(data.get("lidar_hres", 512))
        offsets = [1024+i*hres//4 for i in range(4)]
        front, right, back, left = (scan[i] if 0 <= i < len(scan) else -1
                                    for i in offsets)
        info = self.mapper.summary()
        nav_info = self.navigator.summary()

        sys.stdout.write(
            "\033[2J\033[H"
            "========== Kavosh Sensors ==========\n"
            f"Time: {data['time']:.2f}\n"
            f"GPS filtered: x={data['position']['x']}, y={data['position']['y']}\n"
            f"GPS raw:      x={data['position']['raw_x']}, y={data['position']['raw_y']}\n"
            f"Yaw: {data['orientation']['yaw']}\n"
            f"Distance: {data['distance']}\n"
            f"Color RGB: {data['color']['red']}, "
            f"{data['color']['green']}, "
            f"{data['color']['blue']}\n"
            f"-> special: {self.mapper.last_color_sensor_fusion.get('class')} "
            f"(run {self.mapper.last_color_sensor_fusion.get('run', 0)})"
            f"{(' @tile ' + str(self.mapper.last_color_sensor_fusion.get('committed'))) if self.mapper.last_color_sensor_fusion.get('committed') else ''}\n"
            f"Cliff/hole: dist={self.mapper.last_color_sensor_fusion.get('distance_cm')}cm "
            f"floor~{self.mapper.last_color_sensor_fusion.get('floor_dist_cm')}cm "
            f"pit={self.mapper.last_color_sensor_fusion.get('pit_color')} "
            f"{'HOLE' if (self.mapper.last_color_sensor_fusion.get('committed') and str(self.mapper.last_color_sensor_fusion.get('commit_source', '')).startswith('cliff')) else ('cliff' if self.mapper.last_color_sensor_fusion.get('cliff') else 'floor ok')}\n"
            f"Lidar front: {front} {right} {back} {left}\n"
            "========== Mapping ==========\n"
            f"Wall hits: {info['wall_hits']}\n"
            f"Occupied cells: {info['occupied_cells']}\n"
            f"Free cells: {info['free_cells']}\n"
            f"Visited tiles: {info['visited_tiles']}\n"
            f"Confirmed walls: {info['confirmed_walls']}\n"
            f"Floor tiles: {len(self.mapper._committed_floor_tiles)} "
            f"{''.join(sorted(self.mapper._committed_floor_tiles.values()))}\n"
            f"Robot world: {self.mapper.last_lidar_debug.get('robot_world')}\n"
            f"Front beam -> {self.mapper.last_lidar_debug.get('front_world')}\n"
            f"Settled on tile: {self.mapper.last_settled}\n"
            f"Speed: {self.mapper.last_speed:.0f} mm/s "
            f"(map gate {self.mapper.MAP_MAX_SPEED_MMPS} mm/s)\n"
            f"Report matrix: {info['report_rows']} x {info['report_cols']}\n"
            "========== Navigation Map ==========" "\n"
            f"Nav floor grid: {nav_info['shape'][0]} x {nav_info['shape'][1]} "
            f"({nav_info['grid_mm']} mm cells)\n"
            f"Nav wall model: {nav_info['wall_model']}\n"
            f"Nav wall edges V/H: {nav_info['wall_v_edges']} / "
            f"{nav_info['wall_h_edges']}\n"
            f"Nav cell states: {nav_info['cell_state_counts']}\n"
            f"Nav beam/wall-fill: {nav_info['current_beam_cells']} / "
            f"{nav_info['wall_adjacent_discovered_cells']}\n"
            f"Nav path: {nav_info['plan_status']} "
            f"cells={nav_info['plan_path_cells']} "
            f"goal={nav_info['plan_destination']} "
            f"corner={nav_info['plan_destination_corner']}\n"
            f"Nav floor special cells: {nav_info['special_floor_cells']} "
            f"{nav_info['floor_counts']}\n"
            f"Nav start/current: {nav_info['start_nav']} / "
            f"{nav_info['current_nav_xy']} ({nav_info['pose_source']})\n"
            "========== Controls ==========\n"
            "Drive: Arrow keys at full speed (Up/Down move, Left/Right turn).\n"
            "'s' save a numbered capture; 'a' auto-capture; 'f' border closure.\n"
            f"Auto-capture: {'ON' if self.auto_capture else 'OFF'}  "
            f"Captures saved so far: {self.capture_index}\n"
        )
        if self.last_saved:
            sys.stdout.write(
                f"Last saved (point {self.capture_index}) - upload these:\n"
            )
            for path in self.last_saved:
                sys.stdout.write(f"  {path}\n")
        sys.stdout.flush()

# BEGIN EMBEDDED HeroDriving class
class HeroDriving:
    """Self-contained Hero waypoint driving. Copy this entire class into Hero.py.

    No sibling files or global imports are required. The existing Hero loop
    owns integration and simulation stepping. Requirements: NumPy and
    SciPy, already used by Hero. Original geometry: 20 mm wheel radius,
    34 mm wheel-centre separation, 6.28 rad/s motor limit.

    API:
        driving = HeroDriving(bot)             # all diagnostics disabled
        driving.set_route([(120, 0), (120, 120)])
        result = driving.update(sensor_data)   # once per existing sensor step
        driving.update_route(new_points, revision=1)
        driving.cancel()                       # immediate zero commands
        driving.close()                        # stop and close enabled debug

    Points are planner-relative millimetres. The host owns sensor reads,
    mapper/navigator updates and simulation stepping. `bot` is KavoshRobot:
    motors, stop(), mapper calibration/origin and read_all_sensors() data.
    Navigator is optional for direct points; when present, its known floor-hole
    labels veto unsafe motion. No occupancy map steers the local controller.
    accept_plan()/handle_keys() are optional existing Hero integration helpers.
    The caller must forward revised routes through update_route(), or use the
    existing handle_keys() committed-plan synchronization.

    Result fields include left/right (rad/s), state, reason, target and route
    revision. Inspect complete/blocked; blocked tasks need explicit reissue.
    clearance_report is None unless reports are enabled. Private nested types
    contain the motion and LiDAR implementation; callers need no other class.

    Diagnostics are opt-in: set_debug(reports=True, console=True,
    trace_path='driving.jsonl', map=True). Omitted options disable that feature.
    set_debug() disables all optional diagnostic work and closes its resources.
    The wheel-speed chart is removed. Functional sensor/visibility/progress
    checks and minimal decision flags always run; optional report construction,
    conversion, collection, formatting, drawing and writes do not run disabled.
    The map is an optional click-to-drive tool; Matplotlib and its worker are
    loaded only when map=True. Ordinary driving has no UI or filesystem work.

    Integration contract (all accesses remain inside these adapters):
    - Bot: left_motor/right_motor.getMaxVelocity()/setVelocity(rad/s), stop(),
      drive_from_keys(keys,turbo), timestep(ms), mapper, navigator.
    - Sensor dictionary: time(s), sensor_period_s(s, optional), lidar_raw_m,
      lidar_hres, lidar_fov(rad), lidar(mm, floor guard), distance(cm),
      encoders.left/right(rad), gps_filter_reset/supervisor_relocated(bool).
      orientation.yaw_raw(deg), roll/pitch(rad); position.map_physical_pose_ready,
      map_body_x_float/map_body_y_float(mm), map_position_sigma_mm(optional),
      map_x/map_y or x/y(mm). Body coordinates are translated by mapper origin;
      map Y uses GPS_Y_SIGN. Driving heading is pi/2+radians(yaw_raw).
    - Mapper projection: origin_x/origin_y, FRONT_INDEX, GPS_Y_SIGN,
      _lidar_projection(yaw,data,indices), _lidar_sample_vectors,
      LIDAR_MOUNT_ROTATION, LIDAR_MOUNT_MM.
    - Floor observation adapter: _cs_rgb_hist, _cs_last_tile,
      _committed_floor_tiles, _dist_floor_cm, last_speed, start_tile,
      _is_pit_color, _is_bright_floor, classify_color_sensor, TILE_MM,
      YAW_OFFSET_DEG, YAW_SIGN, DIST_SENSOR_FWD_MM, DIST_SENSOR_DOWN,
      DIST_SENSOR_HEIGHT_CM, DIST_IR_MIN_REFLECTION, DIST_CLIFF_DELTA_CM,
      DIST_CLIFF_STRONG_CM, MIN_RANGE_MM, CS_MAX_SPEED_MMPS,
      CS_MAX_YAW_RATE_DPS, CS_CLIFF_CONFIRM, CS_COMMIT_GOOD_DIST_MM.
      This guard reads measured state; it never writes mapper floor labels.
    - Navigator hole guard: floor, FLOOR_HOLE, NAV_GRID_MM, c. The optional
      committed-plan adapter additionally reads plan_status, plan_raw_points,
      plan_corner_path, _plan_request_id, _plan_preview_owner and
      _plan_source_prefix and _observed_segment_clear for an already committed
      crossing exit. These are audited current-Hero integration details,
      not a portable planner API. Only owned source vertices are removed;
      meaningful connector bends and actual tail revisions remain instructions.
    - Optional map only: nav._used_bbox/_compose_cell_image, cell_state,
      current_beam/current_world_mm; mapper.occupied_geometry(), c, GRID_MM.
    Unproved local entry visibility holds zero wheel commands. Hero does not
    rotate to scan while waiting; valid route-alignment turns remain active.
    Main exchanges mission permission and result state/reason through public
    handle_keys(). It does not read or mutate private driving control state.
    autonomous=True arms following; G/Space/arrows provide explicit takeover.
    Recovery is reverse-only, <=60mm/2s with a 0.5s progress watchdog. A clear
    current raw rear sweep and strict withdrawal from every mapped-hole overlap
    are required. Recovered tasks wait for a fresh committed planner request.

    This packaging preserves the current 34 mm control policy and its known
    limitations; it is not a claim that all possible passage cases are solved.
    """
    import math as _math
    import heapq as _heapq
    import numpy as _np
    from collections import deque as _deque
    from dataclasses import dataclass as _dataclass
    from scipy.spatial import cKDTree as _cKDTree
    from scipy.spatial.distance import cdist as _cdist

    @staticmethod
    def _rotate(points,heading):
        math = HeroDriving._math
        np = HeroDriving._np
        p=np.asarray(points);c,s=math.cos(heading),math.sin(heading)
        return np.stack((p[...,0]*c-p[...,1]*s,p[...,0]*s+p[...,1]*c),axis=-1)

    @staticmethod
    def _capsule_clear(hits, margin, a, b, radius):
        np = HeroDriving._np
        step=b-a
        t=np.clip(np.sum((hits-a)*step,axis=1)/max(float(np.sum(step*step)),1e-20),0,1)
        return bool(np.all(np.sum((hits-a-t[:,None]*step)**2,axis=1) >= (radius+margin)**2))

    @staticmethod
    def _contact_margin(hits,margin,start,radius):
        """Permit a bounded 1mm scrape only at an already adjacent surface.

        Only the tight-attempt caller uses this. Distant/new obstacles retain the
        complete footprint; an overlap beyond 1mm remains a veto. This is the
        caller-authorized contact allowance, not a change to the robot diameter.
        """
        np = HeroDriving._np
        distance=np.linalg.norm(hits-start,axis=1)
        adjacent=(distance>=radius-1.)&(distance<=radius+1.)
        return np.where(adjacent,np.minimum(margin,-1.),margin)

    @staticmethod
    def _point_clear(hits,margin,points,radius):
        """Exact point-disk tests, using nearest distance to skip certain cases."""
        cKDTree = HeroDriving._cKDTree
        cdist = HeroDriving._cdist
        np = HeroDriving._np
        if not len(hits) or not len(points):return np.ones(len(points),bool)
        nearest=cKDTree(hits).query(points)[0]
        safe=nearest>=radius+float(np.max(margin))+1e-9
        ambiguous=(~safe)&(nearest>=radius+float(np.min(margin))-1e-9)
        safe[ambiguous]=np.all(cdist(points[ambiguous],hits,'sqeuclidean')>=(radius+margin)**2,axis=1)
        return safe

    @staticmethod
    def _occluded_corners(groups,polygon):
        """Join observed straight facets only inside a robot-housing shadow.

        The missing corner is positive geometry, never a free-space inference.
        Both facets must support the same nearby forward extension. An ordinary
        doorway/depth discontinuity without self-occlusion cannot seed a corner.
        """
        LidarManeuver = HeroDriving._Maneuver
        math = HeroDriving._math
        np = HeroDriving._np
        prepared=LidarManeuver._prepare_visibility(polygon)
        if prepared is None or len(groups)<2:return np.empty((0,2))
        angles,visible,_,_=prepared;reach=np.linalg.norm(visible,axis=1)
        corners=[]
        for a,b in zip(groups,groups[1:]+groups[:1]):
            if len(a)<3 or len(b)<3:continue
            u=a[-1]-a[0];v=b[-1]-b[0]
            la,lb=np.linalg.norm(u),np.linalg.norm(v)
            if min(la,lb)<2:continue
            u=u/la;v=v/lb
            if max(np.max(abs(np.cross(a-a[0],u))),np.max(abs(np.cross(b-b[0],v))))>.5:continue
            denominator=float(np.cross(u,v))
            if abs(denominator)<.25:continue
            delta=b[0]-a[-1]
            t=float(np.cross(delta,v))/denominator
            s=float(np.cross(delta,u))/denominator
            if not (0<=t<=20 and -20<=s<=0):continue
            p=a[-1]+t*u
            j=np.searchsorted(angles,math.atan2(p[1],p[0]))%len(angles)
            if min(reach[j],reach[(j-1)%len(reach)])<37:corners.append(p)
        return np.asarray(corners).reshape(-1,2)

    @staticmethod
    def _entry_arc(point):
        """Signed travel and curvature of a short tangent arc to a local point."""
        math = HeroDriving._math
        np = HeroDriving._np
        x,y=point;distance2=float(np.dot(point,point))
        if distance2<1e-12:return None
        reverse=x<0
        turn=2*math.atan2(-y if reverse else y,-x if reverse else x)
        if abs(turn)>math.pi/2:return None
        curvature=float(2*y/distance2)
        travel=turn/curvature if abs(curvature)>1e-10 else float(x)
        return travel,curvature

    @staticmethod
    def _arc_distance(points,centre,start,turn):
        """Exact closest distance to a finite circular arc, including endpoints."""
        _rotate = HeroDriving._rotate
        math = HeroDriving._math
        np = HeroDriving._np
        q=points-centre;initial=start-centre
        delta=np.arctan2(initial[0]*q[:,1]-initial[1]*q[:,0],np.sum(q*initial,axis=1))
        delta=delta%(2*math.pi) if turn>=0 else -((-delta)%(2*math.pi))
        on=abs(delta)<=abs(turn)
        end=centre+_rotate(initial,turn)
        return np.where(on,abs(np.linalg.norm(q,axis=1)-np.linalg.norm(initial)),
                        np.minimum(np.linalg.norm(points-start,axis=1),np.linalg.norm(points-end,axis=1)))

    @staticmethod
    def _advancing(points,padding,start,turn):
        """Cells where the moving disk can cover space outside its initial disk.

        Displacement directions of a circular arc span half its heading change.
        A cell wholly behind every displacement cannot add any new swept area;
        its intersection with the sweep is already occupied by the current disk.
        """
        _rotate = HeroDriving._rotate
        math = HeroDriving._math
        np = HeroDriving._np
        p=_rotate(points,-start)
        projection=np.maximum(p[:,0],p[:,0]*math.cos(turn)+p[:,1]*math.sin(turn))
        bearing=np.arctan2(p[:,1],p[:,0])
        directed=bearing%(2*math.pi) if turn>=0 else -((-bearing)%(2*math.pi))
        projection=np.where(abs(directed)<=abs(turn),np.linalg.norm(p,axis=1),projection)
        return projection+padding>0

    @staticmethod
    def _tangent_merge(corner,following):
        """Forward circle from the current tangent onto an outgoing route segment."""
        math = HeroDriving._math
        np = HeroDriving._np
        edge=following-corner;length=float(np.linalg.norm(edge))
        if length<1e-6:return None
        direction=edge/length;turn=math.atan2(direction[1],direction[0])
        if not 1e-5<abs(turn)<=math.pi/2:return None
        shape=np.array([math.sin(turn),1-math.cos(turn)])
        cross=lambda a,b:float(a[0]*b[1]-a[1]*b[0])
        denominator=cross(shape,direction)
        if abs(denominator)<1e-9:return None
        radius=cross(corner,direction)/denominator
        endpoint=radius*shape
        along=float((endpoint-corner)@direction)
        if radius*turn<=0 or abs(radius)<17 or not 0<=along<=length-2:return None
        if not 4<np.linalg.norm(endpoint)<260:return None
        return endpoint

    class _Maneuver:
        def __init__(self, *, reports=True):
            deque = HeroDriving._deque
            math = HeroDriving._math
            self.reports_enabled = bool(reports)
            self.views=deque(maxlen=16)
            self._view_anchor=None
            self.trail=[]
            self.side=0
            self._decision = {}
            if self.reports_enabled:
                self._details = {}
            else:
                self._details = None
            self.curved=False
            self.origin=None
            self.route_axis=None
            self.guidance=[]
            self.failed_curves=[]
            self.failed_preparation_at=None
            self._visibility={}
            self._search=[]
            self._graph_search=None
            self._graph_origin=None
            self._graph_epoch=0
            self._search_decision = {}
            if self.reports_enabled:
                self._search_details = {}
            else:
                self._search_details = None
            self.observed_length=34.
            self.heading_tolerance=math.radians(.15)
            self._obstacle_frame=None
            self._obstacle_data=None
            self.tight_attempt=False
            self.apertures=[]
            self.endpoint=None

        def reset_leg(self):
            self.apertures=[]
            self.endpoint=None
            self.tight_attempt=False
            self._obstacle_frame=None
            self.trail=[];self.side=0;self.curved=False
            self._decision = {}
            if self.reports_enabled:
                self._details = {}
            else:
                self._details = None
            self.origin=self.route_axis=None
            self.guidance=[]
            self.failed_curves=[]
            self.failed_preparation_at=None
            self._search=[]
            self._graph_search=None
            self._graph_origin=None
            self._graph_epoch=0
            self._search_decision = {}
            if self.reports_enabled:
                self._search_details = {}
            else:
                self._search_details = None

        def observe(self,frame):
            math = HeroDriving._math
            polygon=getattr(frame,'free_polygon',None)
            if polygon is None or len(polygon)<32:return
            if self._view_anchor is not None:
                old=self._view_anchor
                moved=math.hypot(frame.x-old.x,frame.y-old.y)
                turn=abs((frame.heading-old.heading+math.pi)%(2*math.pi)-math.pi)
                if moved<4 and turn<math.radians(4):
                    # Refresh this view without evicting other viewing angles.
                    # Stationary search time is not negative sensor evidence.
                    self.views[-1]=frame
                    return
            self.views.append(frame)
            self._view_anchor=frame

        def _obstacles(self,frame):
            """Keep registered raw returns hidden by the known robot housings.

            A self-occluded ray is not negative evidence about a previously seen
            wall. These are recent individual returns, never averaged map cells.
            Current returns always participate, with registration uncertainty added
            only to historical evidence. Cache once per immutable control frame.
            """
            _occluded_corners = HeroDriving._occluded_corners
            _rotate = HeroDriving._rotate
            math = HeroDriving._math
            np = HeroDriving._np
            if self._obstacle_frame is frame:return self._obstacle_data
            hits=[np.asarray(frame.hits)];margins=[np.broadcast_to(frame.hit_margin,(len(frame.hits),))]
            self._live_groups=self._groups(hits[0],2*math.pi/len(frame.free_polygon))
            groups=list(self._live_groups)
            if self.tight_attempt:
                corners=_occluded_corners(self._live_groups,np.asarray(frame.free_polygon))
                if len(corners):
                    hits.append(corners);margins.append(np.zeros(len(corners)))
                    groups.extend([p[None,:] for p in corners])
                    self._live_groups=groups.copy()
            prepared=self._prepare_visibility(np.asarray(frame.free_polygon))
            if prepared is not None:
                angles,polygon,_,_=prepared
                reach=np.linalg.norm(polygon,axis=1)
                for view in self.views:
                    if view is frame or not len(view.hits):continue
                    old=_rotate(_rotate(view.hits,view.heading)+[view.x-frame.x,view.y-frame.y],-frame.heading)
                    bearing=np.arctan2(old[:,1],old[:,0]);j=np.searchsorted(angles,bearing)%len(angles)
                    distance=np.linalg.norm(old,axis=1)
                    hidden=(np.minimum(reach[(j-1)%len(reach)],reach[j])<37)&(distance>37)&(distance<260)
                    if not np.any(hidden):continue
                    reserve=2*math.hypot(frame.position_sigma_mm,view.position_sigma_mm)
                    hits.append(old[hidden]);margins.append(np.broadcast_to(view.hit_margin,(len(view.hits),))[hidden]+reserve)
                    groups.extend(self._groups(old[hidden],2*math.pi/len(frame.free_polygon)))
            self._obstacle_frame=frame
            self._obstacle_regular_margin=np.concatenate(margins)
            if self.tight_attempt:
                # Caller-authorized attempt at nominal physical clearance. Keep
                # every measured obstacle; sampling/registration reserves no
                # longer turn a wider-than-body opening into a hard barrier.
                margins=[np.zeros_like(m) for m in margins]
            self._obstacle_data=(np.concatenate(hits),np.concatenate(margins),groups)
            return self._obstacle_data

        @staticmethod
        def _prepare_visibility(polygon):
            math = HeroDriving._math
            np = HeroDriving._np
            angles=np.arctan2(polygon[:,1],polygon[:,0])
            order=np.argsort(angles);angles=angles[order];polygon=polygon[order]
            keep=np.r_[True,np.diff(angles)>1e-9];angles=angles[keep];polygon=polygon[keep]
            if len(angles)<16:return None
            # A depth discontinuity does not prove the intervening wedge empty.
            # Retain that uncertainty until another actual view covers it.
            j=np.arange(len(angles));a=polygon[(j-1)%len(angles)];b=polygon
            ra,rb=np.linalg.norm(a,axis=1),np.linalg.norm(b,axis=1)
            gap=(angles[j]-angles[(j-1)%len(angles)])%(2*math.pi)
            reach=np.minimum(ra,rb)
            discontinuity=abs(ra-rb)>np.maximum(5,3*np.minimum(ra,rb)*gap)
            if np.any(discontinuity):
                # At a return/no-return edge, a grazing patch does not locate its
                # endpoint. Bound the lost depth by beam footprint / measured
                # incidence, using a coherent three-return patch. Without a fit,
                # use only the nearer-depth free bound and sampling reserve.
                radius=np.linalg.norm(polygon,axis=1);n=len(radius)
                uncertainty=np.full(n,np.inf)
                for shift in (-1,1):
                    p1=np.roll(polygon,-shift,axis=0);p2=np.roll(polygon,-2*shift,axis=0)
                    tangent=p2-polygon;patch_length=np.linalg.norm(tangent,axis=1)
                    normal=np.column_stack((-tangent[:,1],tangent[:,0]))/np.maximum(patch_length[:,None],1e-20)
                    residual=abs(np.sum((p1-polygon)*normal,axis=1))
                    incidence=abs(np.sum(normal*polygon,axis=1))/np.maximum(radius,1e-20)
                    # A flat wall's adjacent returns spread as 1/incidence. A
                    # range-only spacing gate incorrectly labels grazing walls
                    # discontinuous, erasing even their observed near-side space.
                    # Require the same fitted patch, actual angular spacing and
                    # finite incidence before using that footprint bound.
                    a1=abs((np.roll(angles,-shift)-angles+math.pi)%(2*math.pi)-math.pi)
                    a2=np.roll(a1,-shift)
                    r1=np.linalg.norm(p1,axis=1);r2=np.linalg.norm(p2,axis=1)
                    spacing1=np.maximum(radius,r1)*a1/np.maximum(incidence,.02)
                    spacing2=np.maximum(r1,r2)*a2/np.maximum(incidence,.02)
                    coherent=(np.linalg.norm(p1-polygon,axis=1)<=np.maximum(5,3*spacing1)) & (np.linalg.norm(p2-p1,axis=1)<=np.maximum(5,3*spacing2))
                    scale=radius*np.maximum(a1,a2)
                    bound=2*scale/np.maximum(incidence,1e-6)
                    coherent &= (patch_length>1)&(residual<=.5)&(incidence>.02)
                    uncertainty=np.minimum(uncertainty,np.where(coherent,bound,np.inf))
                near=np.where(ra<=rb,(j-1)%n,j)
                # An unfit FAR depth edge cannot erase all nearer free space.
                # Clip the sector before its nearer measured depth, with a full
                # angular-sampling footprint reserve. Never interpolate through
                # the closer return or infer an endpoint from an unsupported fit.
                bound=np.where(np.isfinite(uncertainty[near]),uncertainty[near],np.maximum(ra,rb)*gap)
                reach=np.where(discontinuity,np.maximum(0,np.minimum(ra,rb)-bound),reach)
            return angles,polygon,reach,discontinuity

        @staticmethod
        def _visible_prepared(points,prepared,padding):
            np = HeroDriving._np
            if prepared is None:return np.zeros(len(points),bool)
            angles,polygon,limit,discontinuity=prepared
            bearing=np.arctan2(points[:,1],points[:,0]);j=np.searchsorted(angles,bearing)%len(angles)
            a=polygon[(j-1)%len(angles)];b=polygon[j]
            length=np.linalg.norm(points,axis=1)
            direction=points/np.maximum(length[:,None],1e-20)
            edge=b-a
            denominator=direction[:,0]*edge[:,1]-direction[:,1]*edge[:,0]
            cross=a[:,0]*b[:,1]-a[:,1]*b[:,0]
            reach=np.divide(cross,denominator,out=np.zeros(len(points)),where=abs(denominator)>1e-12)
            valid=(np.linalg.norm(a,axis=1)>1)&(np.linalg.norm(b,axis=1)>1)
            reach=np.where(discontinuity[j],limit[j],np.minimum(reach,limit[j]))
            return valid & (length+padding <= reach)

        @staticmethod
        def _visible(points,polygon,padding):
            LidarManeuver = HeroDriving._Maneuver
            return LidarManeuver._visible_prepared(points,LidarManeuver._prepare_visibility(polygon),padding)

        def _view_visibility(self,frame,points,padding):
            np = HeroDriving._np
            key=id(frame)
            if key not in self._visibility:
                if len(self._visibility)>=18:
                    keep={id(view) for view in self.views}
                    self._visibility={k:v for k,v in self._visibility.items() if k in keep}
                self._visibility[key]=(frame,self._prepare_visibility(np.asarray(frame.free_polygon)))
            return self._visible_prepared(points,self._visibility[key][1],padding)

        def _known(self,frame,points,padding):
            np = HeroDriving._np
            return bool(np.all(self._known_mask(frame,points,padding)))

        def _known_mask(self,frame,points,padding):
            _rotate = HeroDriving._rotate
            math = HeroDriving._math
            np = HeroDriving._np
            known=np.linalg.norm(points,axis=1)<=37-padding
            known |= np.linalg.norm(points-[37,0],axis=1)<=max(0,3-padding)
            # History is deliberately subsampled, but current visibility is not.
            # Requiring registration to an older stored pose discarded usable
            # millimetres in a narrow passage even when this live scan proved it.
            ids=np.flatnonzero(~known)
            if len(ids) and frame.free_polygon is not None:
                known[ids]=self._view_visibility(frame,points[ids],padding)
            world=_rotate(points,frame.heading)+[frame.x,frame.y]
            for view in reversed(self.views):
                if view is frame:continue  # Current scan and body were already tested above.
                ids=np.flatnonzero(~known)
                if not len(ids):break
                local=_rotate(world[ids]-[view.x,view.y],-view.heading)
                registration=2*math.hypot(getattr(frame,'position_sigma_mm',0.),
                                          getattr(view,'position_sigma_mm',0.)) if view is not frame else 0.
                reserve=padding+registration
                # The robot's own previously occupied body footprint is positive
                # free-space evidence too. Camera self-occlusion must not erase it
                # immediately after a curved move. This does not require any
                # destination or travel corridor to have been visited.
                occupied=np.linalg.norm(local,axis=1)<=max(0,37-reserve)
                occupied |= np.linalg.norm(local-[37,0],axis=1)<=max(0,3-reserve)
                known[ids]=occupied | self._view_visibility(view,local,reserve)
            return known

        def _sweep_known(self,frame,lower,upper,inside):
            """Prove coarse cells once; subdivide only their unproved portions.

            Each accepted sample proves its entire cell through half-diagonal
            erosion. Refinement never interpolates across an occlusion wedge.
            """
            math = HeroDriving._math
            np = HeroDriving._np
            grid=2.;half=grid/math.sqrt(2)
            lo=np.floor((lower-half)/grid).astype(int)
            hi=np.ceil((upper+half)/grid).astype(int)
            xx,yy=np.meshgrid(np.arange(lo[0],hi[0]+1)*grid,np.arange(lo[1],hi[1]+1)*grid)
            points=np.column_stack((xx.ravel(),yy.ravel()))
            points=points[inside(points,half)]
            known=self._known_mask(frame,points,half)
            if np.all(known):return True
            # Four by four half-millimetre cells exactly cover each 2mm cell.
            x,y=np.meshgrid(np.arange(4)*.5-.75,np.arange(4)*.5-.75)
            offsets=np.column_stack((x.ravel(),y.ravel()))
            fine=(points[~known,None,:]+offsets).reshape(-1,2)
            half=.5/math.sqrt(2)
            fine=fine[inside(fine,half)]
            known=self._known_mask(frame,fine,half)
            if self.tight_attempt and not np.all(known):
                # Contact permission is not a declaration of free floor. A camera
                # housing can hide a sub-millimetre side fringe of a forward curve.
                # Permit only the existing 1mm contact budget outside the currently
                # occupied chassis, never a forward occlusion or wider unseen area.
                ids=np.flatnonzero(~known);points=fine[ids]
                prepared=self._prepare_visibility(np.asarray(frame.free_polygon))
                if prepared is not None:
                    angles,polygon,_,_=prepared
                    j=np.searchsorted(angles,np.arctan2(points[:,1],points[:,0]))%len(angles)
                    reach=np.linalg.norm(polygon,axis=1)
                    shadow=np.minimum(reach[j],reach[(j-1)%len(reach)])<37
                    fringe=np.linalg.norm(points,axis=1)+half-37<=1.
                    side=abs(points[:,1])>abs(points[:,0])
                    known[ids]=shadow & fringe & side
            return bool(np.all(known))

        def clear_entry(self,frame,point,clearance,observed_length=None):
            """Continuous body/device sweep plus observed swept-body floor area.

            The disk raster uses half-millimetre cells with a half-diagonal bound;
            visibility erodes each cell too. Current raw hits veto older free views.
            """
            _advancing = HeroDriving._advancing
            _arc_distance = HeroDriving._arc_distance
            _capsule_clear = HeroDriving._capsule_clear
            _contact_margin = HeroDriving._contact_margin
            math = HeroDriving._math
            np = HeroDriving._np
            hits,margin,_=self._obstacles(frame)
            length=float(np.linalg.norm(point))
            if length<1e-6 or length>260:return False
            travel=math.atan2(point[1],point[0])
            heading=travel
            delta=(heading+math.pi)%(2*math.pi)-math.pi
            device_margin=_contact_margin(hits,margin,np.array([37.,0.]),3.) if self.tight_attempt else margin
            body_margin=_contact_margin(hits,margin,np.zeros(2),37.) if self.tight_attempt else margin
            if np.any(_arc_distance(hits,np.zeros(2),np.array([37.,0.]),delta)<3+clearance+device_margin-1e-8):return False
            if observed_length==0:return True  # Rotation only; translation is not authorized.
            if not _capsule_clear(hits,body_margin-1e-8,np.zeros(2),point,37+clearance):return False
            forward=37*np.array([math.cos(heading),math.sin(heading)])
            if not _capsule_clear(hits,margin,forward,point+forward,3+clearance):return False
            if observed_length is not None:
                point=point*min(1.,observed_length/length)
                length=float(np.linalg.norm(point))
            def inside(points,padding):
                along=np.clip(np.sum(points*point,axis=1)/max(length**2,1e-20),0,1)
                # Clearance is a raw collision reserve, not unseen extra floor
                # behind a robot moving away from it. Prove the new physical
                # footprint; the initial disk is positive occupied-space evidence.
                return ((np.linalg.norm(points-along[:,None]*point,axis=1)<=37+padding)
                        & _advancing(points,padding,heading,0.))
            return self._sweep_known(frame,np.minimum(0,point)-37-clearance,
                                     np.maximum(0,point)+37+clearance,inside)

        def clear_arc(self,frame,point,clearance,observed_length=None):
            """Prove the complete tangent arc, including the front device.

            Exact finite arcs bound raw-hit collisions. A conservative raster of the
            exact circular swept region must be covered by registered raw views.
            No obstacle/map truth or visited-space mask participates.
            """
            _advancing = HeroDriving._advancing
            _arc_distance = HeroDriving._arc_distance
            _contact_margin = HeroDriving._contact_margin
            _entry_arc = HeroDriving._entry_arc
            math = HeroDriving._math
            np = HeroDriving._np
            arc=_entry_arc(point)
            if arc is None:return False
            travel,curvature=arc
            if abs(curvature)<1e-8:return self.clear_entry(frame,point,clearance,observed_length)
            turn=travel*curvature;radius=1/curvature;centre=np.array([0.,radius])
            hits,margin,_=self._obstacles(frame)
            for start,r in ((np.zeros(2),37.),(np.array([37.,0.]),3.)):
                m=_contact_margin(hits,margin,start,r) if self.tight_attempt else margin
                if np.any(_arc_distance(hits,centre,start,turn)<r+clearance+m-1e-8):return False
            if observed_length is not None:turn*=min(1.,observed_length/abs(travel))
            count=max(2,math.ceil((abs(radius)+40)*abs(turn)/2))
            yaw=np.linspace(0,turn,count+1)
            centres=np.column_stack((radius*np.sin(yaw),radius*(1-np.cos(yaw))))
            sensor=centres+37*np.column_stack((np.cos(yaw),np.sin(yaw)))
            all_path=np.concatenate((centres,sensor))
            def inside(points,padding):
                body=_arc_distance(points,centre,np.zeros(2),turn)
                device=_arc_distance(points,centre,np.array([37.,0.]),turn)
                direction=0. if travel>0 else math.pi
                front=math.atan2(37*curvature,1)+direction
                return ((body<=37+padding)&_advancing(points,padding,direction,turn/2)
                        |(device<=3+padding)&_advancing(points-[37,0],padding,front,turn/2))
            inflation=(abs(radius)+40)*(turn/count)**2/8
            return self._sweep_known(frame,all_path.min(axis=0)-37-clearance-inflation,
                                     all_path.max(axis=0)+37+clearance+inflation,inside)

        @staticmethod
        def _groups(hits,angular_step=0.01227184630308513):
            # Even one valid return can seed a safe approach proposal. Discarding
            # a short facet here removed the far corner of thin walls and made the
            # local graph disconnected. Fitting confidence belongs to the fit,
            # while all proposed motions still need full raw swept validation.
            math = HeroDriving._math
            np = HeroDriving._np
            if len(hits)<3:return [np.asarray(hits)] if len(hits) else []
            order=np.argsort(np.arctan2(hits[:,1],hits[:,0]));p=hits[order]
            ranges=np.linalg.norm(p,axis=1)
            bearings=np.unwrap(np.arctan2(p[:,1],p[:,0]))
            spacing=np.maximum(ranges[1:],ranges[:-1])*np.diff(bearings)
            # The same incidence effect applies when grouping a long corridor
            # side. Preserve coherent grazing triples instead of fragmenting one
            # measured wall into a sequence of misleading artificial corners.
            tangent=p[2:]-p[:-2];length=np.linalg.norm(tangent,axis=1)
            normal=np.column_stack((-tangent[:,1],tangent[:,0]))/np.maximum(length[:,None],1e-20)
            residual=abs(np.sum((p[1:-1]-p[:-2])*normal,axis=1))
            incidence=abs(np.sum(normal*p[1:-1],axis=1))/np.maximum(ranges[1:-1],1e-20)
            flat=(length>1)&(residual<=.5)&(incidence>.02)
            factor=np.where(flat,1/np.maximum(incidence,.02),1.)
            edge_factor=np.maximum(np.r_[factor,1.],np.r_[1.,factor])
            steps=np.diff(p,axis=0);step_length=np.linalg.norm(steps,axis=1)
            cosine=np.sum(steps[:-1]*steps[1:],axis=1)/np.maximum(step_length[:-1]*step_length[1:],1e-20)
            corners=(cosine<math.cos(math.pi/6))&(np.minimum(step_length[:-1],step_length[1:])>.5)
            split=(np.diff(bearings)>2.5*angular_step)|(step_length>np.maximum(5,3*spacing*edge_factor))
            split[:-1] |= corners
            cuts=np.flatnonzero(split)+1
            return [g for g in np.split(p,cuts) if len(g)]

        def advance_guidance(self,frame):
            np = HeroDriving._np
            while self.guidance:
                displacement=np.array([frame.x,frame.y])-self.guidance[0]
                # These are temporary staging regions, not public bending nodes.
                # A guide may differ from the selected approach by the 2mm merge
                # tolerance, and that approach has its own 2mm arrival tolerance.
                if np.linalg.norm(displacement)<=4+2*frame.position_sigma_mm:self.guidance.pop(0)
                else:break

        def propose(self,frame,target,clearance):
            """Choose a connected, bounded route around measured surfaces.

            Graph edges describe intent only. The first complete swept primitive
            must be observed before execution; later edges are revalidated from
            their actual pose. A nearby aperture is never itself a destination.
            """
            _capsule_clear = HeroDriving._capsule_clear
            _point_clear = HeroDriving._point_clear
            _rotate = HeroDriving._rotate
            cKDTree = HeroDriving._cKDTree
            math = HeroDriving._math
            np = HeroDriving._np
            self.observe(frame)
            hits,margin,_=self._obstacles(frame)
            goal=_rotate(np.asarray(target)-[frame.x,frame.y],-frame.heading)
            # Reject an observed endpoint intrusion greater than the complete
            # arrival region. Searching/observation cannot make that body fit.
            if len(hits) and np.min(np.linalg.norm(hits-goal,axis=1))<37-5.:
                self._decision = dict(reason='destination_footprint_obstructed', planner_replan_required=True, destination_rejected=True)
                if self.reports_enabled:
                    self._details = dict(reason='destination_footprint_obstructed', planner_replan_required=True, destination_unreachable=False, destination_rejected=True, endpoint_region_mm=5.0)
                return None
            # Project only inside the 5mm goal region; retain the public target.
            endpoint=goal.copy()
            for _ in range(8):
                delta=endpoint-hits
                distances=np.linalg.norm(delta,axis=1)
                deficit=37+clearance+margin+.05-distances
                if not len(deficit) or float(deficit.max())<=0:break
                j=int(np.argmax(deficit))
                endpoint+=delta[j]/max(distances[j],1e-9)*deficit[j]
            if (np.linalg.norm(endpoint-goal)<=5.
                    and _point_clear(hits,margin,endpoint[None,:],37+clearance)[0]):
                self.endpoint=_rotate(endpoint,frame.heading)+[frame.x,frame.y]
            else:self.endpoint=np.asarray(target)
            self.destination=self.endpoint
            if self._graph_search is not None:return self._continue_graph(frame,clearance)
            if self._search:return self._try_approaches(frame,clearance)
            position=np.array([frame.x,frame.y])
            goal=_rotate(self.destination-position,-frame.heading)
            if self.origin is None:
                self.origin=position.copy()
                direction=np.asarray(target)-self.origin
                self.route_axis=direction/max(np.linalg.norm(direction),1e-12)
            route_normal=np.array([-self.route_axis[1],self.route_axis[0]])
            hits,margin,groups=self._obstacles(frame)
            self.advance_guidance(frame)
            if self.guidance:
                # Keep a proved intent through changing views, but do not persist
                # a route whose continuation now intersects a measured obstacle.
                points=_rotate(np.asarray(self.guidance)-position,-frame.heading)
                chain=np.vstack((np.zeros(2),points,goal))
                if all(_capsule_clear(hits,margin,a,b,37+clearance) for a,b in zip(chain,chain[1:])):
                    self._search=[(self.guidance[0],self.side,list(self.guidance))]
                    selected=self._try_approaches(frame,clearance)
                    if selected is not None:return selected
                self.guidance=[]
            candidates=[]
            if self.reports_enabled:
                widths=[];narrow=[]
            self.apertures=[]
            local_axis=_rotate(self.route_axis,-frame.heading)
            across=np.array([-local_axis[1],local_axis[0]])
            ring=44*np.array([[math.cos(a),math.sin(a)]for a in np.arange(16)*math.pi/8])
            if np.linalg.norm(self.destination-np.asarray(target))>1e-6:
                # A body-feasible endpoint beside a wall may need a tangential
                # final approach so its forward device can fit too.
                candidates.extend(goal+ring)
            for group in groups:
                for endpoint in ((group[0],) if len(group)==1 else (group[0],group[-1])):
                    if np.linalg.norm(endpoint)>240:continue
                    # Offset the measured edge, not a map cell. Sixteen directions
                    # leave enough chord clearance around the circular body.
                    candidates.extend(endpoint+ring)
            # Aperture measurements must come from ONE live scan. Combining
            # registered patches from different poses manufactured thousands of
            # duplicate pairings and mixed their registration errors into widths.
            # Historical points remain collision vetoes and corner proposals above.
            groups=self._live_groups
            # Reject impossible portal pairs with exact lower/upper bounds before
            # building their full point-distance matrix. Duplicate raw patches
            # overlap; they cannot have a closest gap greater than 20mm.
            sample=np.array([g[[0,len(g)//2,-1]]for g in groups])
            if len(groups):
                distances=np.sum((sample[:,None,:,None,:]-sample[None,:,None,:,:])**2,axis=-1)
                near=np.min(distances,axis=(2,3))<=20**2
                lower=np.array([g.min(axis=0)for g in groups]);upper=np.array([g.max(axis=0)for g in groups])
                separation=np.maximum(np.maximum(lower[:,None]-upper[None,:],lower[None,:]-upper[:,None]),0)
                possible=(~near)&(np.sum(separation*separation,axis=2)<120**2)
                pairs=np.argwhere(np.triu(possible,1))
            else:pairs=[]
            for i,j in pairs:
                    a,b=groups[i],groups[j]
                    d2=np.sum((a[:,None,:]-b[None,:,:])**2,axis=2)
                    ia,ib=np.unravel_index(np.argmin(d2),d2.shape)
                    pa,pb=a[ia],b[ib];gap=float(np.linalg.norm(pb-pa))
                    mid=(pa+pb)/2
                    if not 20<gap<120 or np.linalg.norm(mid)>240:continue
                    if self.reports_enabled:
                        widths.append(gap)
                    axis=np.array([-(pb-pa)[1],(pb-pa)[0]])/gap
                    # Separated returns across a housing shadow are not a door.
                    # Its centre must be live-observed free or already under the
                    # physical body, before relaxing any collision reserve.
                    observed_mid=(np.linalg.norm(mid)<=37 or self._view_visibility(frame,mid[None,:],.25)[0])
                    if gap>74 and observed_mid:
                        self.apertures.append((_rotate(mid,frame.heading)+position,
                                               _rotate(axis,frame.heading),gap))
                    # A long parallel passage needs a centre STRIP, not one
                    # midpoint between staggered leading samples. Retain critical
                    # cross-sections at both observed wall ends; the connected
                    # search decides where it can safely enter and leave.
                    directions=[]
                    for group in (a,b):
                        if len(group)<3:break
                        tangent=group[-1]-group[0];span=float(np.linalg.norm(tangent))
                        if span<=4:break
                        tangent=tangent/span
                        if np.max(abs((group-group[0])[:,0]*tangent[1]-(group-group[0])[:,1]*tangent[0]))>.5:break
                        if directions and tangent@directions[0]<0:tangent=-tangent
                        directions.append(tangent)
                    if len(directions)==2 and directions[0]@directions[1]>math.cos(math.pi/12):
                        wall_axis=directions[0]+directions[1];wall_axis/=np.linalg.norm(wall_axis)
                        wall_normal=np.array([-wall_axis[1],wall_axis[0]])
                        wall_sides=[float(np.median(np.sum(group*wall_normal,axis=1)))for group in (a,b)]
                        if 74+2*clearance<abs(wall_sides[1]-wall_sides[0])<120:
                            strip_centre=sum(wall_sides)/2
                            for group in (a,b):
                                ends=np.sum(group*wall_axis,axis=1)
                                for end in (float(ends.min()),float(ends.max())):
                                    candidates.extend((wall_axis*(end+offset)+wall_normal*strip_centre for offset in (-44.,0.,44.)))
                    # Only a gap crossing the required travel strip contributes
                    # a narrow-entry diagnostic. Other sampled pairs are scenery.
                    if self.reports_enabled:
                        lateral=sorted((float(pa@across),float(pb@across)))
                        if (lateral[0]<=0<=lateral[1] and mid@local_axis>0
                                and abs(axis@local_axis)>.8 and gap+float(np.max(margin))*2<74):
                            narrow.append(gap)
                    if gap<74+2*clearance:continue
                    candidates.append(mid)
                    candidates.extend((mid-axis*44,mid+axis*44))
            # Include the accepted line as a rejoin, even when the public goal is
            # farther than the one-scan local approach horizon.
            candidates.append(goal*min(1.,200/max(np.linalg.norm(goal),1e-12)))
            # A proved lateral preparation may be needed before advancing into a
            # tight offset entrance. Keep both corridor-aligned components in the
            # same connected search, rather than repeatedly chasing a combined
            # forward/sideways point whose first swept footprint is not observable.
            candidates=np.asarray(candidates)
            _,first=np.unique(candidates,axis=0,return_index=True)
            candidates=candidates[np.sort(first)]
            components=np.sum(candidates*local_axis,axis=1)[:,None]*local_axis
            candidates=np.concatenate((candidates,np.stack((components,candidates-components),axis=1).reshape(-1,2)))
            nodes=[np.zeros(2),goal]
            worlds=[position,self.destination]
            sides=[0,0]
            candidate_world=_rotate(candidates,frame.heading)+position
            deviations=np.sum((candidate_world-self.origin)*route_normal,axis=1)
            along=np.sum((candidate_world-self.origin)*self.route_axis,axis=1)
            lengths=np.sum(candidates*candidates,axis=1)
            eligible=(lengths>4)&(lengths<260**2)&(abs(deviations)<=74)&(along>=-37)
            ids=np.flatnonzero(eligible)
            bins={};reserved=[]
            def reserve(p):
                key=(math.floor(p[0]/2),math.floor(p[1]/2))
                point=(float(p[0]),float(p[1]))
                bins.setdefault(key,[]).append(point);reserved.append(point)
            for p in nodes:reserve(p)
            for p in _rotate(np.asarray(self.trail).reshape(-1,2)-position,-frame.heading):reserve(p)
            tree=None;size=0
            for first in range(0,len(ids),512):
                part=ids[first:first+512]
                if len(reserved)!=size:tree=cKDTree(reserved);size=len(reserved)
                # These proposals would be discarded by the existing 2mm merge
                # anyway. Remove proved duplicates BEFORE expensive raw-hit disk
                # checks. Keep boundary cases for the unchanged exact merge test.
                part=part[tree.query(candidates[part])[0]>=2-1e-10]
                safe=_point_clear(hits,margin,candidates[part],37+clearance)
                for index in part[safe]:
                    p=candidates[index];world=candidate_world[index];deviation=deviations[index]
                    side=0 if abs(deviation)<=max(1.,2*frame.position_sigma_mm) else math.copysign(1,deviation)
                    px,py=float(p[0]),float(p[1])
                    bx,by=math.floor(px/2),math.floor(py/2)
                    if any((px-x)*(px-x)+(py-y)*(py-y)<4
                           for dx in (-1,0,1)for dy in (-1,0,1)
                           for x,y in bins.get((bx+dx,by+dy),())):continue
                    reserve(p)
                    nodes.append(p);worlds.append(world);sides.append(side)
            nodes=np.asarray(nodes);n=len(nodes)
            self._search_decision = dict(reason='local_maneuver_unresolved', planner_replan_required=False)
            if self.reports_enabled:
                self._search_details = dict(reason='local_maneuver_unresolved', robot_body_diameter_mm=74, measured_gap_widths_mm=widths, observed_pairs_narrower_than_body_mm=narrow, planner_replan_required=False, destination_unreachable=False, detail='No connected observed local approach proved; this does not prove the destination globally unreachable.')
            self._graph_origin=(position.copy(),frame.heading)
            self._graph_pose=(position.copy(),frame.heading)
            self._graph_nodes,self._graph_worlds=nodes,worlds
            self._graph_epoch=0
            self._graph_search=self._routes(nodes,worlds,hits,margin,clearance)
            return self._continue_graph(frame,clearance)

        def _routes(self,nodes,worlds,hits,margin,clearance):
            """Lazy complete visibility graph: compute only expanded edge rows.

            Yield after each numeric row so a difficult proof cannot monopolize
            the sensor/mapping loop. Every returned primitive is separately checked
            against the latest live frame before execution; graph edges are intent.
            """
            _contact_margin = HeroDriving._contact_margin
            cKDTree = HeroDriving._cKDTree
            heapq = HeroDriving._heapq
            math = HeroDriving._math
            np = HeroDriving._np
            n=len(nodes);rows={};forbidden=set();cached_epoch=-1
            tree=cKDTree(hits) if len(hits) else None
            for _ in range(n-1):
                # Reuse static edge rows, but search from the moving body rather
                # than from the pose where a multi-frame search first started.
                epoch=self._graph_epoch
                if epoch!=cached_epoch:
                    rows.pop(0,None);forbidden.clear();cached_epoch=epoch
                heuristic=np.linalg.norm(nodes-nodes[1],axis=1)
                distance=np.full(n,np.inf);distance[0]=0
                parent=np.full(n,-1,int);heap=[(float(heuristic[0]),0)]
                while heap:
                    estimate,i=heapq.heappop(heap)
                    if estimate>distance[i]+heuristic[i]+1e-8:continue
                    if i==1:
                        path=[i]
                        while path[-1]!=0:path.append(int(parent[path[-1]]))
                        path=path[::-1]
                        yield ('route',[worlds[j]for j in path[1:]])
                        if epoch!=self._graph_epoch:forbidden.clear();break
                        forbidden.add(path[1]);break
                    if i not in rows:
                        start=nodes[i].copy();edges=nodes-start
                        length2=np.sum(edges*edges,axis=1)
                        safe=np.ones(n,bool);reserve=np.full(n,100.)
                        m=_contact_margin(hits,margin,nodes[0],37.) if self.tight_attempt and i==0 else margin
                        todo=np.arange(n)
                        if len(hits):
                            # Nearest-distance is 1-Lipschitz. Samples either prove
                            # a collision, or prove >=6mm surplus throughout the
                            # edge (whose existing clearance cost is exactly zero).
                            # All ambiguous edges retain the exact raw predicate.
                            steps=np.maximum(1,np.ceil(np.sqrt(length2)/8).astype(int))
                            count=steps+1;offset=np.r_[0,np.cumsum(count)[:-1]]
                            edge_index=np.repeat(np.arange(n),count)
                            sample=np.arange(int(count.sum()))-np.repeat(offset,count)
                            points=start+edges[edge_index]*(sample/steps[edge_index])[:,None]
                            nearest=np.minimum.reduceat(tree.query(points)[0],offset)
                            blocked=nearest<37+clearance+float(np.min(m))-1e-8
                            certain=nearest-np.sqrt(length2)/(2*steps)>=37+clearance+max(float(np.max(m)),float(np.max(margin)))+6
                            safe[blocked]=False;reserve[certain]=6.
                            todo=np.flatnonzero(~blocked & ~certain)
                            reserve[todo]=np.inf
                        yield ('work',max(n,len(points)*(math.ceil(math.log2(max(2,len(hits))))+16)) if len(hits) else n)
                        # Bound dense work without discarding or averaging any raw
                        # return. A fixed slice count preserves exact input replay.
                        chunk=max(1,131072//max(1,len(todo)))
                        for first in range(0,len(hits),chunk) if len(todo) else ():
                            part=slice(first,first+chunk);delta=hits[part]-start
                            dot=np.einsum('ij,kj->ik',edges[todo],delta)
                            t=np.clip(dot/np.maximum(length2[todo,None],1e-20),0,1)
                            squared=np.sum(delta*delta,axis=1)-2*t*dot+t*t*length2[todo,None]-1e-8
                            safe[todo] &= np.all(squared>=(37+clearance+m[part]-1e-8)**2,axis=1)
                            reserve[todo]=np.minimum(reserve[todo],np.min(np.sqrt(np.maximum(0,squared))-37-clearance-margin[part],axis=1))
                            yield ('work',len(todo)*len(hits[part]))
                        if not len(hits):reserve[:]=100.
                        value=np.sqrt(length2)+6+24*np.clip(1-reserve/6,0,1)**2
                        rows[i]=np.where(safe,value,np.inf);rows[i][i]=np.inf
                        if epoch!=self._graph_epoch:forbidden.clear();break
                    cost=rows[i].copy()
                    incoming=(self._graph_pose[1]-self._graph_origin[1] if i==0 else math.atan2(*(nodes[i]-nodes[parent[i]])[::-1]))
                    departure=np.arctan2(nodes[:,1]-nodes[i,1],nodes[:,0]-nodes[i,0])
                    cost+=18*abs((departure-incoming+math.pi)%(2*math.pi)-math.pi)
                    cost[0]=np.inf
                    if i==0 and forbidden:cost[list(forbidden)]=np.inf
                    trial=distance[i]+cost
                    for j in np.flatnonzero(trial<distance):
                        distance[j]=trial[j];parent[j]=i
                        heapq.heappush(heap,(float(trial[j]+heuristic[j]),int(j)))
                else:return

        def _continue_graph(self,frame,clearance):
            _rotate = HeroDriving._rotate
            math = HeroDriving._math
            np = HeroDriving._np
            if self._graph_origin is not None:
                position=np.array([frame.x,frame.y]);old,heading=self._graph_pose
                turn=abs((frame.heading-heading+math.pi)%(2*math.pi)-math.pi)
                if np.linalg.norm(position-old)+37*turn>self.observed_length:
                    origin,initial_heading=self._graph_origin
                    self._graph_nodes[0]=_rotate(position-origin,-initial_heading)
                    self._graph_worlds[0]=position
                    self._graph_pose=(position,frame.heading)
                    self._graph_epoch+=1
            # Fixed slices keep raw replay deterministic; wall-clock deadlines
            # would make steering decisions depend on other applications' load.
            work=0;cost=0
            while work<64 and cost<4*131072:
                try:kind,value=next(self._graph_search)
                except StopIteration:
                    self._graph_search=None
                    self._decision = self._search_decision.copy()
                    if self.reports_enabled:
                        self._details = self._search_details.copy()
                    return None
                work+=1
                cost+=2*131072 if kind=='route' else value
                if kind=='route':
                    world=value[0];route=value[:-1]
                    self._search=[(world,0,route)]
                    selected=self._try_approaches(frame,clearance)
                    if selected is not None:
                        self._graph_search=None
                        return selected
            self._decision = {**self._search_decision, 'reason': 'entry_search_pending'}
            if self.reports_enabled:
                self._details = {**self._search_details, 'reason': 'entry_search_pending'}
            return None

        def _try_approaches(self,frame,clearance):
            _entry_arc = HeroDriving._entry_arc
            _rotate = HeroDriving._rotate
            _tangent_merge = HeroDriving._tangent_merge
            math = HeroDriving._math
            np = HeroDriving._np
            for _ in range(min(2,len(self._search))):
                world,side,route=self._search.pop(0)
                # Round a connected corner by matching its departure tangent.
                # Independent arcs through polygon vertices oversteer the next
                # chord and create alternating left/right corrections.
                chain=list(route)+[self.destination]
                for index in range(min(2,len(chain)-1)):
                    corner,following=_rotate(np.asarray(chain[index:index+2])-[frame.x,frame.y],-frame.heading)
                    merged=_tangent_merge(corner,following)
                    if merged is not None and self.clear_arc(frame,merged,clearance,observed_length=self.observed_length):
                        world=_rotate(merged,frame.heading)+[frame.x,frame.y]
                        route=[world]+list(route[index+1:])
                        self._search=[];self.curved=True;self.guidance=route
                        self.trail.append(world.copy())
                        self._decision = dict(reason='observed_local_approach', planner_replan_required=False)
                        if self.reports_enabled:
                            self._details = dict(reason='observed_local_approach', point_mm=world.tolist(), curved=True, tangent_merge=True, entry_guidance_mm=[g.tolist() for g in route], observed_prefix_mm=min(self.observed_length, float(np.linalg.norm(merged))), continuation_requires_live_validation=True, planner_replan_required=False, destination_unreachable=False)
                        return tuple(map(float,world))
                p=_rotate(world-[frame.x,frame.y],-frame.heading)
                arc=_entry_arc(p)
                compatible=True
                if arc is not None and route:
                    following=route[1] if len(route)>1 else self.destination
                    departure=_rotate(following-world,-frame.heading)
                    turn=arc[0]*arc[1]
                    correction=(math.atan2(departure[1],departure[0])-turn+math.pi)%(2*math.pi)-math.pi
                    compatible=turn*correction>=-abs(turn)*math.radians(.15)
                # Forward curves only. A point behind the front sensor requires a
                # proved in-place turn followed by forward translation.
                curved=(compatible and arc is not None and arc[0]>0 and abs(arc[1])*17<=1
                        and not any(np.linalg.norm(world-old)<2 for old in self.failed_curves)
                        and self.clear_arc(frame,p,clearance,observed_length=self.observed_length))
                straight=self.clear_entry(frame,p,clearance,observed_length=self.observed_length) if not curved else False
                alignment_only=(not curved and not straight
                    # A preparation that still exposed no safe travel needs a
                    # new observation, not another tiny alignment at the same
                    # body pose. Actual proved curves/straight motions remain
                    # eligible; moving beyond the guarded prefix permits retry.
                    and (self.failed_preparation_at is None
                         or math.dist((frame.x,frame.y),self.failed_preparation_at)>self.observed_length)
                    and abs(math.atan2(p[1],p[0]))>self.heading_tolerance
                    and self.clear_entry(frame,p,clearance,observed_length=0.))
                if curved or straight or alignment_only:
                    self._search=[];self.side=self.side or side;self.curved=curved
                    self.guidance=list(route)
                    self.trail.append(world.copy())
                    self._decision = dict(reason='observed_local_approach', alignment_only_until_live_sweep_proved=alignment_only, planner_replan_required=False)
                    if self.reports_enabled:
                        self._details = dict(reason='observed_local_approach', point_mm=world.tolist(), curved=curved, measured_gap_widths_mm=self._search_details.get('measured_gap_widths_mm', []), entry_guidance_mm=[g.tolist() for g in self.guidance], observed_prefix_mm=0.0 if alignment_only else min(self.observed_length, float(np.linalg.norm(p))), alignment_only_until_live_sweep_proved=alignment_only, continuation_requires_live_validation=True, planner_replan_required=False, destination_unreachable=False)
                    return tuple(map(float,world))
            self._decision = {**self._search_decision, 'reason': 'entry_search_pending' if self._search else 'local_maneuver_unresolved'}
            if self.reports_enabled:
                self._details = {**self._search_details, 'reason': 'entry_search_pending' if self._search else 'local_maneuver_unresolved'}
            if self._search:self._decision['planner_replan_required']=False
            return None

        @property
        def report(self):
            return self._details if self.reports_enabled else self._decision

        @report.setter
        def report(self, value):
            self._decision = {key: item for key, item in value.items() if key in (
                'reason', 'planner_replan_required', 'destination_rejected',
                'alignment_only_until_live_sweep_proved')}
            if self.reports_enabled:
                self._details = value

    @staticmethod
    def _angle(value):
        math = HeroDriving._math
        return (value + math.pi) % (2 * math.pi) - math.pi

    @staticmethod
    def _interval_distance(point,previous,frame):
        """Closest measured passage during the completed sensor interval."""
        math = HeroDriving._math
        dx,dy=frame.x-previous.x,frame.y-previous.y
        t=max(0.,min(1.,((point[0]-previous.x)*dx+(point[1]-previous.y)*dy)/max(dx*dx+dy*dy,1e-20)))
        return math.hypot(previous.x+t*dx-point[0],previous.y+t*dy-point[1])

    @_dataclass(frozen=True)
    class _Frame:
        time: float
        x: float
        y: float
        heading: float
        left_encoder: float
        right_encoder: float
        hits: object
        scan_valid: bool = True
        # Per-return beam/measurement uncertainty, not an occupancy-cell radius.
        hit_margin: object = 0.5
        relocated: bool = False
        free_polygon: object = None
        position_sigma_mm: float = 0.

    @_dataclass(frozen=True)
    class _Command:
        left: float = 0.0
        right: float = 0.0
        state: str = "idle"
        reason: str = ""
        target: object = None
        remaining_mm: float = 0.0
        lateral_mm: float = 0.0
        offset_mm: float = 0.0
        route_revision: int = 0
        pending_revision: object = None
        local_target: object = None
        clearance_report: object = None
        started: bool = False

    class _Controller:
        """Nonblocking rotate/translate controller; call update once per sensor step.

        set_route starts an explicit new task; offer_route reconciles newer remaining
        destinations without restarting a compatible leg. Revisions are task-local.
        Blocked is latched until a caller explicitly retries with set_route().
        The 37 mm body and 37+3 mm forward device match custom_robot.proto.
        Speed constants are calibrated inputs, not assumptions of zero inertia.
        Arrival adds at most one tolerance of allowance for twice the reported GPS
        sigma; the destination itself is unchanged. A lateral endpoint miss re-aims
        at that point instead of stalling with zero forward projection.
        """
        # Direct control fixtures retain the transfer's inspection behavior;
        # the Hero adapter disables unsolicited observation turns below.
        observation_turns = True

        def __init__(self, *, wheel_radius_mm=20.0, axle_mm=34.0,
                     max_wheel_speed=6.28, position_tolerance_mm=1.0,
                     heading_tolerance_deg=0.15, clearance_mm=0.5,
                     allow_maneuvers=True, centering=True,
                     curved_approach=False, reports=True):
            DriveCommand = HeroDriving._Command
            LidarManeuver = HeroDriving._Maneuver
            deque = HeroDriving._deque
            math = HeroDriving._math
            self.reports_enabled = bool(reports)
            values = (wheel_radius_mm, axle_mm, max_wheel_speed,
                      position_tolerance_mm, heading_tolerance_deg)
            if (not all(math.isfinite(v) and v > 0 for v in values)
                    or not math.isfinite(clearance_mm) or clearance_mm < 0):
                raise ValueError("Geometry/tolerances must be positive; clearance finite and nonnegative")
            self.radius, self.axle = wheel_radius_mm, axle_mm
            self.max_speed = min(max_wheel_speed, 6.28)
            self.tolerance = position_tolerance_mm
            self.heading_tolerance = math.radians(heading_tolerance_deg)
            self.clearance = clearance_mm
            self._normal_clearance = clearance_mm
            self.allow_maneuvers = allow_maneuvers
            self.centering = centering
            self.curved_approach=curved_approach
            self._arc_guard=None
            self.queue = deque()
            self.state = "idle"
            self.reason = ""
            self.target = None
            self.start = None
            self.direction = None
            self.offset = 0.0
            self._last = None
            self._last_command = DriveCommand()
            self._progress = None
            self._anchor = None
            self._rotation_target = None
            self._rotation_progress = None
            self._unwrapped_heading = None
            self.route_revision = self._seen_revision = 0
            self._pending_route = None
            self._local = LidarManeuver(reports=self.reports_enabled) if allow_maneuvers else None
            self._local_driver = None
            self._observe_heading = None
            self._observed_for_leg = False
            self._observation_pose = None
            self._observation_count = 0
            self._observation_direction = 0
            self._observation_returned = False
            self._curve_plan = None
            self._throat = None
            self._throat_aligned = False
            self._nominal_motion = False
            self._motion_watch = None
            self._pivot_command = None
            self._turn_departure=False
            self._turn_response=False
            self._linear_rate = self._angular_rate = 0.
            self._control_dt = .016
            self._starting = False

        def set_route(self, points):
            DriveCommand = HeroDriving._Command
            LidarManeuver = HeroDriving._Maneuver
            deque = HeroDriving._deque
            route = self._validated_route(points)
            self._throat = None
            self._throat_aligned = False
            self._nominal_motion = False
            self._motion_watch = None
            self.clearance = self._normal_clearance
            self.queue = deque(route)
            self.target = self.start = self.direction = None
            self.offset = 0.0
            self._progress = None
            self.reason = ""
            self.state = "align" if route else "idle"
            self._last_command = DriveCommand(state=self.state)
            self._anchor = self._rotation_target = self._rotation_progress = None
            self._pivot_command=None
            self._turn_departure=False
            self._turn_response=False
            self._last = None
            self._unwrapped_heading = None
            self._curve_plan = None
            self.route_revision = self._seen_revision = 0
            self._pending_route = None
            self._local = LidarManeuver(reports=self.reports_enabled) if self.allow_maneuvers else None
            self._local_driver = None
            self._observe_heading = None
            self._observed_for_leg = False
            self._observation_pose = None
            self._observation_count = 0
            self._observation_direction = 0
            self._observation_returned = False

        @staticmethod
        def _validated_route(points):
            math = HeroDriving._math
            route = [tuple(map(float, p)) for p in points]
            if any(len(p) != 2 or not all(map(math.isfinite, p)) for p in route):
                raise ValueError("Each destination must contain two finite millimetre coordinates")
            return route

        def offer_route(self, points, revision):
            """Offer remaining destinations, in order, from a validated current pose.

            A route may start with the current pose. Past source vertices must not
            be included. Copy before accepting so caller mutation cannot change it.
            Older/duplicate revisions and blocked-task retries never change state.
            """
            if isinstance(revision, bool) or not isinstance(revision, int) or revision < 1:
                raise ValueError('Route revision must be a positive integer')
            if revision <= self._seen_revision or self.state == 'blocked':
                return False
            route = self._validated_route(points)
            self._pending_route = (route, revision)
            self._seen_revision = revision
            return True

        def _reconcile_route(self, frame, previous, dt):
            angle = HeroDriving._angle
            deque = HeroDriving._deque
            math = HeroDriving._math
            np = HeroDriving._np
            if self._pending_route is None:
                return None
            route, revision = self._pending_route
            position = np.array([frame.x, frame.y])
            points = list(route)
            while len(points) > 1 and math.dist(points[0], position) <= self.tolerance:
                points.pop(0)
            if (len(points)>1 and self._anchor is not None and self.start is not None
                    and self.direction is not None and math.dist(points[0],self._anchor)<1e-9
                    and math.dist(self.start,self._anchor)<1e-9):
                # Pivot/GPS drift can put a just-reached anchor outside the
                # arrival circle. A revision continuing the same forward leg
                # must not revisit that proved-past anchor or normalize0/0.
                drift=position-self.start;future=np.asarray(points[1])-self.start
                if (float(np.linalg.norm(drift))<=max(2*self.tolerance,
                        getattr(self,'_anchor_capture_radius',0.))+2*frame.position_sigma_mm
                        and abs(float(np.cross(future,self.direction)))<=self.tolerance
                        and float(future@self.direction)>self.tolerance):
                    points.pop(0)
            if self.state == 'handoff':
                dl = frame.left_encoder-previous.left_encoder
                dr = frame.right_encoder-previous.right_encoder
                linear = self.radius*abs(dl+dr)/(2*dt)
                angular = max(self.radius*abs(dr-dl)/(self.axle*dt),
                              abs(angle(frame.heading-previous.heading))/dt)
                if linear > 5 or angular > .08:
                    return self._command(reason='settling_route_change')
                # Actual motion has settled. Adopt and compute the checked new
                # command in THIS update, without route-adopted/first-sample idle
                # ticks. Keep registered sensor evidence, not old steering intent.
                seen = self._seen_revision
                evidence=self._local
                self.set_route(points)
                if evidence is not None:
                    evidence.reset_leg();self._local=evidence
                self._last=frame;self._unwrapped_heading=frame.heading
                self.route_revision, self._seen_revision = revision, seen
                self._pending_route=None
                return None
            if not points:
                self.set_route([])
                self.route_revision = self._seen_revision = revision
                return self._command(reason='route_cancelled')
            if self.target is None:
                self.queue = deque(points)
                self.state = 'align'
            elif math.dist(self.target, points[0]) <= self.tolerance:
                # Preserve the accepted endpoint and ALL control history. Its
                # replacement lies within the same arrival tolerance.
                self.queue = deque(points[1:])
            elif (self._throat is not None
                    and (np.asarray(points[0])-position)@self._throat[1]>self.tolerance
                    and abs(np.cross(np.asarray(points[0])-self._throat[0],self._throat[1]))<=74):
                # A new remaining point on the same exit side does not invalidate
                # the crossing already under the body. Finish it without stopping.
                self.target=points[0];self.queue=deque(points[1:])
            else:
                leg = np.asarray(points[0])-self.start
                along = float(np.dot(leg, self.direction))
                current = float(np.dot(position-self.start, self.direction))
                cross = abs(float(leg[0]*self.direction[1]-leg[1]*self.direction[0]))
                if (self.state not in ('maneuver','observe') and cross <= self.tolerance
                        and not (self._local and (self._local._search or self._local._graph_search or self._local.guidance))
                        and along > max(0.,current+self.tolerance)):
                    # An explicitly revised bend on the same forward leg can
                    # shorten/extend it without a stop or dropping a new bend.
                    direction = leg/float(np.linalg.norm(leg))
                    change = angle(math.atan2(direction[1],direction[0])-
                                   math.atan2(self.direction[1],self.direction[0]))
                    self.target = points[0]
                    self.direction = direction
                    self.queue = deque(points[1:])
                    self._progress = None
                    if self._rotation_target is not None:
                        self._rotation_target += change
                else:
                    self.state = 'handoff'
                    return self._command(reason='route_change_stop')
            self.route_revision = revision
            self._pending_route = None
            return None

        def cancel(self):
            self.set_route([])

        def _command(self, left=0., right=0., *, reason="", remaining=0., lateral=0.):
            DriveCommand = HeroDriving._Command
            math = HeroDriving._math
            report = None
            if self.reports_enabled:
                report=self._local._details if self._local and self._local._details else None
                if self._local is not None and self.target is not None and self._local.endpoint is not None:
                    shift=math.dist(self._local.endpoint,self.target)
                    if shift>1e-6:
                        report={**(report or {}),'endpoint_adjustment_mm':shift,
                                'endpoint_region_mm':5.,'planner_target_unchanged':True}
                if report and self._nominal_motion:
                    report={**report,'clearance_policy':'nominal_body_attempt',
                            'extra_clearance_mm':0.,'sensor_uncertainty_reserve_applied':False,
                            'housing_shadow_probe_mm':1.,'unknown_space_marked_free':False,
                            'maximum_local_contact_allowance_mm':None if self._throat is not None else 1.}
            left=max(-self.max_speed,min(self.max_speed,float(left)))
            right=max(-self.max_speed,min(self.max_speed,float(right)))
            self._last_command = DriveCommand(left, right, self.state,
                reason or self.reason, self.target, float(remaining), float(lateral), self.offset,
                self.route_revision, self._pending_route[1] if self._pending_route else None,
                self._local_driver.target if self._local_driver else None,
                report,self._starting and self.state not in ('idle','blocked','complete'))
            heading=(self._observe_heading if self.state=='observe' else self._rotation_target)
            pivot=self._pivot_state(heading) if heading is not None else None
            self._pivot_command=((heading,self.radius*(right-left)/self.axle,pivot[2])
                if self.state in ('align','observe') and pivot is not None and left==-right and left!=0 else None)
            return self._last_command

        def _block(self, reason):
            self.state, self.reason = "blocked", reason
            if self._local is not None and 'progress' in reason:
                self._local._decision = {**self._local._decision, 'planner_replan_required': True}
                if self.reports_enabled:
                    self._local._details = {**self._local._details, 'planner_replan_required': True, 'destination_unreachable': False, 'failure_reason': reason}
            return self._command()

        def _pivot_state(self,heading):
            angle = HeroDriving._angle
            pivot=self._pivot_command
            if pivot is not None and abs(angle(heading-pivot[0]))<1e-9:return pivot
            # Reuse the measured-settling bounds of route adoption. A pivot from
            # rest keeps the original fast profile; a moving handoff needs body-
            # lag braking latched for this heading, not repeated mode decisions.
            return (heading,2*self.radius*self.max_speed/self.axle,
                    self._linear_rate>5 or abs(self._angular_rate)>.08)

        def _rotation_speed(self,error,dt):
            # Calibrated at the original Erebus 16 ms physics/sensor cadence.
            math = HeroDriving._math
            maximum=2*self.radius*self.max_speed/self.axle
            heading=(self._unwrapped_heading or 0.)+error
            pivot=self._pivot_state(heading)
            proportional=.6*abs(error)/dt
            speed=proportional
            if pivot[2]:
                previous=self._last_command
                commanded=self.radius*(previous.right-previous.left)/self.axle
                lag=(max(0.,math.copysign(1.,error)*(self._angular_rate-commanded))
                     if previous.left+previous.right==0 else 0.)
                # Never zero/restart outside the existing inner capture band.
                speed=max(min(proportional,self.heading_tolerance/(2*dt)),proportional-.5*lag)
            if pivot[1]*error>0:speed=min(speed,abs(pivot[1]))
            return math.copysign(min(maximum,speed),error)

        def _departure_clear(self,hits,margin,left,right,dt):
            # Bound both the ongoing body yaw and the new motor command. Body
            # translation stays inside the widened segment; the front device is
            # checked over the complete angular sector plus translation distance.
            _arc_distance = HeroDriving._arc_distance
            _capsule_clear = HeroDriving._capsule_clear
            math = HeroDriving._math
            np = HeroDriving._np
            horizon=2*dt
            v=self.radius*(left+right)/2;w=self.radius*(right-left)/self.axle
            length=max(abs(v),self._linear_rate)*horizon
            sector=min(math.pi,max(abs(w),abs(self._angular_rate))*horizon)
            reserve=length*min(1.,sector)
            if not _capsule_clear(hits,margin,np.zeros(2),np.array([length,0.]),37+self.clearance+reserve):return False
            start=37*np.array([math.cos(sector),-math.sin(sector)])
            return not np.any(_arc_distance(hits,np.zeros(2),start,2*sector)<3+self.clearance+margin+length)

        def _responsive_clear(self,frame,hits,margin,dt,error,distance):
            _capsule_clear = HeroDriving._capsule_clear
            _rotate = HeroDriving._rotate
            np = HeroDriving._np
            speed=self.radius*self.max_speed
            reserve=speed*(abs(error)+2*dt*abs(self._angular_rate))/8.
            ahead=_rotate(self.direction*min(40.,distance),-frame.heading)
            if self._arc_guard is not None:
                # A straight staging chord does not authorize a curved departure.
                # Check the actual candidate two-interval curve in the same known
                # footprint used by the final motion guard, before choosing it.
                yaw=24.*error-(.25*self._angular_rate if self._turn_response else 0.)
                turn=max(-self.max_speed/2,min(self.max_speed/2,yaw*self.axle/(2*self.radius)))
                forward=max(0.,min(self.max_speed-abs(turn),distance/(self.radius*dt)*.8))
                x,y,_=self._path(forward-turn,forward+turn,self.radius,self.axle,np.array([2*dt]))
                if not self._arc_guard.clear_arc(frame,np.array([x[0],y[0]]),self.clearance):return False
            return (_capsule_clear(hits,margin,np.zeros(2),ahead,40+self.clearance+reserve)
                    and self._departure_clear(hits,margin,self.max_speed,self.max_speed,dt)
                    and self._departure_clear(hits,margin,-self.max_speed,self.max_speed,dt))

        def _forward_capture(self,error,dt):
            math = HeroDriving._math
            previous=self._last_command
            crossing=(previous.left==-previous.right and previous.left!=0
                      and (previous.right-previous.left)*self._angular_rate>0
                      and error*(error+self._angular_rate*dt)<=0)
            return (abs(error)<=abs(self._angular_rate)*dt+self.heading_tolerance and error*self._angular_rate>0
                    or crossing or abs(error)<=math.radians(12) and abs(self._angular_rate)<=.08)

        def _steering_rate(self,error,arrival_tolerance):
            math = HeroDriving._math
            if self._turn_departure:
                # Once measured body yaw follows the forward wheel command,
                # further momentum compensation creates an unnecessary counterturn.
                if (not self._turn_response or abs(self._angular_rate)<=.08) and abs(error)<=math.atan2(arrival_tolerance,120.):
                    self._turn_departure=False
                    w=8.*error
                else:w=24.*error-(.25*self._angular_rate if self._turn_response else 0.)
            else:w=8.*error
            if abs(error)<math.radians(.08) and not self._turn_departure:w=0.
            # Both wheels remain forward during correction; the faster stays at6.28.
            return max(-self.radius*self.max_speed/self.axle,min(self.radius*self.max_speed/self.axle,w))

        def _begin_leg(self,position):
            np = HeroDriving._np
            self.clearance = self._normal_clearance
            self._nominal_motion = False
            self.target=self.queue.popleft()
            self.start=position.copy() if self._anchor is None else np.asarray(self._anchor)
            leg=np.asarray(self.target)-self.start
            self.direction=leg/max(float(np.linalg.norm(leg)),1e-12)
            self.offset=0.;self._progress=None
            self._turn_departure=False
            self._turn_response=False
            self._rotation_target=self._rotation_progress=None
            self.state='curve' if self.curved_approach else 'align'
            self._observed_for_leg=False
            self._observation_count=0;self._observation_direction=0
            self._observation_returned=False;self._observation_pose=None
            if self._local is not None:self._local.reset_leg()

        def _approach(self, frame, previous, preview=False):
            # Match visibility authorization to the actual guarded command horizon,
            # not an unrelated whole wheelbase beyond an occluding corner.
            WaypointDriver = HeroDriving._Controller
            angle = HeroDriving._angle
            math = HeroDriving._math
            self._local.observed_length=2*self._control_dt*self.radius*self.max_speed
            self._local.heading_tolerance=self.heading_tolerance
            point = self._local.propose(frame, self.target, 0. if self._local.tight_attempt else self.clearance)
            if self._local._decision.get('destination_rejected'):
                return self._block('destination_footprint_obstructed')
            if self._commit_throat(frame):
                crossing=self._throat_command(frame,previous)
                if crossing is not None:return crossing
            if point is None:
                if self._local._decision.get('reason')=='entry_search_pending':
                    return None if preview else self._command(reason='checking_local_entry')
                if not self._local.tight_attempt:
                    # The planner/caller owns reachability. First try ordinary
                    # reserves, then attempt the connected nominal body geometry
                    # before handing its destination back. Live positive returns,
                    # visibility, forward-only motion and progress guards remain.
                    self._local.tight_attempt=True
                    self._local._obstacle_frame=None
                    self._local.guidance=[]
                    self._local.trail=[]
                    if self._commit_throat(frame):
                        crossing=self._throat_command(frame,previous)
                        if crossing is not None:return crossing
                    return None if preview else self._command(reason='precision_entry_attempt')
                if preview:return None
                if self._local._decision.get('planner_replan_required'):
                    reason=('opening_narrower_than_robot' if self._local._decision.get('reason')=='observed_entry_narrower_than_body'
                            else 'opening_clearance_unproved' if self._local._decision.get('reason')=='entry_clearance_unproved'
                            else 'opening_requires_planner_detour')
                    return self._block(reason)
                if not self.observation_turns:
                    self._progress = None  # Waiting is not a failed motor command.
                    return self._command(reason='waiting_for_entry_visibility')
                position=(frame.x,frame.y)
                if (not self._observed_for_leg or self._observation_pose is not None
                        and math.dist(position,self._observation_pose)>=74):
                    # Expose self-occluded sectors with a bounded monotone sweep.
                    # A single exact 45-degree view can still straddle a sampled
                    # depth edge. Continue the SAME sweep to 90 only if necessary.
                    view_target=self._local.guidance[0] if self._local.guidance else self.target
                    bearing=math.atan2(view_target[1]-frame.y,view_target[0]-frame.x)
                    if self._observation_pose is None or math.dist(position,self._observation_pose)>=74:
                        self._observation_count=0
                        self._observation_direction=math.copysign(1.,angle(bearing-frame.heading) or 1.)
                        self._observation_returned=False
                    self._observation_count+=1
                    self._observe_heading = frame.heading+self._observation_direction*math.pi/4
                    if self._local.tight_attempt:
                        # Observe through a physically available angular sector.
                        # The chassis may fit while its offset front sensor cannot
                        # perform an arbitrary 45-degree sweep beside a corner.
                        w=self._rotation_speed(self._observation_direction*math.pi/4,self._control_dt)
                        l,r=-w*self.axle/(2*self.radius),w*self.axle/(2*self.radius)
                        hits,margin,_=self._local._obstacles(frame)
                        if not self._clear(hits,margin,l,r,(math.pi/4)/abs(w)):
                            turn=angle(math.atan2(self.target[1]-frame.y,self.target[0]-frame.x)-frame.heading)
                            w=self._rotation_speed(turn,self._control_dt)
                            l,r=-w*self.axle/(2*self.radius),w*self.axle/(2*self.radius)
                            if abs(w)>1e-9 and self._clear(hits,margin,l,r,abs(turn/w)):
                                self._observe_heading=frame.heading+turn
                                self._observation_count=2;self._observation_returned=True
                    self._observation_pose = position
                    self._rotation_progress = (frame.time, abs(angle(self._observe_heading-frame.heading)))
                    self.state = 'observe'
                    return self._observation_command(frame)
                self._local._decision = {**self._local._decision, 'planner_replan_required': True}
                if self.reports_enabled:
                    self._local._details = {**self._local._details, 'planner_replan_required': True, 'destination_unreachable': False}
                return self._block('local_maneuver_unresolved')
            if self._local.tight_attempt:
                self._nominal_motion=True;self.clearance=0.
            local_distance=math.dist(point,(frame.x,frame.y))
            self._local_driver = WaypointDriver(wheel_radius_mm=self.radius,axle_mm=self.axle,
                max_wheel_speed=self.max_speed,position_tolerance_mm=max(self.tolerance,min(2.,local_distance/4)) if self._local.curved else self.tolerance,
                heading_tolerance_deg=math.degrees(self.heading_tolerance),clearance_mm=self.clearance,
                allow_maneuvers=False,centering=False,
                curved_approach=self._local.curved, reports=self.reports_enabled)
            self._local_driver._arc_guard=self._local
            self._local_driver.set_route([point])
            self._local_driver._last=previous if previous is not frame else None
            self._local_driver._unwrapped_heading=previous.heading
            self.state = 'maneuver'
            local=self._local_driver.update(frame,initial_dt=self._control_dt)
            return self._command(local.left,local.right,reason='local_approach_selected')

        def _observation_command(self,frame):
            angle = HeroDriving._angle
            error=angle(self._observe_heading-frame.heading)
            w=self._rotation_speed(error,self._control_dt)
            left,right=-w*self.axle/(2*self.radius),w*self.axle/(2*self.radius)
            hits,margin,_=self._local._obstacles(frame)
            if not self._nominal_motion:margin=self._local._obstacle_regular_margin
            if not self._clear(hits,margin,left,right,2*self._control_dt):
                return self._block('entry_observation_rotation_obstructed')
            return self._command(left,right,reason='observe_entry_clearance')

        def _commit_throat(self,frame):
            """Commit only to a measured aperture already overlapping the body.

            No distant detours or new destinations. Its axis and side are latched
            before contact can restart temporary-point search and observation turns.
            """
            angle = HeroDriving._angle
            math = HeroDriving._math
            np = HeroDriving._np
            position=np.array([frame.x,frame.y]);goal=np.asarray(self.target)
            choices=[]
            for mid,axis,width in self._local.apertures:
                if not 74<width<=80:continue  # Body fits; front-device envelope is tight.
                axis=axis.copy()
                if (goal-position)@axis<0:axis=-axis
                normal=np.array([-axis[1],axis[0]])
                along=float((position-mid)@axis);lateral=float((position-mid)@normal)
                if abs(along)>37 or (goal-position)@axis<=0 or abs((goal-mid)@normal)>74:continue
                section=math.sqrt(max(0.,37**2-along**2))
                if abs(lateral)>width/2-section+3:continue
                if not self._local.tight_attempt:
                    # A body-overlapping measured gap can fit the chassis while
                    # the offset front device cannot make the required turn with
                    # ordinary reserves. Prove that conflict directly rather than
                    # exhaust the whole ordinary graph before permitting contact.
                    error=angle(math.atan2(axis[1],axis[0])-frame.heading)
                    hits,margins,_=self._local._obstacles(frame)
                    if abs(error)>self.heading_tolerance:
                        w=self._rotation_speed(error,.016)
                        left,right=-w*self.axle/(2*self.radius),w*self.axle/(2*self.radius)
                        horizon=abs(error/w)
                    else:left=right=self.max_speed;horizon=.032
                    if self._clear(hits,margins,left,right,horizon):continue
                choices.append((abs(lateral)+.1*abs(along),mid,axis,width))
            if not choices:return False
            _,mid,axis,width=min(choices,key=lambda c:c[0])
            self._throat=(mid.copy(),axis.copy(),width)
            self._throat_aligned=False;self._local_driver=None
            self._local.tight_attempt=True;self._nominal_motion=True;self.clearance=0.
            self._local._obstacle_frame=None
            self._local._search=[];self._local._graph_search=None;self._local.guidance=[]
            self._progress=(frame.time,float((position-mid)@axis))
            self.state='throat'
            return True

        def _throat_command(self,frame,previous):
            """Hold one crossing intent; tolerate side contact without pivoting.

            This short phase is authorized by the measured wider-than-body gap.
            It keeps live front/hole, invalid-sensor and actual-progress vetoes.
            It deliberately does not demand precision side clearance or rerun the
            local search while the body is in the throat.
            """
            _interval_distance = HeroDriving._interval_distance
            _rotate = HeroDriving._rotate
            angle = HeroDriving._angle
            math = HeroDriving._math
            np = HeroDriving._np
            mid,axis,width=self._throat
            position=np.array([frame.x,frame.y]);delta=np.asarray(self.target)-position
            normal=np.array([-axis[1],axis[0]])
            along=float((position-mid)@axis);lateral=float((position-mid)@normal)
            distance=float(np.linalg.norm(delta));dt=self._control_dt
            heading=math.atan2(axis[1],axis[0])
            aligned_hits=_rotate(np.asarray(frame.hits),frame.heading-heading)
            nearby=abs(aligned_hits[:,0])<37
            pinched=(np.any(nearby&(aligned_hits[:,1]>0)&(aligned_hits[:,1]<40))
                     and np.any(nearby&(aligned_hits[:,1]<0)&(aligned_hits[:,1]>-40)))
            arrival=self.tolerance+min(self.tolerance,2*frame.position_sigma_mm)
            captured=_interval_distance(self.target,previous,frame)
            adjusted=(self._local.endpoint is not None and math.dist(self._local.endpoint,self.target)>1e-6)
            if adjusted and captured<=5.+arrival:
                self._throat=None;self.state='drive'
                return None
            if captured<=arrival and self.queue:
                following=np.asarray(self.queue[0])-position
                if following@axis>arrival and abs(np.cross(following,axis))<=74:
                    self.state='drive'
                    return None  # Acknowledge this node, retain the crossing intent.
            if captured<=arrival or along>37 and not pinched:
                self._throat=None;self._local.reset_leg();self._local_driver=None
                self._nominal_motion=False;self.clearance=self._normal_clearance;self.state='align'
                self.start=position.copy();self.direction=delta/max(distance,1e-12);self.offset=0.
                self._rotation_target=self._rotation_progress=self._progress=None
                return None
            self.state='throat'
            if self._progress is None:self._progress=(frame.time,along)
            deadband=max(.5,(width-74)/4)
            correction=(max(-math.pi/12,min(math.pi/12,
                        math.atan2(-2*math.copysign(max(0.,abs(lateral)-deadband),lateral),17.)))
                        if along<0 else 0.)
            error=angle(heading+correction-frame.heading)
            self._local._decision = dict(reason='committed_throat_crossing', planner_replan_required=False)
            if self.reports_enabled:
                self._local._details = dict(reason='committed_throat_crossing', axis_heading_deg=math.degrees(heading) % 360, measured_width_mm=width, side_contact_permitted=True, search_and_observation_suspended=True, planner_replan_required=False, destination_unreachable=False)
            if not self._throat_aligned:
                if abs(error)>math.radians(3):
                    if frame.time-self._progress[0]>2:return self._block('no_throat_alignment_progress')
                    w=self._rotation_speed(error,dt)
                    return self._command(-w*self.axle/(2*self.radius),w*self.axle/(2*self.radius),
                                         reason='throat_entry_alignment',remaining=distance,lateral=lateral)
                self._throat_aligned=True;self._progress=(frame.time,along)
            if along-self._progress[1]>max(.5,2*frame.position_sigma_mm):
                self._progress=(frame.time,along)
            elif frame.time-self._progress[0]>2:
                return self._block('no_throat_translation_progress')
            # A wall directly across the front is not a side scrape. Keep this
            # live sensor corridor; do not push into a closed end or unseen hole.
            hits=np.asarray(frame.hits)
            if np.any((hits[:,0]>0)&(hits[:,0]<40)&(abs(hits[:,1])<3)):
                return self._block('throat_front_obstructed')
            turn=max(-self.max_speed/2,min(self.max_speed/2,6*error*self.axle/(2*self.radius)))
            if abs(error)<math.radians(.5):turn=0.
            forward=min(self.max_speed-abs(turn),.8*max(0.,float(delta@axis))/(self.radius*dt))
            return self._command(forward-turn,forward+turn,reason='committed_throat_crossing',
                                 remaining=distance,lateral=lateral)

        @staticmethod
        def _path(left, right, radius, axle, times):
            np = HeroDriving._np
            v = radius * (left + right) / 2
            w = radius * (right - left) / axle
            a = w * times
            if abs(w) < 1e-10:
                return v * times, np.zeros_like(times), a
            return v / w * np.sin(a), v / w * (1 - np.cos(a)), a

        def _clear(self, hits, margin, left, right, horizon):
            """Exact continuous body/device sweep against every current raw return."""
            _arc_distance = HeroDriving._arc_distance
            _capsule_clear = HeroDriving._capsule_clear
            _contact_margin = HeroDriving._contact_margin
            np = HeroDriving._np
            if self._turn_departure and not self._departure_clear(hits,margin,left,right,horizon/2):return False
            speed = self.radius * max(abs(left), abs(right))
            omega = self.radius * abs(right - left) / self.axle
            reach = speed * horizon + 40.0
            nearby = np.hypot(hits[:, 0], hits[:, 1]) <= reach + margin + self.clearance
            p, m = hits[nearby], margin[nearby]
            if not len(p):
                return True
            v=self.radius*(left+right)/2;w=self.radius*(right-left)/self.axle
            tight=((self._local is not None and self._nominal_motion)
                   or (self._arc_guard is not None and self._arc_guard.tight_attempt))
            for start,r in ((np.zeros(2),37.),(np.array([37.,0.]),3.)):
                reserve=_contact_margin(p,m,start,r) if tight else m
                if abs(w)<1e-10:
                    if not _capsule_clear(p,reserve-1e-8,start,start+[v*horizon,0],r+self.clearance):return False
                elif np.any(_arc_distance(p,np.array([0.,v/w]),start,w*horizon)<r+self.clearance+reserve-1e-8):return False
            return True

        def _curve_command(self,frame,hits,margin,dt,distance):
            _entry_arc = HeroDriving._entry_arc
            _rotate = HeroDriving._rotate
            math = HeroDriving._math
            np = HeroDriving._np
            position=np.array([frame.x,frame.y])
            if self._curve_plan is None:
                local=_rotate(np.asarray(self.target)-position,-frame.heading)
                arc=_entry_arc(local)
                if arc is None:return self._block('local_arc_heading_diverged')
                if arc[0]<0:
                    self.curved_approach=False;self.state='align'
                    return self._command(reason='forward_entry_alignment_required')
                self._curve_plan=(position,frame.heading,*arc)
            origin,heading,total,k=self._curve_plan
            here=_rotate(position-origin,-heading)
            # Follow the latched path with a wheelbase-sized preview, rather than
            # chasing a noisy millimetre-scale endpoint with unbounded curvature.
            if abs(k)<1e-8:
                progress=math.copysign(1,total)*here[0]
                preview=np.array([math.copysign(progress+self.axle,total),0.])
            else:
                r=1/k;turn=total*k
                theta=math.atan2(here[0]/r,1-here[1]/r)
                progress=max(0.,min(abs(total),theta/k*math.copysign(1,total)))
                theta=k*math.copysign(progress,total)+math.copysign(min(self.axle/abs(r),math.pi/3),turn)
                preview=np.array([r*math.sin(theta),r*(1-math.cos(theta))])
            travel=math.copysign(max(0.,abs(total)-progress),total)
            if abs(travel)<self.tolerance/2:return self._block('local_arc_tracking_error')
            preview=_rotate(_rotate(preview,heading)+origin-position,-frame.heading)
            curvature=2*preview[1]/max(float(preview@preview),1e-12)
            factors=np.array([1-curvature*self.axle/2,1+curvature*self.axle/2])
            speed=min(self.max_speed/float(np.max(abs(factors))),
                      .8*abs(travel)/(self.radius*dt))
            left,right=np.clip(math.copysign(speed,travel)*factors,-self.max_speed,self.max_speed)
            # The complete approach was proved before selection. Re-prove the
            # actual corrected next two intervals from the live pose and raw scan;
            # anticipate a newly seen obstacle up to 80mm ahead on the same intent.
            v=self.radius*abs(left+right)/2
            self._arc_guard.observe(frame)
            x,y,_=self._path(left,right,self.radius,self.axle,np.array([2*dt]))
            raw_clear=self._clear(hits,margin,left,right,2*dt)
            if not raw_clear or not self._arc_guard.clear_arc(frame,np.array([x[0],y[0]]),self.clearance):
                local=_rotate(np.asarray(self.target)-position,-frame.heading)
                if self._arc_guard.clear_entry(frame,local,self.clearance):
                    self.curved_approach=False;self.state='align'
                    self.start=position;self.direction=(np.asarray(self.target)-position)/distance
                    self._rotation_target=self._rotation_progress=None
                    return self._command(reason='entry_curve_to_straight')
                return self._block('local_arc_current_scan_blocked' if not raw_clear else 'local_arc_clearance_unproved')
            return self._command(left,right,reason='guided_entry_curve',remaining=distance)

        def _passage_offset(self, frame, hits, margin, lateral, remaining, heading):
            """Latch one measured corridor centre instead of chasing scan noise.

            A matched pair of sides/corners must bound the proposed local strip.
            A single close wall does not authorize an unobserved sideways detour.
            The collision predicate independently checks the commanded sweep.
            """
            angle = HeroDriving._angle
            math = HeroDriving._math
            np = HeroDriving._np
            if not self.centering:return 0.
            relative = angle(frame.heading - heading)
            c, s = math.cos(relative), math.sin(relative)
            u = hits[:, 0] * c - hits[:, 1] * s
            v = hits[:, 0] * s + hits[:, 1] * c + lateral
            window = (u > -10) & (u < min(180., remaining + 37))
            upper = window & (v > 10)
            lower = window & (v < -10)
            if not (upper.any() and lower.any()):
                return self.offset
            # Spatial median rejects an isolated speckle for steering only. Raw
            # points, including that speckle, still participate in _clear().
            hi = float(np.percentile(v[upper], 5))
            lo = float(np.percentile(v[lower], 95))
            width = hi - lo
            if width < 76 or width >= 120:
                return self.offset
            # A bend/corner pair is not a parallel corridor. Recentring against
            # its changing percentiles chases the curved wall left/right. Those
            # entries belong to the connected maneuver search; lane centering
            # needs two measured straight sides supporting the travel heading.
            for side,edge in ((upper,hi),(lower,lo)):
                selected=side & (abs(v-edge)<=2.)
                along=u[selected];across=v[selected]
                if not len(along) or np.ptp(along)<37:return self.offset
                centered=along-along.mean()
                slope=float(centered@(across-across.mean())/(centered@centered))
                residual=across-across.mean()-slope*centered
                if abs(slope)>math.tan(math.pi/12) or np.max(abs(residual))>.5:
                    return self.offset
            proposed = (hi + lo) / 2
            # Local correction is bounded to half the preferred 60mm grid spacing.
            # Destinations themselves are never snapped or moved onto that grid.
            if abs(proposed) > 30:
                return self.offset
            # Do not recenter a generous corridor or oscillate between noisy fits.
            if self.offset == 0 and min(hi, -lo) < 40:
                self.offset = proposed
            elif self.offset:
                # A fixed 2mm hysteresis was larger than the available error budget
                # in a 2mm-margin opening. Use its measured feasible centre interval.
                reserve = 37+self.clearance+float(np.max(margin[window]))
                if not (lo+reserve <= self.offset <= hi-reserve):
                    self.offset = proposed
            return self.offset

        def update(self, frame, *, initial_dt=None):
            DriveCommand = HeroDriving._Command
            _capsule_clear = HeroDriving._capsule_clear
            _interval_distance = HeroDriving._interval_distance
            _rotate = HeroDriving._rotate
            angle = HeroDriving._angle
            math = HeroDriving._math
            np = HeroDriving._np
            self._starting=False
            numbers = (frame.time, frame.x, frame.y, frame.heading,
                       frame.left_encoder, frame.right_encoder)
            if not all(map(math.isfinite, numbers)):
                return self._block("invalid_pose_or_encoder")
            if frame.relocated:
                return self._block("relocated_reissue_route")
            if self._last is None:
                self._last = frame
                self._unwrapped_heading = frame.heading
                if initial_dt is None:return self._command(reason="first_sample")
                # The caller knows the real sensor/control period. IMU heading,
                # pose and raw geometry suffice for the first checked command;
                # no invented preceding motion sample or idle tick is necessary.
                previous=frame;dt=initial_dt;self._starting=True
            else:
                dt = frame.time - self._last.time
                previous = self._last
            self._last = frame
            if not isinstance(dt,(int,float)) or not math.isfinite(dt) or not (0 < dt <= .064):
                return self._block("stale_or_discontinuous_sample")
            self._control_dt=dt
            self._angular_rate=angle(frame.heading-previous.heading)/dt
            self._linear_rate=self.radius*abs(frame.left_encoder-previous.left_encoder+frame.right_encoder-previous.right_encoder)/(2*dt)
            self._unwrapped_heading += angle(frame.heading-previous.heading)
            hits = np.asarray(frame.hits, dtype=float)
            if (not frame.scan_valid or hits.ndim != 2 or hits.shape[1:] != (2,)
                    or not np.isfinite(hits).all()):
                return self._block("invalid_current_scan")
            try:
                margin = np.broadcast_to(np.asarray(frame.hit_margin, dtype=float), (len(hits),))
            except ValueError:
                return self._block("invalid_scan_uncertainty")
            if not np.isfinite(margin).all() or np.any(margin < 0):
                return self._block("invalid_scan_uncertainty")
            if not math.isfinite(frame.position_sigma_mm) or frame.position_sigma_mm<0:
                return self._block('invalid_pose_uncertainty')
            if frame.free_polygon is not None:
                polygon=np.asarray(frame.free_polygon)
                if (polygon.ndim!=2 or polygon.shape[1:]!=(2,) or len(polygon)<32
                        or not np.isfinite(polygon).all()):
                    return self._block('invalid_current_visibility')
            elif self._local_driver is not None or self._arc_guard is not None or self._throat is not None:
                return self._block('local_visibility_unavailable')
            if self.state != 'blocked':
                reconciled = self._reconcile_route(frame, previous, dt)
                if reconciled is not None:
                    return reconciled
            if self._throat is None and ((self._local is not None and self._local.tight_attempt)
                    or (self._arc_guard is not None and self._arc_guard.tight_attempt)):
                if frame.free_polygon is None:
                    return self._block('local_visibility_unavailable')
                guard=self._arc_guard or self._local
                hits,margin,_=guard._obstacles(frame)
                if self._local is not None and not self._nominal_motion:
                    # Preparing a nominal-clearance search does not authorize
                    # contact on the old straight intent. Keep its normal guard
                    # until an actual maneuver or measured crossing is selected.
                    margin=guard._obstacle_regular_margin
            if self._local is not None:
                self._local.observe(frame)
            if self.state in ("blocked", "idle", "complete"):
                return self._command()
            if self.allow_maneuvers:
                # This watchdog belongs to the task, not a replaceable temporary
                # driver. Replanning cannot reset a frozen actuator's timeout.
                pose=np.array([frame.x,frame.y]);watch=self._motion_watch
                if (watch is None or np.linalg.norm(pose-watch[1])>max(.5,2*frame.position_sigma_mm)
                        or abs(angle(frame.heading-watch[2]))>math.radians(.5)):
                    self._motion_watch=(frame.time,pose,frame.heading)
                elif self._last_command.left==self._last_command.right==0:
                    self._motion_watch=(watch[0]+dt,watch[1],watch[2])
                elif frame.time-watch[0]>2:
                    reason=('no_throat_translation_progress' if self.state=='throat' and self._throat_aligned
                            else 'no_throat_alignment_progress' if self.state=='throat'
                            else 'no_rotation_progress' if self.state in ('align','observe')
                            else 'no_translation_progress')
                    return self._block(reason)

            position = np.array([frame.x, frame.y])
            if self._throat is not None:
                crossing=self._throat_command(frame,previous)
                if crossing is not None:return crossing
            enc_w=self.radius*((frame.right_encoder-previous.right_encoder)
                             -(frame.left_encoder-previous.left_encoder))/(self.axle*dt)
            heading_step=angle(frame.heading-previous.heading)
            preceding=self._last_command
            if (preceding.left==-preceding.right and abs(preceding.left)>=.95*self.max_speed
                    and abs(enc_w-self._angular_rate)*dt>self.heading_tolerance):
                self._turn_response=True
            elif self._turn_departure and min(preceding.left,preceding.right)>=0:
                # Original Erebus position sensors resolve6.28mrad. Bound both
                # encoder differences before deciding that body/wheel yaw agree.
                uncertainty=2*self.radius*.00628/self.axle+self.heading_tolerance
                if abs(enc_w-self._angular_rate)*dt<=uncertainty:self._turn_response=False
            settled=(abs(enc_w)<=.08 and abs(heading_step)<=self.heading_tolerance/2)
            if self.state == 'observe':
                error=angle(self._observe_heading-frame.heading)
                if self._rotation_progress[1]-abs(error)>math.radians(.5):
                    self._rotation_progress=(frame.time,abs(error))
                elif frame.time-self._rotation_progress[0]>2:
                    return self._block('no_observation_rotation_progress')
                if abs(error)<=self.heading_tolerance and (settled or abs(error)<=self.heading_tolerance/2
                        or error*(error+heading_step)<=0):
                    if self._observation_count>=2 and not self._observation_returned:
                        # End the bounded visibility sweep facing the intended
                        # travel direction. Aperture sides then have fresh ranges,
                        # rather than only GPS-registered returns hidden by a
                        # camera housing after the sweep. Translation stays zero.
                        self._observation_returned=True
                        bearing=math.atan2(self.target[1]-frame.y,self.target[0]-frame.x)
                        error=angle(bearing-frame.heading)
                        if abs(error)>self.heading_tolerance:
                            self._observe_heading=frame.heading+error
                            self._rotation_progress=(frame.time,abs(error))
                            return self._observation_command(frame)
                    self._observed_for_leg=self._observation_count>=2
                    self.state='align';self._rotation_target=self._rotation_progress=None
                else:
                    return self._observation_command(frame)
            if self.state=='maneuver':
                shifted=(self._local.endpoint is not None and math.dist(self._local.endpoint,self.target)>1e-6)
                region=(5. if shifted else 0.)+self.tolerance+min(self.tolerance,2*frame.position_sigma_mm)
                if _interval_distance(self.target,previous,frame)<=region:
                    self._local_driver=None;self.state='drive'
            if self.state == 'maneuver':
                # Revalidate the remaining staging primitive as soon as its hidden
                # corner becomes visible, while the current two-step sweep is still
                # usable. Do not wait until the body arrives against that corner.
                child=self._local_driver
                if child.target is not None:
                    point=_rotate(np.asarray(child.target)-position,-frame.heading)
                    proof=(self._local.clear_arc if child.curved_approach else self._local.clear_entry)
                    if not proof(frame,point,self.clearance,observed_length=2*dt*self.radius*self.max_speed):
                        replacement=self._approach(frame,previous,preview=True)
                        if replacement is not None:return replacement
                local=self._local_driver.update(frame)
                if local.state=='blocked':
                    if local.reason in ('no_rotation_progress','no_translation_progress'):
                        # A frozen actuator/sensor response is not evidence that
                        # another geometric primitive will work. Preserve the
                        # stopped failure instead of cycling through approaches.
                        return self._block(local.reason)
                    # A new view may invalidate a *future* part of an approach.
                    # Stop and choose another proved preparation on the same
                    # latched side; one failed local primitive is not a barrier.
                    self._local._decision = {**self._local._decision}
                    if self.reports_enabled:
                        self._local._details = {**self._local._details, 'last_approach_failure': local.reason}
                    if (self._local._decision.get('alignment_only_until_live_sweep_proved')
                            and local.reason=='local_motion_visibility_unproved'):
                        self._local.failed_preparation_at=(frame.x,frame.y)
                        self._local.guidance=[]
                    if self._local_driver.curved_approach:
                        point=np.asarray(self._local_driver.target)
                        self._local.failed_curves.append(point.copy())
                        self._local.trail=[p for p in self._local.trail if np.linalg.norm(p-point)>=2]
                    self._local_driver=None
                    self.state='align';self._rotation_target=self._rotation_progress=None
                    return self._approach(frame,previous)
                if local.state=='complete':
                    self._local_driver=None
                    self.state='align';self.start=position.copy();self._anchor=None
                    leg=(self._local.endpoint if self._local.endpoint is not None else np.asarray(self.target))-position
                    self.direction=leg/max(float(np.linalg.norm(leg)),1e-12)
                    self._rotation_target=self._rotation_progress=self._progress=None
                    self.offset=0.
                else:
                    return self._command(local.left,local.right,reason='local_approach',
                                         remaining=float(np.linalg.norm(np.asarray(self.target)-position)),
                                         lateral=local.lateral_mm)
            if self.target is None:
                if not self.queue:
                    self.state = "complete"
                    return self._command()
                self._begin_leg(position)
            endpoint=(self._local.endpoint if self._local is not None and self._local.endpoint is not None
                      else np.asarray(self.target))
            delta = np.asarray(endpoint) - position
            distance = float(np.linalg.norm(delta))
            heading = math.atan2(self.direction[1], self.direction[0])
            normal = np.array([-self.direction[1], self.direction[0]])
            lateral = float(np.dot(position - self.start, normal))
            remaining = float(np.dot(delta, self.direction))
            arrival_tolerance=self.tolerance+min(self.tolerance,2*frame.position_sigma_mm)
            public_distance=math.dist(self.target,position)
            adjusted=math.dist(endpoint,self.target)>1e-6
            captured=_interval_distance(endpoint,previous,frame)
            public_capture=_interval_distance(self.target,previous,frame)
            if captured<=arrival_tolerance or adjusted and public_capture<=5.+arrival_tolerance:
                # The measured pose is inside the accepted arrival region. Start
                # the checked next command now; final destinations still command
                # zero wheels. Previous-interval motion is not an idle-tick gate.
                target = self.target
                self._anchor = target
                self._anchor_capture_radius=max(public_distance,arrival_tolerance+(5. if adjusted else 0.))
                self.target = None
                self.state = "align" if self.queue else "complete"
                # The node is measured reached. Stage the next leg now rather
                # than paying an idle notification
                # tick. Keep the old point in the returned reached event; internal
                # target/intent already own the next leg for revision safety.
                left=right=0.
                if self.queue:
                    self._begin_leg(position)
                    if self._throat is not None:
                        self._local.tight_attempt=True;self._nominal_motion=True;self.clearance=0.
                        crossing=self._throat_command(frame,previous)
                        if crossing is not None:
                            result=self._command(crossing.left,crossing.right,reason='reached',remaining=distance,lateral=lateral)
                            return DriveCommand(**{**result.__dict__,'target':target})
                    hits=np.asarray(frame.hits,dtype=float)
                    margin=np.broadcast_to(np.asarray(frame.hit_margin,dtype=float),(len(hits),))
                    next_delta=np.asarray(self.target)-position
                    if float(np.linalg.norm(next_delta))>arrival_tolerance:
                        error=angle(math.atan2(self.direction[1],self.direction[0])-frame.heading)
                        responsive=self._responsive_clear(frame,hits,margin,dt,error,float(np.linalg.norm(next_delta)))
                        self._turn_departure=responsive
                        if responsive and self._forward_capture(error,dt) or abs(error)<=self.heading_tolerance:
                            self.state='drive'
                            turn=self._steering_rate(error,arrival_tolerance)*self.axle/(2*self.radius)
                            forward=max(0.,min(self.max_speed-abs(turn),float(next_delta@self.direction)/(self.radius*dt)*.8))
                            left,right=forward-turn,forward+turn
                        else:
                            self._rotation_target=self._unwrapped_heading+error
                            w=(math.copysign(2*self.radius*self.max_speed/self.axle,error)
                               if responsive else self._rotation_speed(error,dt))
                            left,right=-w*self.axle/(2*self.radius),w*self.axle/(2*self.radius)
                        if not self._clear(hits,margin,left,right,2*dt):
                            left=right=0.;self.state='align'
                result = self._command(left,right,reason="reached", remaining=public_distance, lateral=lateral)
                return DriveCommand(**{**result.__dict__, "target": target})
            if remaining < -self.tolerance and not self.curved_approach:
                return self._block("overshot_destination")
            if self._progress is None or self._progress[1] - distance > .5:
                self._progress = (frame.time, distance)
            elif frame.time - self._progress[0] > 2 and self.state in ('drive','curve'):
                return self._block("no_translation_progress")
            if self.curved_approach:
                return self._curve_command(frame,hits,margin,dt,distance)
            if self._local is not None and self._local.guidance:
                self._local.advance_guidance(frame)
                if self._local.guidance:return self._approach(frame,previous)
            if self._local is not None and self._local.tight_attempt:
                # Leave precision mode as soon as the ordinary travel corridor
                # reopens. Do not carry tight-entry calculations/slowdown through
                # a spacious remainder of the same public leg.
                ahead=self.direction*min(160.,max(0.,remaining))
                local=_rotate(ahead,-frame.heading)
                ordinary=np.broadcast_to(np.asarray(frame.hit_margin,dtype=float),(len(frame.hits),))
                reserve=float(np.max(ordinary)) if len(ordinary) else 0.
                if _capsule_clear(hits,reserve,np.zeros(2),local,37+self._normal_clearance):
                    self._local.tight_attempt=False;self._nominal_motion=False;self._local._obstacle_frame=None
                    self.clearance=self._normal_clearance
                    hits=np.asarray(frame.hits,dtype=float);margin=ordinary
            if self.state=='drive' and remaining<=self.tolerance and distance>arrival_tolerance:
                # At the end, a GPS/lateral tracking error can leave the robot
                # beside the point with zero forward projection. Continuing the
                # old leg would stop forever. Re-aim at the unchanged point, with
                # the ordinary in-place rotation and raw swept collision checks.
                self.start=position.copy();self.direction=delta/distance
                self.state='align';self.offset=0.
                self._rotation_target=self._rotation_progress=None
                heading=math.atan2(delta[1],delta[0]);lateral=0.;remaining=distance
                normal=np.array([-self.direction[1],self.direction[0]])
            drive_heading=heading
            error = angle(drive_heading - frame.heading)
            self._passage_offset(frame,hits,margin,lateral,remaining,heading)
            if (self._local is not None and frame.free_polygon is not None
                    and remaining>40):
                # Test the measured centred approach too. A nonzero centering
                # offset is not proof that there is room to enter that strip.
                lookahead=120. if self.offset else 160.
                goal=self.direction*min(lookahead,remaining)+normal*(self.offset-lateral)
                c,s=math.cos(frame.heading),math.sin(frame.heading)
                local_goal=np.array([goal[0]*c+goal[1]*s,-goal[0]*s+goal[1]*c])
                if not _capsule_clear(hits,margin,np.zeros(2),local_goal,37+self.clearance):
                    # Search ahead while the presently commanded sweep is safe.
                    # A farther blocked chord is a reason to prepare a maneuver,
                    # not to zero the wheels during every bounded search slice.
                    approach=self._approach(frame,previous,preview=True)
                    if approach is not None:return approach
            if self.state!='align' and abs(error)>math.radians(12):
                self.state='align'
                self._rotation_target=self._rotation_progress=None
            if self.state == "align":
                if self._rotation_target is None:
                    self._rotation_target = self._unwrapped_heading + error
                error = self._rotation_target - self._unwrapped_heading
                if self._rotation_progress is None or self._rotation_progress[1]-abs(error) > math.radians(.5):
                    self._rotation_progress = (frame.time, abs(error))
                elif frame.time-self._rotation_progress[0] > 2:
                    return self._block("no_rotation_progress")
                # Capture the inner band while moving. Stopping on the outer
                # edge can rebound just outside tolerance and restart the turn.
                # A genuinely settled pose retains the full existing tolerance.
                # Prove the forward recovery from raw geometry before omitting
                # the stationary precision tail. Unknown/blocked recovery retains
                # the existing accurate pivot and local maneuver policy.
                responsive=self._responsive_clear(frame,hits,margin,dt,error,distance)
                self._turn_departure=self._turn_departure or responsive
                departing=responsive and self._forward_capture(error,dt)
                if departing or abs(error)<=self.heading_tolerance and (settled or abs(error)<=self.heading_tolerance/2
                        or error*(error+heading_step)<=0):
                    self._turn_departure=departing
                    self.state = "drive"
                    self._progress = (frame.time, distance)
                else:
                    w=(math.copysign(2*self.radius*self.max_speed/self.axle,error)
                       if responsive else self._rotation_speed(error,dt))
                    left, right = -w*self.axle/(2*self.radius), w*self.axle/(2*self.radius)
                    if not self._clear(hits, margin, left, right, 2*dt):
                        if self._local is not None and frame.free_polygon is not None:
                            return self._approach(frame,previous)
                        return self._block("rotation_sweep_obstructed")
                    return self._command(left, right, remaining=distance, lateral=lateral)
            self._passage_offset(frame, hits, margin, lateral, remaining, heading)
            # A latched offset fades on departure, preserving the original endpoint.
            target_offset = self.offset * min(1., max(0., remaining / 100.))
            correction = max(-math.radians(6), min(math.radians(6),
                math.atan2(2.0*(target_offset-lateral), max(20., min(100., remaining)))))
            yaw_error = angle(drive_heading + correction - frame.heading)
            w=self._steering_rate(yaw_error,arrival_tolerance)
            # Saturate the faster wheel at its limit; only endpoint/control/collision
            # constraints reduce speed. Endpoints use sub-step wheel velocities.
            turn = w * self.axle / (2*self.radius)
            # A straight, on-track interval can cross the destination's accepted
            # region at full speed; interval capture above acknowledges it on the
            # next sensor sample. Only off-track/adjusted-endpoint control needs a
            # sub-step approach. Raw two-interval collision guards still apply.
            endpoint_limit=(self.max_speed if not adjusted and abs(lateral)<=arrival_tolerance
                            else remaining/(self.radius*dt)*.8)
            forward = max(0., min(self.max_speed-abs(turn), endpoint_limit))
            left, right = forward-turn, forward+turn
            if not self._clear(hits, margin, left, right, 2*dt):
                # Scale the same intent, never alternate speculative steering sides.
                low, high = 0., 1.
                for _ in range(8):
                    scale = (low+high)/2
                    if self._clear(hits, margin, left*scale, right*scale, 2*dt):
                        low = scale
                    else:
                        high = scale
                if low < .05:
                    if self._local is not None and frame.free_polygon is not None:
                        return self._approach(frame,previous)
                    return self._block("current_lidar_blocks_swept_footprint")
                left, right = left*low, right*low
            if self._arc_guard is not None:
                # A connected route can extend behind an occluding corner. Only
                # its observed prefix is authorized; every actual next sweep is
                # proved again before writing motors, including steering changes.
                self._arc_guard.observe(frame)
                x,y,_=self._path(left,right,self.radius,self.axle,np.array([2*dt]))
                if not self._arc_guard.clear_arc(frame,np.array([x[0],y[0]]),self.clearance):
                    return self._block('local_motion_visibility_unproved')
            return self._command(left, right, remaining=distance, lateral=lateral)


    class _FloorGuard:
        def __init__(self):
            self.pending=None
            self.clear_samples=0
            self.clear_tile=None
            self.suspects=set()
            self.settled_samples=0
            self.previous_pose=None

        def update(self, mapper, data):
            math=HeroDriving._math;np=HeroDriving._np
            if mapper is None or not hasattr(mapper,'_cs_rgb_hist'):return False
            if data.get('gps_filter_reset') or data.get('supervisor_relocated'):
                self.pending=None;self.clear_samples=0;self.suspects.clear()
                self.settled_samples=0;self.previous_pose=None
            distance=data.get('distance');position=data.get('position',{})
            if (distance is None or not math.isfinite(float(distance)) or mapper.origin_x is None
                    or not position.get('map_physical_pose_ready') or not mapper._cs_rgb_hist):
                return self.pending is not None
            orientation=data.get('orientation',{});yaw=float(orientation['yaw_raw'])
            now=float(data['time']);rate=math.inf
            if self.previous_pose is not None:
                previous_time,previous_yaw=self.previous_pose
                if now>previous_time:rate=abs(math.remainder(yaw-previous_yaw,360.))/(now-previous_time)
            self.previous_pose=now,yaw
            angle=math.radians(mapper.YAW_OFFSET_DEG+mapper.YAW_SIGN*yaw)
            rx=position.get('map_x',position['x'])-mapper.origin_x
            ry=mapper.GPS_Y_SIGN*position.get('map_y',position['y'])-mapper.origin_y
            point=np.array([rx,ry])+mapper.DIST_SENSOR_FWD_MM*np.array([math.sin(angle),math.cos(angle)])
            tile=tuple(np.rint(point/mapper.TILE_MM).astype(int))
            median=np.median(np.asarray(mapper._cs_rgb_hist),axis=0)
            pit=bool(mapper._is_pit_color(*median));bright=bool(mapper._is_bright_floor(*median))
            char=mapper.classify_color_sensor(*median)
            floor=mapper._dist_floor_cm
            cliff=floor is not None and distance>=floor+mapper.DIST_CLIFF_DELTA_CM
            tilt=abs(orientation.get('roll',0.))+abs(orientation.get('pitch',0.))
            down=math.asin(mapper.DIST_SENSOR_DOWN)-tilt;strong=False
            if down>0:
                height=mapper.DIST_SENSOR_HEIGHT_CM+2*math.hypot(mapper.DIST_SENSOR_HEIGHT_CM,3.7)*math.sin(tilt/2)
                strong=distance>=height/math.sin(down)/mapper.DIST_IR_MIN_REFLECTION+mapper.DIST_CLIFF_DELTA_CM and (floor is None or distance>=floor+mapper.DIST_CLIFF_STRONG_CM)
            cliff=cliff or strong
            scan=data.get('lidar');front=float(scan[mapper.FRONT_INDEX]) if scan is not None and len(scan)>mapper.FRONT_INDEX else 0.
            occluded=mapper.MIN_RANGE_MM<=front<=mapper.DIST_SENSOR_FWD_MM
            known=(tile==mapper.start_tile or mapper._committed_floor_tiles.get(tile) in ('3','4','b','y','g','p','o','r')
                or tile==mapper._cs_last_tile and char is not None)
            candidate=cliff and not known and not occluded and ((pit and not bright) or strong and tile!=mapper._cs_last_tile)
            self.suspects={p for p in self.suspects if mapper._committed_floor_tiles.get(p)!='2'}
            if self.pending is None and candidate and tile not in self.suspects and mapper._committed_floor_tiles.get(tile)!='2':
                self.pending=tile;self.suspects.add(tile);self.clear_samples=0;self.settled_samples=0
            if self.pending is not None:
                if mapper._committed_floor_tiles.get(self.pending)=='2':
                    self.pending=None;self.clear_samples=0
                else:
                    settled=mapper.last_speed<=mapper.CS_MAX_SPEED_MMPS and rate<=mapper.CS_MAX_YAW_RATE_DPS
                    self.settled_samples=self.settled_samples+1 if settled else 0
                    if self.settled_samples>=mapper.CS_CLIFF_CONFIRM:
                        # An inconclusive view is retained as a local hazard,
                        # not a permanent freeze. Only sweeps outside it may run.
                        self.pending=None;self.settled_samples=0
            centred=np.linalg.norm(point-np.asarray(tile)*mapper.TILE_MM)<=mapper.CS_COMMIT_GOOD_DIST_MM
            clear=(tile in self.suspects and not cliff and front>mapper.DIST_SENSOR_FWD_MM
                and centred and mapper.last_speed<=mapper.CS_MAX_SPEED_MMPS and rate<=mapper.CS_MAX_YAW_RATE_DPS)
            self.clear_samples=(self.clear_samples+1 if self.clear_tile==tile else 1) if clear else 0
            self.clear_tile=tile if clear else None
            if self.clear_samples>=mapper.CS_CLIFF_CONFIRM:
                self.suspects.discard(tile);self.clear_samples=0
            return self.pending is not None

        def rejects(self, mover, command, dt):
            math=HeroDriving._math;np=HeroDriving._np
            """Temporary candidates constrain actual short wheel sweeps, not maps."""
            if not self.suspects or not (command.left or command.right):return False
            frame=mover.last_frame
            if frame is None:return True
            driver=mover.driver
            x,y,_=driver._path(command.left,command.right,driver.radius,driver.axle,np.linspace(0,2*dt,9))
            c,s=math.cos(frame.heading),math.sin(frame.heading)
            centres=np.column_stack((frame.x+c*x-s*y,frame.y+s*x+c*y))
            lower=np.asarray(list(self.suspects),float)*120.-60.
            gap=centres[:,None,:]-np.clip(centres[:,None,:],lower,lower+120.)
            sampling=float(np.linalg.norm(np.diff(centres,axis=0),axis=1).max())/2
            return bool(np.any(np.linalg.norm(gap,axis=2)<37.+driver._normal_clearance+sampling))

    def __init__(self, bot, *, reports=False, debug_console=False,
                 debug_trace_path=None, debug_map=False, autonomous=False, **options):
        WaypointDriver = HeroDriving._Controller
        self.bot = bot
        limit = min(bot.left_motor.getMaxVelocity(), bot.right_motor.getMaxVelocity(), 6.28)
        self.driver = WaypointDriver(max_wheel_speed=limit, reports=reports, **options)
        self.driver.observation_turns = False
        self.last_frame = None
        self.enabled = False
        self._request_id = None
        self._plan_key = None
        self._revision = 0
        self._floor_guard = HeroDriving._FloorGuard()
        self._autonomous = bool(autonomous)
        self._attempted_request = None
        self._recovery = None
        self._recovery_failed = False
        self._recovery_request = None
        self._recovery_previous = None
        self._recovery_proof = HeroDriving._Maneuver(reports=False)
        self._pivot_watch = None
        self._debug_console = False
        self._debug_stream = None
        self._debug_map = None
        self._debug_active = False
        self._debug_status = None
        if debug_console or debug_trace_path is not None or debug_map:
            self.set_debug(reports=reports, console=debug_console,
                           trace_path=debug_trace_path, map=debug_map)

    def set_route(self, points):
        self._recovery = None
        self._recovery_failed = False
        self._recovery_previous = None
        self._recovery_request = None
        self.bot.stop()
        self.driver.set_route(points)
        self._adoption_watch=None
        self._held_planner_bend=None
        self.enabled = bool(self.driver.queue)
        self._request_id = None
        self._plan_key = None
        self._revision = 0

    def update_route(self, points, revision):
        """Replace the remaining destinations of this task, newest revision wins."""
        accepted = self.driver.offer_route(points, revision)
        if accepted:
            self._revision = max(self._revision, revision)
        return accepted

    @property
    def committed_crossing(self):
        """Already aligned local crossing only; never permission for a new entry."""
        return bool(self._autonomous and self.enabled and self.driver._throat is not None
                    and self.driver._throat_aligned and self.driver.target is not None)

    def _checked_crossing_exit(self, frame):
        if not self.committed_crossing or frame is None:return False
        nav=self.bot.navigator;check=getattr(nav,'_observed_segment_clear',None)
        if check is None:return False
        a=(frame.x/nav.NAV_GRID_MM+nav.c,frame.y/nav.NAV_GRID_MM+nav.c)
        b=tuple(v/nav.NAV_GRID_MM+nav.c for v in self.driver.target)
        return bool(check(a,b))

    def _retained_route_clear(self, frame):
        """Reuse the original validator; do not re-search or discard real bends."""
        if frame is None:return False
        nav=self.bot.navigator
        prepare=getattr(nav,'_planning_maps',None)
        if prepare is None or self.driver.target is None:return False
        prepare()
        engine=getattr(nav,'_native_collision_engine',None)
        if engine is None:return False
        points=[nav.current_world_mm,self.driver.target,*self.driver.queue]
        return bool(engine.check(points,frame.heading))

    def _planner_route(self):
        np = HeroDriving._np
        math = HeroDriving._math
        nav = self.bot.navigator
        if (not nav.plan_status.startswith('ok')
                or getattr(nav, '_plan_preview_owner', None) is not None):
            return None
        vertices = nav.plan_raw_points or nav.plan_corner_path
        if not vertices:
            return None
        points = [((float(x)-nav.c)*nav.NAV_GRID_MM,
                   (float(y)-nav.c)*nav.NAV_GRID_MM) for x,y in vertices]
        prefix = tuple(getattr(nav, '_plan_source_prefix', ()))
        owned = bool(prefix and tuple(map(tuple, vertices[:len(prefix)])) == prefix)
        tail = tuple(map(tuple, vertices[len(prefix):] if owned else vertices))
        connectors=points[1:len(prefix)] if owned else []
        # Recomputing the planner's dynamic source is not a new instruction.
        # Ignore its connector only when it stays inside the already accepted
        # straight leg's tracking tolerance. A real off-leg bend remains an
        # instruction even when the fixed route tail has not changed.
        driver=self.driver
        compatible=False
        captured_source=False
        if connectors and driver.start is not None and driver.direction is not None:
            relative=np.asarray(connectors)-driver.start
            cross=abs(relative[:,0]*driver.direction[1]-relative[:,1]*driver.direction[0])
            along=np.sum(relative*driver.direction,axis=1)
            current=(np.array([self.last_frame.x,self.last_frame.y]) if self.last_frame is not None else driver.start)
            length=float(np.dot(np.asarray(driver.target)-driver.start,driver.direction)) if driver.target is not None else 0.
            capture=max(2*driver.tolerance,getattr(driver,'_anchor_capture_radius',0.))+2*getattr(self.last_frame,'position_sigma_mm',0.)
            # These vertices are explicitly owned SOURCE proofs, not fixed
            # route bends. A connector on the already accepted straight leg
            # must not shorten that leg to a point already behind the body.
            compatible=bool(np.all((cross<=driver.tolerance)&(along>=-capture)&(along<=length+driver.tolerance)))
            # Pose drift during a pivot can put the freshly proved source
            # connector just behind an already captured anchor. That source
            # proof is not a new instruction to turn back and recapture it.
            if (driver._anchor is not None and math.dist(driver.start,driver._anchor)<1e-9):
                captured_source=bool(math.dist(current,driver._anchor)<=capture and
                    np.all(np.linalg.norm(np.asarray(connectors)-driver._anchor,axis=1)<=capture))
                compatible|=captured_source
        key = (nav._plan_request_id, tail, () if compatible else tuple(connectors))
        # The explicitly owned dynamic source is a pose, not a destination to
        # revisit after a worker completes. Keep its actual connector bends.
        previous_key=getattr(self,'_plan_key',None)
        continuing_tail=previous_key is not None and previous_key[:2]==key[:2]
        # A source connector can be omitted only while the old leg survives,
        # or when its anchor is already captured. A replacement route needs it.
        omit_source=compatible and (continuing_tail or captured_source)
        remaining = ((points[len(prefix):] if omit_source else points[1:]) or points[-1:]) if owned else points
        return points, remaining, key

    def accept_plan(self):
        nav = self.bot.navigator
        route = self._planner_route()
        if route is None:
            raise ValueError('Wait for a committed planner route before driving')
        # Committed safety-validated vertices, never display-only geometry.
        points, remaining, key = route
        self.set_route(remaining)
        self._request_id = nav._plan_request_id
        self._plan_key = key
        return points

    def cancel(self):
        """Disarm autonomous following and stop immediately."""
        self._autonomous = False
        self._recovery = None
        self._stop_task()

    def _stop_task(self):
        self.driver.cancel()
        self._adoption_watch=None
        self._held_planner_bend=None
        self.enabled = False
        self._request_id = None
        self._plan_key = None
        self.bot.stop()

    def handle_keys(self, data, keys, previous_keys=(), *, manual=False, turbo=False,
                    mission_active=None, ready=True, inspection_hold=False, encounter_hold=False):
        """One motor owner. G toggles following; arrows/Space disarm it.

        mission_active is the existing selector's committed-goal permission.
        None retains the standalone G/manual adapter. A new mission is accepted
        once, and a blocked request requires explicit retry or a new request.
        """
        C = HeroDriving._Command
        if not ready:
            if manual or ord(' ') in keys:
                self._autonomous=False
                self._stop_task()
            self.bot.stop()
            return C(state='waiting_for_map_render')
        if manual or ord(' ') in keys:
            if manual:
                try:
                    self._remember_recovery_view(self.frame(data))
                except (KeyError, ValueError, TypeError, IndexError, ZeroDivisionError, AttributeError):
                    pass
            self._autonomous = False
            self._recovery = None
            self._recovery_failed = False
            self._recovery_previous = None
            self._stop_task()
            if manual:
                self.bot.drive_from_keys(keys, turbo=turbo)
            return C(state='manual' if manual else 'idle', reason='cancelled')
        if ord('G') in keys and ord('G') not in previous_keys:
            if (self._autonomous or self.enabled) and not self._recovery_failed:
                self._autonomous = False
                self._recovery = None
                self._stop_task()
                return C(reason='cancelled')
            self._autonomous = True
            self._attempted_request = None
            self._recovery_failed = False
            self._recovery_previous = None
        # Recovery also runs while the selector has no safe source/goal.
        if self._autonomous:
            try:
                frame = self.frame(data)
                self._remember_recovery_view(frame)
                period = float(data.get('sensor_period_s', getattr(self.bot,'timestep',16)/1000))
                recovery = self._recover_hole(frame, period)
            except (KeyError, ValueError, TypeError, IndexError, ZeroDivisionError):
                self._stop_task()
                return C(state='blocked', reason='invalid_hero_sensor_frame')
            if recovery is not None:
                self.last_frame = frame
                self.bot._motor_command = [recovery.left, recovery.right]
                self.bot.left_motor.setVelocity(recovery.left)
                self.bot.right_motor.setVelocity(recovery.right)
                if self._debug_active:
                    self._debug_after_update(data, recovery)
                return recovery
            if frame is None:
                # Relocation rebuilds the physical GPS window. Do not inspect
                # or adopt a route until its current sensor pose is available.
                return self._update_frame(data, frame, period)
        if inspection_hold or encounter_hold:
            # Reporting is deliberate rest, not failed movement. Resume the
            # retained planner route with a fresh actuator task and timers.
            self._stop_task()
            self._attempted_request = None
            result=(C(state='yield_hold',reason='moving_obstacle_yield') if encounter_hold else
                    C(state='inspection_hold',reason='token_verification_stop'))
            self.last_frame=frame if self._autonomous else None
            if self._debug_active:self._debug_after_update(data,result)
            return result
        nav = self.bot.navigator
        if mission_active is False:
            if self.enabled:
                self._stop_task()
                self._attempted_request = None
            else:
                self.bot.stop()
            return C(state='waiting_for_plan' if self._autonomous else 'idle')
        request = nav._plan_request_id
        if (self._autonomous and not self.enabled and request != self._attempted_request
                and request != self._recovery_request):
            try:
                self.accept_plan()
                self._attempted_request = request
            except ValueError:
                pass
        if self.enabled:
            if self._request_id is not None and request != self._request_id:
                self.driver.cancel()
                self.bot.stop()
                self._request_id = request
                self._plan_key = None
                return C(state='waiting_for_plan', reason='planner_request_changed')
            crossing_only=False
            if self._request_id is not None:
                route = self._planner_route()
                if route is None:
                    # Finish only an already entered/aligned local crossing on
                    # still-observed floor. The same live driver/hole watchdogs
                    # remain authoritative; stop before adopting its old tail.
                    crossing_only=self._checked_crossing_exit(frame) if self._autonomous else False
                    if not crossing_only:
                        self.driver.cancel()
                        self._plan_key = None
                        self.bot.stop()
                        return C(state='waiting_for_plan', reason='route_not_committed')
                else:
                    points, remaining, key = route
                    if self._plan_key is None:
                        self._held_planner_bend=None
                        self.driver.set_route(remaining)
                        self._revision = 0
                    elif key != self._plan_key:
                        # A newly chosen source connection is not a new fixed
                        # route. Retain the active instruction only after the
                        # unchanged native checker proves its complete tail,
                        # current approach and turn on today's measurements.
                        source_only=key[:2]==self._plan_key[:2]
                        retained_clear=self._retained_route_clear(frame)
                        held=getattr(self,'_held_planner_bend',None)
                        bend=None if self.driver.target is None else tuple(self.driver.target)
                        passed_held=held is not None and bend!=held
                        same_goal=bool(key[1] and self._plan_key[1] and key[1][-1]==self._plan_key[1][-1])
                        hold=(not source_only and not passed_held and same_goal and retained_clear
                              and bend is not None and bool(self.driver.queue)
                              and self.driver.state in ('drive','align'))
                        if hold:
                            # Finish the already accepted intermediate bend.
                            # A fresh source attachment must not cut its corner.
                            self._held_planner_bend=bend
                        else:
                            self._held_planner_bend=None
                            if not (source_only and retained_clear):
                                self.update_route(remaining, self._revision+1)
                    if not (key != self._plan_key and getattr(self,'_held_planner_bend',None) is not None):
                        self._plan_key = key
            result = (self._update_frame(data,frame,period,crossing_only=True) if crossing_only
                      else self._update_frame(data,frame,period) if self._autonomous
                      else self.update(data))
            if result.state in ('complete', 'blocked') or result.reason == 'route_cancelled':
                self.enabled = False
                self._attempted_request = request
            return result
        self.bot.stop()
        return C(state='idle')

    def _remember_recovery_view(self, frame):
        """Keep registered raw views across manual takeover and task cancellation."""
        np=HeroDriving._np;math=HeroDriving._math
        if frame is None:
            return
        if frame.relocated:
            self._recovery_proof=HeroDriving._Maneuver(reports=False)
            return
        polygon=np.asarray(frame.free_polygon) if frame.free_polygon is not None else np.empty((0,2))
        hits=np.asarray(frame.hits)
        if (frame.scan_valid and all(map(math.isfinite,(frame.time,frame.x,frame.y,frame.heading,frame.position_sigma_mm)))
                and frame.position_sigma_mm>=0 and polygon.ndim==2 and polygon.shape[1:]==(2,)
                and len(polygon)>=32 and np.isfinite(polygon).all()
                and hits.ndim==2 and hits.shape[1:]==(2,) and np.isfinite(hits).all()
                and np.isfinite(frame.hit_margin).all() and np.all(np.asarray(frame.hit_margin)>=0)):
            self._recovery_proof.observe(frame)

    def _recover_hole(self, frame, period):
        """Bounded straight retreat; never a general reverse driving mode.

        Withdraw only an already overlapping mapped-hole safety footprint.
        Every step must strictly reduce every existing overlap, enter no new
        hole, and pass the same raw body/device sweep and visibility proofs.
        """
        np = HeroDriving._np; math = HeroDriving._math; C = HeroDriving._Command
        nav = getattr(self.bot, 'navigator', None)
        if nav is None or not hasattr(nav, 'floor'):
            return None
        if self._recovery_failed:
            return C(state='blocked', reason='hole_recovery_requires_manual_retry')
        previous = self._recovery_previous
        self._recovery_previous = frame
        def reject(reason):
            self._stop_task()
            self._recovery_failed = True
            self._recovery = None
            return C(state='blocked', reason=reason)
        if frame is None:
            return reject('hole_recovery_sensor_pose_unavailable') if self._recovery else None
        if not all(map(math.isfinite,(frame.time,frame.x,frame.y,frame.heading))):
            return reject('hole_recovery_invalid_sensor_frame')
        at = np.array([frame.x, frame.y])
        radius = 37. + self.driver._normal_clearance + .25
        grid = nav.NAV_GRID_MM
        # Only local floor cells can intersect this bounded retreat. No scan
        # of the entire map in the sensor loop.
        lo = np.maximum(np.floor((at-radius-65)/grid+nav.c).astype(int),0)
        hi = np.minimum(np.ceil((at+radius+65)/grid+nav.c).astype(int)+1, nav.floor.shape[::-1])
        ys,xs = np.nonzero(nav.floor[lo[1]:hi[1],lo[0]:hi[0]] == nav.FLOOR_HOLE)
        lower = (np.column_stack((xs+lo[0],ys+lo[1]))-nav.c)*grid
        upper = lower+grid
        separation = at-np.clip(at,lower,upper)
        distance = np.linalg.norm(separation,axis=1)
        overlap = distance < radius
        if not np.any(overlap):
            if self._recovery is not None:
                self._stop_task()
                self._recovery = None
                self._recovery_request = nav._plan_request_id
                return C(state='recovered', reason='hole_clear_request_new_plan')
            return None
        values = (frame.time,frame.x,frame.y,frame.heading,frame.left_encoder,frame.right_encoder,period,frame.position_sigma_mm)
        hits = np.asarray(frame.hits)
        polygon = np.asarray(frame.free_polygon) if frame.free_polygon is not None else np.empty((0,2))
        if (not all(map(math.isfinite,values)) or not 0 < period <= .064
                or frame.position_sigma_mm < 0 or frame.relocated or not frame.scan_valid
                or hits.ndim != 2 or hits.shape[1:] != (2,) or not np.isfinite(hits).all()
                or polygon.ndim != 2 or polygon.shape[1:] != (2,) or len(polygon)<32
                or not np.isfinite(polygon).all()):
            return reject('hole_recovery_invalid_sensor_frame')
        if previous is not None and not 0 < frame.time-previous.time <= .064:
            return reject('hole_recovery_stale_sample')
        if self._recovery is None:
            self._stop_task()  # Cancel forward intent BEFORE taking motor ownership.
            self._recovery = (frame.time,at.copy(),frame.heading,frame.time,at.copy())
            self._recovery_request = nav._plan_request_id
        started, origin, heading, progress_time, progress_at = self._recovery
        if math.dist(at,origin)>60 or frame.time-started>2.:
            return reject('hole_recovery_limit_reached')
        if abs(HeroDriving._angle(frame.heading-heading))>math.radians(5):
            return reject('hole_recovery_heading_changed')
        if math.dist(at,progress_at)>.5:
            progress_time,progress_at = frame.time,at.copy()
            self._recovery = (started,origin,heading,progress_time,progress_at)
        elif frame.time-progress_time>.5:
            return reject('hole_recovery_no_progress')
        speed = min(self.driver.max_speed,max(0.,60-math.dist(at,origin))/(self.driver.radius*2*period))
        if speed<1e-6:
            return reject('hole_recovery_limit_reached')
        length = self.driver.radius*speed*2*period
        delta = -length*np.array([math.cos(frame.heading),math.sin(frame.heading)])
        if (np.any(distance[overlap]<1e-8)
                or np.any(np.sum(separation[overlap]*delta,axis=1)<=1e-8)):
            return reject('hole_recovery_reverse_does_not_withdraw')
        sample = at+np.linspace(0,1,9)[:,None]*delta
        gap = sample[:,None,:]-np.clip(sample[:,None,:],lower,upper)
        if np.any(np.linalg.norm(gap[:,~overlap],axis=2)<radius+length/16):
            return reject('hole_recovery_new_hole_in_sweep')
        raw_margin = np.asarray(frame.hit_margin)
        margin = np.broadcast_to(raw_margin,(len(hits),))
        if not np.isfinite(raw_margin).all() or not np.isfinite(margin).all() or np.any(margin<0):
            return reject('hole_recovery_invalid_scan_uncertainty')
        local_delta = np.array([-length,0.])
        proof = self._recovery_proof
        proof.tight_attempt = False
        proof._obstacle_frame = None
        hits,margin,_ = proof._obstacles(frame)
        for offset,r in ((np.zeros(2),37.),(np.array([37.,0.]),3.)):
            if not HeroDriving._capsule_clear(hits,margin,offset,offset+local_delta,r+self.driver._normal_clearance):
                return reject('hole_recovery_rear_obstructed')
        def inside(points,padding):
            result = np.zeros(len(points),bool)
            for offset,r in ((np.zeros(2),37.),(np.array([37.,0.]),3.)):
                relative=points-offset
                t=np.clip(np.sum(relative*local_delta,axis=1)/(length*length),0.,1.)
                result |= np.linalg.norm(relative-t[:,None]*local_delta,axis=1)<=r+padding
            return result
        if not proof._sweep_known(frame,np.array([-40.-length,-40.]),np.array([40.,40.]),inside):
            return reject('hole_recovery_rear_visibility_unproved')
        return C(left=-speed,right=-speed,
                 state='recovering',reason='withdrawing_mapped_hole_overlap')

    def frame(self, data):
        DriveFrame = HeroDriving._Frame
        math = HeroDriving._math
        np = HeroDriving._np
        mapper = self.bot.mapper
        position, orientation = data['position'], data['orientation']
        if not position.get('map_physical_pose_ready') or mapper.origin_x is None:
            return None
        yaw = float(orientation['yaw_raw'])
        # Hero's map forward is (-sin(yaw), cos(yaw)), hence atan2 = 90+yaw.
        heading = math.pi/2 + math.radians(yaw)
        hres = int(data['lidar_hres'])
        raw = np.asarray(data['lidar_raw_m'], dtype=float)
        layers = len(raw)//hres if hres else 0
        if not layers:
            return None
        layer = min(mapper.FRONT_INDEX//hres, layers-1)
        ranges = raw[layer*hres:(layer+1)*hres]*1000
        # Missing/NaN/zero rays are unknown. Infinity is a valid no-return ray.
        valid_scan = (len(ranges) == hres and hres >= 128
                      and not np.any(np.isnan(ranges) | (ranges <= 0))
                      and data['lidar_fov'] > 1.99*math.pi)
        indices = np.flatnonzero(np.isfinite(ranges) & (ranges > 0) & (ranges <= 500))
        # Reuse Hero's renderer/mount calibration, not its averaged map.
        offset,all_rays=mapper._lidar_projection(yaw,data,np.arange(hres))
        points = offset + all_rays[indices]*ranges[indices, None]
        forward = np.array([math.cos(heading), math.sin(heading)])
        left = np.array([-forward[1], forward[0]])
        hits = np.column_stack((points[:,0]*forward[0]+points[:,1]*forward[1],
                                points[:,0]*left[0]+points[:,1]*left[1]))
        # Classify the robot's own camera housings in its rigid 3D frame.
        # Their world-horizontal shadows move when the body tilts; a fixed
        # planar mask can mistake the housing itself for a collision hazard.
        body=(mapper._lidar_sample_vectors[indices] @ mapper.LIDAR_MOUNT_ROTATION.T
              * ranges[indices,None] + mapper.LIDAR_MOUNT_MM)
        self_hit=(body[:,2]**2+(body[:,1]-55.5)**2+(abs(body[:,0])-30)**2 < 7**2)
        hits = hits[~self_hit]
        # Half a beam footprint covers angular sampling; raw floating ranges
        # avoid the extra integer millimetre rounding in data['lidar'].
        margin = (.25 + ranges[indices]*math.sin(data['lidar_fov']/hres/2))[~self_hit]
        reach=np.minimum(ranges,500.)
        reach=np.where(np.isnan(reach)|(reach<=0),0,reach)
        reach=np.maximum(0,reach-(.25+reach*math.sin(data['lidar_fov']/hres/2)))
        visible=offset+all_rays*reach[:,None]
        polygon=np.column_stack((visible[:,0]*forward[0]+visible[:,1]*forward[1],
                                 visible[:,0]*left[0]+visible[:,1]*left[1]))
        return DriveFrame(float(data['time']),
            float(position['map_body_x_float'])-mapper.origin_x,
            mapper.GPS_Y_SIGN*float(position['map_body_y_float'])-mapper.origin_y,
            heading, float(data['encoders']['left']), float(data['encoders']['right']),
            hits, valid_scan, margin, bool(data.get('gps_filter_reset') or data.get('supervisor_relocated')),polygon,
            float(position.get('map_position_sigma_mm') or 0.))

    def update(self, data):
        DriveCommand = HeroDriving._Command
        math = HeroDriving._math
        np = HeroDriving._np
        try:
            frame = self.frame(data)
            period=float(data.get('sensor_period_s',getattr(self.bot,'timestep',16)/1000))
        except (KeyError, ValueError, TypeError, IndexError, ZeroDivisionError):
            self.driver._block('invalid_hero_sensor_frame')
            self.bot.stop()
            return self.driver._command()
        return self._update_frame(data,frame,period)

    def _update_frame(self, data, frame, period, *, crossing_only=False, command=None):
        DriveCommand = HeroDriving._Command
        math = HeroDriving._math
        np = HeroDriving._np
        if self._debug_map is not None:
            self._debug_map.poll(data, self.driver._last_command)
        self.last_frame = frame
        if frame is None:
            self.bot.stop()
            return DriveCommand(state='waiting_for_sensors', target=self.driver.target)
        floor_hold = self._floor_guard.update(getattr(self.bot,'mapper',None),data)
        result = self.driver.update(frame,initial_dt=period) if command is None else command
        if not all(math.isfinite(v) for v in (result.left,result.right,
                result.remaining_mm,result.lateral_mm,result.offset_mm)):
            result=self.driver._block('nonfinite_control_geometry')
        if crossing_only and self.driver._throat is None and result.state!='blocked':
            self._stop_task()
            result=DriveCommand(state='recovered',reason='crossing_clear_request_new_plan')
        if floor_hold or self._floor_guard.rejects(self,result,period):
            # Consume actual sensor/pose progress while commanding zero wheels.
            # Keep the task enabled so a settled/cleared view can resume it.
            result = self.driver._command(reason='floor_observation_hold' if floor_hold else 'floor_candidate_sweep_veto')
        # LiDAR does not see a floor hole. Keep the planner's existing hole
        # labels as a separate veto on the actual short commanded sweep; they
        # never choose steering or substitute averaged occupancy for LiDAR.
        nav = getattr(self.bot, 'navigator', None)
        if nav is not None and (result.left or result.right):
            dt = float(data.get('sensor_period_s', self.bot.timestep/1000))
            x, y, _ = self.driver._path(result.left, result.right, self.driver.radius,
                self.driver.axle, np.linspace(0, 2*dt, 9))
            c, s = math.cos(frame.heading), math.sin(frame.heading)
            centres = np.column_stack((frame.x+c*x-s*y, frame.y+s*x+c*y))
            grid, centre = nav.NAV_GRID_MM, nav.c
            radius = 37+self.driver.clearance+.25
            active=self.driver._local_driver or self.driver
            if active._turn_departure:
                speed=max(self.driver.radius*abs(result.left+result.right)/2,active._linear_rate)
                yaw=max(abs(self.driver.radius*(result.right-result.left)/self.driver.axle),abs(active._angular_rate))
                radius+=2*dt*speed*min(1.,2*dt*yaw)
            low = np.floor((centres.min(axis=0)-radius)/grid+centre).astype(int)
            high = np.floor((centres.max(axis=0)+radius)/grid+centre).astype(int)
            x0,y0 = np.maximum(low,0);x1,y1 = np.minimum(high+1,nav.floor.shape[::-1])
            py,px = np.nonzero(nav.floor[y0:y1,x0:x1] == nav.FLOOR_HOLE)
            if len(px):
                origins = (np.column_stack((px+x0,py+y0))-centre)*grid
                delta = np.maximum(abs(centres[:,None,:]-(origins+grid/2))-grid/2,0)
                if np.any(np.sum(delta**2,axis=2) < radius**2):
                    result = self.driver._block('known_floor_hole_in_swept_footprint')
        result=self._guard_pivot(frame,result,period)
        # Route handoff replaces the nested controller and its local timers.
        # Repeated align/handoff cannot erase task-level translation evidence.
        watch=getattr(self,'_adoption_watch',None)
        if result.state not in ('align','handoff'):
            self._adoption_watch=None
        elif watch is None or math.dist((frame.x,frame.y),watch[1])>max(self.driver.tolerance,2*frame.position_sigma_mm):
            self._adoption_watch=(frame.time,(frame.x,frame.y))
        elif frame.time-watch[0]>2.:
            result=self.driver._block('no_route_adoption_progress')
        # Hero.motor normalizes small inputs UP to max_speed. Direct velocity
        # writes preserve slow final-step and angular corrections exactly.
        self.bot._motor_command = [result.left, result.right]
        self.bot.left_motor.setVelocity(result.left)
        self.bot.right_motor.setVelocity(result.right)
        if self._debug_active:
            self._debug_after_update(data, result)
        return result

    def inspection_turn(self, data, heading):
        """A mission-authorized heading, with the ordinary live safety guards.

        The caller proves the complete known-space turn and bounds its duration.
        This adapter retains current raw body/device, floor and hole vetoes.
        """
        np,math,C=HeroDriving._np,HeroDriving._math,HeroDriving._Command
        try:
            frame=self.frame(data);dt=float(data.get('sensor_period_s',.016))
            linear_rate=float(data.get('position',{}).get('speed_mmps',0.))
        except (KeyError,ValueError,TypeError,IndexError,ZeroDivisionError):
            self.bot.stop();return C(state='blocked',reason='invalid_inspection_turn_frame')
        previous=getattr(self,'_inspection_turn_previous',None)
        if frame is None:
            self.bot.stop();return C(state='blocked',reason='invalid_inspection_turn_frame')
        hits=np.asarray(frame.hits)
        polygon=np.asarray(frame.free_polygon) if frame.free_polygon is not None else None
        values=(frame.time,frame.x,frame.y,frame.heading,frame.left_encoder,frame.right_encoder,
                frame.position_sigma_mm,dt,heading,linear_rate)
        if (not all(map(math.isfinite,values)) or not 0<dt<=.064 or frame.position_sigma_mm<0 or linear_rate<0
                or not frame.scan_valid or frame.relocated
                or hits.ndim!=2 or hits.shape[1:]!=(2,) or not np.isfinite(hits).all()
                or (previous is not None and not 0<frame.time-previous.time<=.064)
                or (polygon is not None and (polygon.ndim!=2 or polygon.shape[1:]!=(2,)
                    or len(polygon)<32 or not np.isfinite(polygon).all()))):
            self.bot.stop();return C(state='blocked',reason='invalid_inspection_turn_frame')
        raw_margin=np.asarray(frame.hit_margin,float)
        if not np.isfinite(raw_margin).all() or np.any(raw_margin<0):
            self.bot.stop();return C(state='blocked',reason='invalid_inspection_turn_frame')
        try:margin=np.broadcast_to(raw_margin,(len(hits),))
        except ValueError:
            self.bot.stop();return C(state='blocked',reason='invalid_inspection_turn_frame')
        if not np.isfinite(margin).all() or np.any(margin<0):
            self.bot.stop();return C(state='blocked',reason='invalid_inspection_turn_frame')
        self._inspection_turn_previous=frame
        error=HeroDriving._angle(heading-frame.heading)
        if abs(error)<=self.driver.heading_tolerance:
            self.bot.stop();return C(state='inspection_hold',reason='inspection_heading_reached')
        control=self.driver
        control._angular_rate=(HeroDriving._angle(frame.heading-previous.heading)/(frame.time-previous.time)
            if previous is not None else 0.)
        control._linear_rate=linear_rate
        control._unwrapped_heading=frame.heading;control._rotation_target=heading
        w=control._rotation_speed(error,dt);speed=w*control.axle/(2*control.radius)
        margin=margin+2*frame.position_sigma_mm
        if not control._departure_clear(hits,margin,-speed,speed,dt):
            self.bot.stop();return C(state='blocked',reason='inspection_turn_raw_sweep_blocked')
        result=C(left=-speed,right=speed,state='inspection_align',reason='token_camera_alignment')
        return self._update_frame(data,frame,dt,command=result)

    def _guard_pivot(self, frame, result, period):
        """A shortest-heading alignment cannot require repeated full circles.

        Use measured yaw, not elapsed time or wheel odometry. Preserve ordinary
        180-degree turns plus two control intervals of possible body yaw. This
        guard stops an invalid intent; it does not invent replacement steering.
        """
        math=HeroDriving._math
        control=self.driver._local_driver or self.driver
        if control.target is None:
            self._pivot_watch=None
            return result
        at=(frame.x,frame.y);target=control.target
        watch=self._pivot_watch
        if (watch is None or math.dist(at,watch[0])>self.driver.tolerance
                or math.dist(target,watch[1])>self.driver.tolerance):
            self._pivot_watch=(at,target,frame.heading,0.)
            return result
        rotation=watch[3]+HeroDriving._angle(frame.heading-watch[2])
        self._pivot_watch=(watch[0],watch[1],frame.heading,rotation)
        allowance=2*period*2*self.driver.radius*self.driver.max_speed/self.driver.axle
        if result.left*result.right<0 and abs(rotation)>math.pi+allowance+self.driver.heading_tolerance:
            return self.driver._block('excess_rotation_for_leg')
        return result

    def set_debug(self, *, reports=False, console=False, trace_path=None, map=False):
        """Replace diagnostic settings. No diagnostic collection is left running off."""
        if self._debug_stream is not None:
            self._debug_stream.close()
            self._debug_stream = None
        if not map and self._debug_map is not None:
            self._debug_map.close()
            self._debug_map = None
        self._debug_console = bool(console)
        self._debug_status = None
        self._debug_active = bool(console)
        for control in (self.driver, self.driver._local_driver):
            if control is None:
                continue
            control.reports_enabled = bool(reports)
            for local in (control._local, control._arc_guard):
                if local is None:
                    continue
                local.reports_enabled = bool(reports)
                local._details = dict(local._decision) if reports else None
                local._search_details = dict(local._search_decision) if reports else None
        if trace_path is not None:
            import json
            self._debug_json = json
            self._debug_stream = open(trace_path, 'w', buffering=1)
            self._debug_active = True
        if map and self._debug_map is None:
            self._debug_map = self._Map(self.bot, self)

    def _debug_after_update(self, data, result):
        if self._debug_console:
            status = (result.state, result.reason, result.target)
            if status != self._debug_status:
                print('Driving:', result)
                self._debug_status = status
        if self._debug_stream is not None:
            control=self.driver._local_driver or self.driver
            self._debug_stream.write(self._debug_json.dumps(
                {'time': data['time'], **result.__dict__, 'control': {
                    'heading':control._unwrapped_heading,
                    'rotation_target':control._rotation_target,
                    'target':control.target,
                    'start':None if control.start is None else list(control.start),
                    'direction':None if control.direction is None else list(control.direction),
                    'anchor':control._anchor,
                    'anchor_capture_mm':getattr(control,'_anchor_capture_radius',None),
                    'remaining_points':list(control.queue),
                    'revision':control.route_revision,
                    'local':control is not self.driver,
                    'turn_departure':control._turn_departure},
                 'measured_pose':None if self.last_frame is None else {
                    name:getattr(self.last_frame,name) for name in ('x','y','heading',
                        'left_encoder','right_encoder','position_sigma_mm')},
                 'planner_source_prefix':getattr(getattr(self.bot,'navigator',None),'_plan_source_prefix',()),
                 'planner_vertices':getattr(getattr(self.bot,'navigator',None),'plan_raw_points',()),
                 'held_planner_bend':getattr(self,'_held_planner_bend',None)}) + '\n')

    def close(self):
        """Stop motors and close only this driver's enabled diagnostic resources."""
        self.cancel()
        self.set_debug()

    class _Map:
        """Optional click-to-drive debug worker; never created on the default path."""
        def __init__(self, bot, driving):
            import os, socket, subprocess, sys, threading
            from queue import Queue
            self.bot, self.driving = bot, driving
            self.pending = Queue(maxsize=1)
            self.events = Queue()
            self.stopped = threading.Event()
            parent, child = socket.socketpair()
            self.socket, self.last, self.points = parent, 0., []
            source = sys.modules[driving.__class__.__module__].__file__
            bootstrap = """import ast,sys,types
p=sys.argv[2]
t=ast.parse(open(p).read())
c=next(n for n in t.body if isinstance(n,ast.ClassDef) and n.name=='HeroDriving')
m=types.ModuleType('_driving_map_worker');m.__file__=p;sys.modules[m.__name__]=m
exec(compile(ast.Module(body=[c],type_ignores=[]),p,'exec'),m.__dict__)
m.HeroDriving._Map.worker(int(sys.argv[1]))
"""
            self.process = subprocess.Popen(
                [sys.executable, '-c', bootstrap, str(child.fileno()), source],
                pass_fds=(child.fileno(),), env=dict(os.environ, PYTHONDONTWRITEBYTECODE='1'))
            child.close()
            threading.Thread(target=self._send, daemon=True).start()
            threading.Thread(target=self._receive, daemon=True).start()

        def _send(self):
            import pickle
            from queue import Empty
            try:
                with self.socket.makefile('wb') as stream:
                    while not self.stopped.is_set():
                        try:
                            item = self.pending.get(timeout=.1)
                        except Empty:
                            continue
                        pickle.dump(item, stream, protocol=5)
                        stream.flush()
            except (OSError, EOFError, ValueError, pickle.UnpicklingError):
                self.stopped.set()

        def _receive(self):
            import pickle
            try:
                with self.socket.makefile('rb') as stream:
                    while not self.stopped.is_set():
                        self.events.put(pickle.load(stream))
            except (OSError, EOFError, ValueError, pickle.UnpicklingError):
                self.stopped.set()

        def handle(self, event):
            if event['kind'] == 'stop':
                self.points = []
                self.driving.cancel()
            elif event['kind'] == 'point':
                point = tuple(map(float, event['point']))
                if not event.get('append'):
                    self.points = [point]
                    self.driving.set_route(self.points)
                else:
                    driver = self.driving.driver
                    remaining = (list(driver._pending_route[0]) if driver._pending_route is not None else
                                 ([driver.target] if driver.target is not None else []) + list(driver.queue))
                    remaining.append(point)
                    self.points = remaining
                    if not self.driving.enabled:
                        self.driving.set_route(remaining)
                    else:
                        self.driving.update_route(remaining, self.driving._revision + 1)

        def poll(self, data, result):
            from queue import Empty, Full
            while True:
                try:
                    self.handle(self.events.get_nowait())
                except Empty:
                    break
            if data['time'] - self.last < .35 or self.stopped.is_set():
                return
            packet = self._map_packet(data, result)
            self.last = data['time']
            try:
                self.pending.get_nowait()
            except Empty:
                pass
            try:
                self.pending.put_nowait(packet)
            except Full:
                pass

        def _map_packet(self, data, result):
            np = HeroDriving._np
            nav, mapper = self.bot.navigator, self.bot.mapper
            x0, x1, y0, y1 = nav._used_bbox()
            grid = nav.NAV_GRID_MM
            image = nav._compose_cell_image(nav.floor[y0:y1,x0:x1],
                nav.cell_state[y0:y1,x0:x1], nav.current_beam[y0:y1,x0:x1]).copy()
            material = np.argwhere(mapper.occupied_geometry())
            walls = np.column_stack(((material[:,1]-mapper.c)*mapper.GRID_MM,
                                     (material[:,0]-mapper.c)*mapper.GRID_MM))
            driver = self.driving.driver
            return dict(image=image, extent=[(x0-nav.c)*grid, (x1-nav.c)*grid,
                (y0-nav.c)*grid, (y1-nav.c)*grid], walls=walls, pose=nav.current_world_mm,
                route=([driver.target] if driver.target is not None else [])+list(driver.queue),
                local=result.local_target, state=result.state, reason=result.reason)

        def close(self):
            import socket, subprocess
            self.stopped.set()
            try:
                self.socket.shutdown(socket.SHUT_RDWR)
            except OSError:
                pass
            self.socket.close()
            try:
                self.process.wait(timeout=2)
            except subprocess.TimeoutExpired:
                self.process.terminate()
                self.process.wait(timeout=2)

        @staticmethod
        def worker(fd):
            import pickle, socket, threading
            from queue import Queue, Empty
            import matplotlib.pyplot as plt
            from matplotlib.widgets import CheckButtons
            np = HeroDriving._np
            sock = socket.socket(fileno=fd)
            incoming, stopped = Queue(maxsize=1), threading.Event()
            def read():
                try:
                    with sock.makefile('rb') as stream:
                        while True:
                            packet = pickle.load(stream)
                            try:
                                incoming.get_nowait()
                            except Empty:
                                pass
                            incoming.put_nowait(packet)
                except (EOFError, OSError, ValueError, pickle.UnpicklingError):
                    stopped.set()
            threading.Thread(target=read, daemon=True).start()
            fig, ax = plt.subplots(figsize=(8,7))
            fig.subplots_adjust(bottom=.13)
            fig.canvas.manager.set_window_title('Direct point following — driving debug')
            choice = CheckButtons(fig.add_axes([.12,.025,.31,.08]), ['Snap clicks to 60 mm'], [True])
            with sock.makefile('wb') as output:
                def send(event):
                    try:
                        pickle.dump(event, output, protocol=5)
                        output.flush()
                    except (OSError, ValueError):
                        stopped.set()
                def click(event):
                    if event.button == 3:
                        send(dict(kind='stop'))
                        return
                    if event.inaxes != ax or ax.get_navigate_mode():
                        return
                    if event.button != 1 or event.xdata is None or event.ydata is None:
                        return
                    point = [event.xdata,event.ydata]
                    if choice.get_status()[0]:
                        point = [round(value/60)*60 for value in point]
                    send(dict(kind='point',point=point,append=bool(event.key and 'shift' in event.key)))
                def key(event):
                    if event.key in (' ', 'escape'):
                        send(dict(kind='stop'))
                fig.canvas.mpl_connect('button_press_event',click)
                fig.canvas.mpl_connect('key_press_event',key)
                fig.canvas.mpl_connect('close_event',lambda event:send(dict(kind='stop')))
                plt.show(block=False)
                while not stopped.is_set() and plt.fignum_exists(fig.number):
                    try:
                        current = incoming.get_nowait()
                    except Empty:
                        pass
                    else:
                        ax.clear()
                        ax.imshow(current['image'],origin='lower',extent=current['extent'],interpolation='nearest')
                        walls = np.asarray(current['walls'])
                        if len(walls):
                            ax.scatter(walls[:,0],walls[:,1],s=4,c='#303942',marker='s')
                        route, pose = np.asarray(current['route']), current['pose']
                        if len(route):
                            path = np.vstack(([pose] if pose is not None else [],route))
                            ax.plot(path[:,0],path[:,1],'o-',c='#b04d00')
                        if pose is not None:
                            ax.add_patch(plt.Circle(pose,37,fill=False,color='#1364c7'))
                            ax.plot(*pose,'o',color='#1364c7')
                        if current['local'] is not None:
                            ax.plot(*current['local'],'D',color='#9b2db2',markersize=8)
                        ax.set_aspect('equal')
                        fig.canvas.draw_idle()
                    plt.pause(.03)
            plt.close(fig)
            sock.close()
# END EMBEDDED HeroDriving class




































# BEGIN EMBEDDED EXPLORATION
"""Map-derived exploration suggestions; no motor commands or planner replacement.

The only route producer is NavigationMapper._plan_path. Its existing current-map
adoption and bend annotations remain authoritative. World/judge reference data is
deliberately absent from this module.
"""
import collections
import concurrent.futures
import contextlib
import hashlib
import json
import math
from pathlib import Path
import struct
import sys
import threading
import time

import numpy as np
import cv2
from scipy import ndimage
from scipy.sparse import csr_matrix
from scipy.sparse.csgraph import dijkstra


def _finite_log(value):
    """Rare diagnostic fallback: preserve invalid numbers explicitly as text."""
    if isinstance(value,(float,np.floating)) and not math.isfinite(value):return str(value)
    if isinstance(value,dict):return {k:_finite_log(v) for k,v in value.items()}
    if isinstance(value,(tuple,list,np.ndarray)):return [_finite_log(v) for v in value]
    return value


class ExplorationLog:
    def __init__(self, path=None):
        self.stream = None
        self.lock = threading.Lock()
        self.last = {}
        self.io = dict(events=0, bytes=0, max_lock_wait_ms=0., max_encode_ms=0., max_write_ms=0.)
        if path is not None:
            path = Path(path)
            path.parent.mkdir(parents=True, exist_ok=True)
            self.stream = path.open('a', buffering=65536)

    def event(self, event, **values):
        if self.stream is not None:
            waiting=time.perf_counter_ns()
            with self.lock:
                acquired=time.perf_counter_ns()
                payload=dict(event=event,wall_ns=acquired,**values)
                encode=lambda v:v.item() if isinstance(v,np.generic) else list(v)
                try:line=json.dumps(payload,allow_nan=False,default=encode)+'\n'
                except ValueError:line=json.dumps(_finite_log(payload),allow_nan=False,default=encode)+'\n'
                encoded=time.perf_counter_ns()
                self.stream.write(line)
                # Persist every completed control frame and critical decision,
                # batching its stage records instead of flushing each line.
                if event in ('complete_frame','state','error','target_committed','selection_completed','log_io_summary'):
                    self.stream.flush()
                written=time.perf_counter_ns()
                self.io['events']+=1;self.io['bytes']+=len(line)
                self.io['max_lock_wait_ms']=max(self.io['max_lock_wait_ms'],(acquired-waiting)/1e6)
                self.io['max_encode_ms']=max(self.io['max_encode_ms'],(encoded-acquired)/1e6)
                self.io['max_write_ms']=max(self.io['max_write_ms'],(written-encoded)/1e6)

    @contextlib.contextmanager
    def stage(self, name, **values):
        if self.stream is None:
            yield
            return
        start, cpu = time.perf_counter_ns(), time.thread_time_ns()
        try:
            yield
        finally:
            elapsed = (time.perf_counter_ns()-start)/1e6
            self.last[name] = elapsed
            self.event('timing', stage=name, wall_ms=elapsed,
                       cpu_ms=(time.thread_time_ns()-cpu)/1e6, **values)

    def close(self):
        if self.stream is not None:
            self.event('log_io_summary',**self.io)
            self.stream.close()
            self.stream = None


class ExplorationClock:
    """Tap the existing receiver without consuming/reordering its L packets."""
    def __init__(self, receiver=None, log=None):
        self.receiver = receiver
        self.log = log
        self.packet = None
        self.now = 0.

    def getBytes(self):
        packet = self.receiver.getBytes()
        if len(packet) == struct.calcsize('c f i i') and packet[:1] == b'G':
            _, score, game, real = struct.unpack('c f i i', packet)
            if math.isfinite(score) and 0 <= game <= 480 and 0 <= real <= 600:
                self.packet = dict(score=score, game=game, real=real,
                                   received_wall=time.monotonic(), received_sim=self.now)
                if self.log is not None:
                    self.log.event('competition_clock',time=self.now,game_left_s=game,real_left_s=real)
        return packet

    def __getattr__(self, name):
        return getattr(self.receiver, name)


def _candidate_key(kind, point):
    return kind, int(point[0]), int(point[1])


def _enclosure_labels(engine,shape,lo,body):
    fine=2.5;scale=6;opened=np.ones(shape,bool)
    centres=np.indices(shape).transpose(1,2,0)[:,:,::-1]*scale+3
    for rects,radius in [(engine.rectangles,body-2. if engine.gateway_contact else body),(engine.pits,body)]:
        if not len(rects):continue
        image=np.ones((int(shape[0]*scale+1),int(shape[1]*scale+1)),np.uint8)
        for rect in rects:
            a,b=np.rint((rect.reshape(2,2)-lo*15)/fine).astype(int)
            cv2.rectangle(image,tuple(a),tuple(b),0,-1)
        distance=cv2.distanceTransform(image,cv2.DIST_L2,cv2.DIST_MASK_PRECISE)
        upper=distance[centres[:,:,1],centres[:,:,0]]*fine+math.sqrt(2)*(fine/2+7.5)+1e-4
        opened&=upper>=radius
    labels,_=ndimage.label(opened,np.ones((3,3)))
    return labels

def exploration_envelope(nav,engine):
    # Every painted pixel is within sqrt(2)*1.25mm of measured material.
    # Distance at a15mm square's centre plus that bound and its half-diagonal
    # is an upper bound for EVERY body position in that square. Remove only
    # squares proved fully inside material's configuration obstacle.
    yy,xx=np.nonzero(nav.cell_state!=nav.CELL_UNKNOWN)
    if not len(xx):return np.ones_like(nav.cell_state,dtype=bool),dict(enclosed=False,potential_unknown=None)
    boxes=np.concatenate((engine.rectangles,engine.pits))
    points=np.column_stack((xx,yy))*30-nav.c*30
    if len(boxes):points=np.vstack((points,boxes[:,:2],boxes[:,2:]))
    lo=np.floor((points.min(axis=0)-90)/15).astype(int);hi=np.ceil((points.max(axis=0)+90)/15).astype(int)
    shape=(hi-lo)[::-1]
    digest=hashlib.blake2b(digest_size=16)
    for value in (engine.rectangles,engine.pits):digest.update(value.tobytes())
    digest.update(np.asarray([*lo,*hi,nav.ROBOT_RADIUS_MM,bool(engine.gateway_contact)],dtype=float).tobytes())
    key=digest.digest();cache=getattr(nav,'_exploration_geometry_cache',None)
    if cache is not None and cache[0]==key:labels=cache[1]
    else:
        labels=_enclosure_labels(engine,shape,lo,nav.ROBOT_RADIUS_MM)
        labels.flags.writeable=False;nav._exploration_geometry_cache=(key,labels)
    at=np.floor(np.asarray(nav.current_world_mm)/15).astype(int)-lo
    if np.any(at<0) or np.any(at>=np.array(shape[::-1])):return np.ones_like(nav.cell_state,dtype=bool),dict(enclosed=False,potential_unknown=None)
    label=labels[at[1],at[0]]
    if not label:return np.ones_like(nav.cell_state,dtype=bool),dict(enclosed=False,potential_unknown=None)
    reach=labels==label
    potential=np.zeros((2*nav.n,2*nav.n),bool)
    offset=lo+2*nav.c;end=offset+np.array(shape[::-1]);a=np.maximum(offset,0);b=np.minimum(end,2*nav.n)
    potential[a[1]:b[1],a[0]:b[0]]=reach[a[1]-offset[1]:b[1]-offset[1],a[0]-offset[0]:b[0]-offset[0]]
    potential=potential.reshape(nav.n,2,nav.n,2).any(axis=(1,3))
    exterior=bool(label and(any(labels[0]==label)or any(labels[-1]==label)or any(labels[:,0]==label)or any(labels[:,-1]==label)))
    return potential,dict(enclosed=bool(label and not exterior),qualification='Optimistic measured robot-centre enclosure; not an invented field boundary',potential_unknown=int(np.count_nonzero(potential&(nav.cell_state==nav.CELL_UNKNOWN))))


def terminal_wall_remnants(nav, potential):
    """Small wall-side remnants with no onward unknown-space opening nearby."""
    wall=(nav.cell_state==nav.CELL_UNVISITED)&(nav.floor!=nav.FLOOR_HOLE)&(
        nav.wall_v[:,:-1]|nav.wall_v[:,1:]|nav.wall_h[:-1]|nav.wall_h[1:])
    n=nav.n;lattice=np.zeros((2*n+1,2*n+1),bool)
    lattice[1::2,1::2]=wall
    lattice[1::2,2:-1:2]=wall[:,:-1]&wall[:,1:]&~nav.wall_v[:,1:-1]
    lattice[2:-1:2,1::2]=wall[:-1]&wall[1:]&~nav.wall_h[1:-1]
    labels,_=ndimage.label(lattice);labels=labels[1::2,1::2]
    opening=ndimage.maximum_filter((nav.cell_state==nav.CELL_UNKNOWN)&potential,size=7,mode='constant')
    low=np.zeros_like(labels)
    for label,slices in enumerate(ndimage.find_objects(labels),1):
        if slices is None:continue
        ys,xs=slices;patch=labels[ys,xs]==label
        if max(ys.stop-ys.start,xs.stop-xs.start)*nav.NAV_GRID_MM>120.:continue
        if np.any(opening[ys,xs]&patch) or np.any((nav.floor[ys,xs]==getattr(nav,'FLOOR_CHECKPOINT',-1))&patch):continue
        low[ys,xs][patch]=label
    return low


def face_cells(nav, faces):
    """Decode the router's existing oriented wall-side IDs, without new views."""
    faces=np.asarray(faces,dtype=np.int64);base=2*nav.n*(nav.n+1)
    vertical=faces<base;index=np.where(vertical,faces,faces-base)//2
    width=np.where(vertical,nav.n+1,nav.n);x=index%width;y=index//width
    x-=vertical&(faces%2==0);y-=~vertical&(faces%2==0)
    return x,y


def available_faces(nav, faces):
    """Selection reward only: failed inspections never alter measured walls."""
    mask=getattr(nav,'_exploration_unavailable',None)
    if mask is None:return faces
    faces=np.asarray(faces,dtype=np.int64);x,y=face_cells(nav,faces)
    return faces[~mask[y,x]]


def isolated_floor_remnants(nav, potential, enclosure):
    """One-view mapping leftovers, never an opening to another unknown region.

    This is a finite trip-value policy, not visited/free evidence. Keep an
    unbounded discovery frontier, unseen wall side or checkpoint eligible.
    """
    low=np.zeros_like(potential)
    if not enclosure.get('enclosed'):return low
    unknown=(nav.cell_state==nav.CELL_UNKNOWN)&potential
    labels,count=ndimage.label(unknown)
    wall_unseen=(nav.cell_state!=2)&(nav.wall_v[:,:-1]|nav.wall_v[:,1:]
        |nav.wall_h[:-1]|nav.wall_h[1:])
    near_wall=ndimage.maximum_filter(wall_unseen,size=7,mode='constant')
    mapper=nav._planning_mapper;ratio=nav.NAV_GRID_MM/mapper.GRID_MM
    for label,slices in enumerate(ndimage.find_objects(labels),1):
        if slices is None:continue
        ys,xs=slices
        if max(ys.stop-ys.start,xs.stop-xs.start)*nav.NAV_GRID_MM>120.:continue
        patch=labels[ys,xs]==label
        if np.any(near_wall[ys,xs]&patch) or np.any((nav.floor[ys,xs]==getattr(nav,'FLOOR_CHECKPOINT',-1))&patch):continue
        safe=True
        for y,x in np.argwhere(patch)+[ys.start,xs.start]:
            a=np.floor((np.array([x,y])-nav.c)*ratio+mapper.c).astype(int)
            b=np.ceil((np.array([x+1,y+1])-nav.c)*ratio+mapper.c).astype(int)
            if np.any(a<0) or np.any(b>mapper.n) or not np.all(mapper.logodds[a[1]:b[1],a[0]:b[0]]<=mapper.FREE_T):
                safe=False;break
        if safe:low[ys,xs]|=patch
    return low


def wall_observation_cells(nav):
    state,floor,wh,wv=nav.cell_state,nav.floor,nav.wall_h,nav.wall_v
    wall_cells=((state==nav.CELL_UNVISITED)&(floor!=nav.FLOOR_HOLE)
        &(wv[:,:-1]|wv[:,1:]|wh[:-1]|wh[1:]))
    supported=np.zeros_like(wall_cells)
    supported[:,1:]|=wall_cells[:,:-1]&~wv[:,1:-1]
    supported[:,:-1]|=wall_cells[:,1:]&~wv[:,1:-1]
    supported[1:]|=wall_cells[:-1]&~wh[1:-1]
    supported[:-1]|=wall_cells[1:]&~wh[1:-1]
    return wall_cells,wall_cells&supported


def observation_candidates(nav, centre_seen):
    """Oriented wall sides, open unknown boundaries, physical floor visits.

    Proposals are not safety certificates. Exact physical/observation/rotation
    gates and route production are delegated to the unchanged planner.
    """
    state, floor, wh, wv = nav.cell_state, nav.floor, nav.wall_h, nav.wall_v
    known = (state != nav.CELL_UNKNOWN) & (floor != nav.FLOOR_HOLE)
    _,wall_cells=wall_observation_cells(nav)
    potential = getattr(nav, '_exploration_potential', np.ones_like(known))
    terminal = terminal_wall_remnants(nav, potential)
    nav._exploration_terminal_walls=terminal
    negligible = np.zeros_like(known)
    for label, slices in enumerate(ndimage.find_objects(terminal), 1):
        if slices is None or max(s.stop-s.start for s in slices)*nav.NAV_GRID_MM > 60.:
            continue
        ys, xs = slices
        y, x = np.nonzero(terminal[ys,xs] == label)
        tiles = set(zip(np.floor((x+xs.start-nav.c+2)/4).astype(int),
                        np.floor((y+ys.start-nav.c+2)/4).astype(int)))
        if tiles and tiles <= centre_seen:
            negligible[ys,xs] |= terminal[ys,xs] == label
    wall_cells &= ~negligible
    unavailable=getattr(nav,'_exploration_unavailable',None)
    if unavailable is not None:wall_cells &= ~unavailable
    nav._exploration_negligible_walls = negligible
    walls = []
    for side, mask in enumerate((wall_cells & wv[:, :-1], wall_cells & wv[:, 1:],
                                 wall_cells & wh[:-1, :], wall_cells & wh[1:, :])):
        y, x = np.nonzero(mask)
        for end in (0, 1):
            if side < 2:
                gx = x + (side == 1) + (2 if side == 0 else -2)
                gy = 2*np.rint((y+end-nav.c)/2).astype(int)+nav.c
            else:
                gy = y + (side == 3) + (2 if side == 2 else -2)
                gx = 2*np.rint((x+end-nav.c)/2).astype(int)+nav.c
            walls.append(np.column_stack((gx, gy)))
    y, x = np.nonzero(wall_cells)
    walls.append(np.column_stack((4*np.floor((x-nav.c+2)/4).astype(int)+nav.c,
                                  4*np.floor((y-nav.c+2)/4).astype(int)+nav.c)))
    wall_points = np.unique(np.concatenate(walls), axis=0)

    potential=getattr(nav,'_exploration_potential',None)
    unknown = state == nav.CELL_UNKNOWN
    if potential is not None:unknown=unknown&potential
    if unavailable is not None:unknown &= ~unavailable
    frontier = np.zeros_like(known)
    frontier[:, 1:] |= unknown[:, :-1] & ~wv[:, 1:-1]
    frontier[:, :-1] |= unknown[:, 1:] & ~wv[:, 1:-1]
    frontier[1:, :] |= unknown[:-1, :] & ~wh[1:-1, :]
    frontier[:-1, :] |= unknown[1:, :] & ~wh[1:-1, :]
    frontier &= known
    yy, xx = np.indices(known.shape)
    phase = ((xx-nav.c) % 2 == 0) & ((yy-nav.c) % 2 == 0)
    approaches = ndimage.maximum_filter(frontier, size=5, mode='constant') & known & phase
    y, x = np.nonzero(approaches)
    frontier_points = np.column_stack((x, y))

    # Official physical visits differ from the cyan beam's observation state.
    # One physical tile is enough; do not chase each of its sixteen30mm cells.
    tile_x = np.floor((xx-nav.c+2)/4).astype(int)
    tile_y = np.floor((yy-nav.c+2)/4).astype(int)
    tile_ids = (tile_y+nav.n)*(2*nav.n)+(tile_x+nav.n)
    physically_seen = np.isin(tile_ids, [(ty+nav.n)*(2*nav.n)+(tx+nav.n) for tx,ty in centre_seen])
    # Partial/irregular tiles may have a blocked centre but a reachable side.
    # Keep the normal60mm lattice; reaching any one pose marks that tile seen.
    mapper=getattr(nav,'_planning_mapper',None)
    mapped_floor=set(getattr(mapper,'_nonpassage_floor_tiles',()))
    mapped_floor.update(tile for tile,char in getattr(mapper,'_committed_floor_tiles',{}).items() if char!='4')
    mapped_floor.difference_update(tile for tile,char in getattr(mapper,'_committed_floor_tiles',{}).items() if char=='4')
    mapped_ids=[(ty+nav.n)*(2*nav.n)+(tx+nav.n) for tx,ty in mapped_floor]
    # Known swamp/normal semantics earn no visit reward. Checkpoints still do.
    ground = (known & ~physically_seen & phase
              & (floor!=nav.FLOOR_SWAMP) & ~np.isin(tile_ids,mapped_ids))
    if potential is not None:ground&=potential
    if unavailable is not None:ground&=~unavailable
    y, x = np.nonzero(ground)
    # A60mm lattice point can be exactly on a physical tile boundary. The
    # driver's valid arrival circle then ends on the previous tile forever.
    # Move boundary samples30mm into their OWN tile; keep partial-tile views.
    x=x+np.where((x-nav.c)%4==2,1,0)
    y=y+np.where((y-nav.c)%4==2,1,0)
    ground_points = np.unique(np.column_stack((x, y)),axis=0)
    ground_points=ground_points[known[ground_points[:,1],ground_points[:,0]]]
    groups = {}
    for kind, points in (('wall', wall_points), ('frontier', frontier_points), ('ground', ground_points)):
        groups[kind] = points[np.all((points > 0) & (points < nav.n), axis=1)]
    return groups, frontier, known


def coverage_summary(nav):
    """Measured-wall enclosure, not a bounding-box guess about the arena.

    Flood on the edge/centre lattice so a wall separates both sides even when
    both cells are unknown. An open frontier is never sealed for completion.
    """
    n=nav.n
    lattice=np.zeros((2*n+1,2*n+1),bool)
    lattice[1::2,1::2]=True
    lattice[::2,1::2]=~nav.wall_h
    lattice[1::2,::2]=~nav.wall_v
    seed=np.zeros_like(lattice);seed[[0,-1],:]=lattice[[0,-1],:];seed[:,[0,-1]]=lattice[:,[0,-1]]
    exterior=ndimage.binary_propagation(seed,mask=lattice)[1::2,1::2]
    inside=~exterior
    known=nav.cell_state!=nav.CELL_UNKNOWN
    floor=nav.floor!=nav.FLOOR_HOLE
    x,y=nav.current_nav
    enclosed=bool(inside[y,x])
    eligible=inside&floor
    total=int(eligible.sum())
    remnants=(nav.cell_state==nav.CELL_UNVISITED)&eligible
    labels,count=ndimage.label(remnants)
    sizes=np.bincount(labels.ravel())[1:] if count else np.empty(0,int)
    return dict(boundary_confirmed_at_robot=enclosed,interior_cells=total,
        known_inside=int(np.count_nonzero(known&eligible)),
        observed_inside=int(np.count_nonzero((nav.cell_state==2)&eligible)),
        known_fraction=(float(np.count_nonzero(known&eligible))/total if enclosed and total else None),
        unvisited_components=int(count),unvisited_component_sizes=sorted(map(int,sizes),reverse=True)[:20],
        known_total=int(known.sum()),qualification='Measured enclosure only; unknown cells stay unknown. Completion uses useful reachable work, not this percentage alone.')


def _distance_connections(nav, a, b, initial=False):
    """Batch existing physical and observed predicates; no route queries."""
    engine=nav._native_collision_engine
    native=sys.modules[type(engine).__module__]
    a=np.ascontiguousarray(a,np.float64);b=np.ascontiguousarray(b,np.float64)
    physical=np.zeros(len(a),np.uint8);observed=np.zeros(len(a),np.uint8)
    allow=bool(initial and getattr(nav,'_initial_escape',None) is not None)
    native.lib.implicit_physical_batch(engine.handle,len(a),a,b,*nav.current_world_mm,
        _heading(nav),int(allow),nav.c*30.,physical)
    native.lib.implicit_observed_batch(engine.handle,len(a),a,b,observed)
    return (physical&observed).astype(bool)


def coarse_distances(nav, known):
    """One robot-width-aware30mm distance field, for ranking only.

    No heading search or route refinement. A missing off-grid connection is
    unresolved, never an unreachable proof. The original router stays final.
    """
    n=nav.n
    signature=getattr(nav,'_plan_graph_signature',None)
    key=(signature if signature is not None else nav._native_collision_engine,
        known.tobytes(),nav.floor.tobytes(),getattr(nav,'PATH_SWAMP_COST',5.))
    cached=getattr(nav,'_exploration_distance_graph',None)
    if cached is not None and cached[0]==key:
        _,a,b,weights,vertices=cached
    else:
        # Vertices are actual navigation corners, including boundary corners
        # beside known floor. Native footprint checks decide usable edges.
        domain=known.copy();domain[:,1:]|=known[:,:-1];domain[1:]|=known[:-1]
        domain[1:,1:]|=known[:-1,:-1]
        ids=np.arange(n*n,dtype=np.int32).reshape(n,n);aa=[];bb=[]
        for dx,dy in ((1,0),(0,1),(1,1),(-1,1)):
            ya,yb=0,n-dy;xa,xb=max(0,-dx),min(n,n-dx)
            keep=domain[ya:yb,xa:xb]&domain[ya+dy:yb+dy,xa+dx:xb+dx]
            left=ids[ya:yb,xa:xb][keep];right=ids[ya+dy:yb+dy,xa+dx:xb+dx][keep]
            aa.extend((left,right));bb.extend((right,left))
        a=np.concatenate(aa);b=np.concatenate(bb)
        world=lambda indexes:(np.column_stack((indexes%n,indexes//n))-nav.c)*30.
        start,end=world(a),world(b)
        safe=_distance_connections(nav,start,end)
        a,b,start,end=a[safe],b[safe],start[safe],end[safe]
        mid=np.floor((start+end)/60.+nav.c).astype(int)
        terrain=np.where(nav.floor[mid[:,1],mid[:,0]]==nav.FLOOR_SWAMP,
            getattr(nav,'PATH_SWAMP_COST',5.),1.)
        weights=np.linalg.norm(end-start,axis=1)*terrain
        vertices=np.flatnonzero(domain)
        for array in (a,b,weights,vertices):array.flags.writeable=False
        nav._exploration_distance_graph=key,a,b,weights,vertices
    # Exact live source connector, rebuilt even when geometry is cached.
    source=np.asarray(nav.current_world_mm)
    xy=(np.column_stack((vertices%n,vertices//n))-nav.c)*30.
    nearby=np.linalg.norm(xy-source,axis=1)<=30.*math.sqrt(2)+1e-7
    ends=xy[nearby];targets=vertices[nearby]
    safe=_distance_connections(nav,np.tile(source,(len(ends),1)),ends,initial=True)
    ends,targets=ends[safe],targets[safe]
    mid=np.floor((ends+source)/60.+nav.c).astype(int)
    terrain=np.where(nav.floor[mid[:,1],mid[:,0]]==nav.FLOOR_SWAMP,
        getattr(nav,'PATH_SWAMP_COST',5.),1.)
    extra=np.linalg.norm(ends-source,axis=1)*terrain
    graph=csr_matrix((np.r_[weights,extra],(np.r_[a,np.full(len(targets),n*n)],np.r_[b,targets])),shape=(n*n+1,n*n+1))
    # ponytail: position-only field; detailed turns belong to the final router.
    return dijkstra(graph,indices=n*n,directed=True)[:n*n]


def local_fingerprint(nav, point, radius=5):
    x, y = map(int, point)
    a, b = slice(max(0, y-radius), min(nav.n, y+radius+1)), slice(max(0, x-radius), min(nav.n, x+radius+1))
    digest = hashlib.blake2b(digest_size=12)
    # A visitation-only change elsewhere cannot revive a failed viewpoint.
    for arr in (nav.cell_state[a, b] != nav.CELL_UNKNOWN, nav.floor[a, b], nav.wall_h[a, b], nav.wall_v[a, b]):
        digest.update(arr.tobytes())
    mapper = nav._planning_mapper
    scale = nav.NAV_GRID_MM/mapper.GRID_MM
    xa, xb = np.clip(np.floor((np.array([x-radius,x+radius+1])-nav.c)*scale+mapper.c).astype(int),0,mapper.n)
    ya, yb = np.clip(np.floor((np.array([y-radius,y+radius+1])-nav.c)*scale+mapper.c).astype(int),0,mapper.n)
    digest.update((mapper.logodds[ya:yb,xa:xb] <= mapper.FREE_T).tobytes())
    return digest.digest()


def _point_distance(points, distances, nav):
    x, y = points.T
    coarse = distances[y*nav.n+x]
    euclidean = np.linalg.norm((points-nav.c)*30-np.asarray(nav.current_world_mm), axis=1)
    # Preserve off-grid/narrow opportunities for the original router. A coarse
    # miss supplies no distance proof and must not bury them behind long loops.
    return np.where(np.isfinite(coarse), coarse, euclidean), euclidean


def _heading(nav):
    m = nav._planning_mapper
    return math.pi/2-math.radians(m.YAW_OFFSET_DEG+m.YAW_SIGN*nav.current_yaw_deg)


def _put_goal(nav, point):
    nav.plan_requested_corner = nav.plan_destination_corner = tuple(map(int, point))
    nav.plan_destination_click = None
    nav.plan_destination = nav._corner_adjacent_cell(nav.plan_destination_corner)


def conservative_route_seconds(nav, world):
    """Return reserve, not the planner's route objective.

    Every possible swamp contact is charged at the rule's10x maximum. Sampling
    uses an inflated radius, so sub-sample contacts are conservatively included.
    """
    world = np.asarray(world,float).reshape(-1,2)
    if len(world)<2:
        return 0.
    delta=np.diff(world,axis=0)
    lengths=np.linalg.norm(delta,axis=1)
    angles=np.arctan2(delta[:,1],delta[:,0])
    rotation=float(np.abs(np.remainder(np.diff(angles)+math.pi,2*math.pi)-math.pi).sum())
    wet_y,wet_x=np.nonzero(nav.floor==nav.FLOOR_SWAMP)
    boxes=(np.column_stack((wet_x,wet_y))-nav.c)*30.
    seconds=rotation/2.
    for a,b,length in zip(world,world[1:],lengths):
        factor=1.
        if len(boxes):
            steps=max(1,int(math.ceil(length/15.)))
            # Charge only possibly wet subsegments, not the whole leg when
            # one endpoint grazes swamp. Half-step inflation retains coverage
            # of every possible body position between samples.
            centres=a+(np.arange(steps)[:,None]+.5)*(b-a)/steps
            difference=np.maximum(np.abs(centres[:,None,:]-(boxes+15.))-15.,0.)
            radius=nav.ROBOT_RADIUS_MM+length/(2*steps)
            wet=np.any(np.sum(difference*difference,axis=2)<radius*radius,axis=1)
            factor=1.+9.*float(np.mean(wet))
        engine=getattr(nav,'_native_collision_engine',None)
        extra=engine.gateway_rate(a,b) if engine is not None and hasattr(engine,'gateway_rate') else 0.
        seconds+=float(length)*(factor+extra)/80.
    return seconds*1.25


def estimate_home(worker, home, log):
    with log.stage('home_reserve_route'):
        worker.PATH_INSPECT_WALLS=False
        _put_goal(worker,home)
        worker._plan_path(worker.current_nav,worker.plan_destination)
        if not worker.plan_status.startswith('ok') or not worker.plan_raw_points:
            return dict(status='unresolved_home',reason=worker.plan_status)
        world=(np.asarray(worker._path_corner_points())-worker.c)*30.
        return dict(status='home_estimate',seconds=conservative_route_seconds(worker,world),
                    world=world.tolist(),position=list(worker.current_world_mm))


def departure_clear(nav, point):
    """An observation stop must leave room to turn for its next route.

    Reuse the existing exact body/front-sensor turn predicate. A travel-safe
    arrival heading alone is insufficient for a subsequent target or return.
    """
    before=np.arange(4,dtype=float)*math.pi/2
    return bool(np.all(nav._heading_turn_mask(point[0],point[1],before,before+math.pi/2)))


def route_has_opportunity(engine, native, world, target, yaw, length_limit, faces, allow_extra=True):
    """Reuse the incumbent's prefixes before paying for another full query.

    This is only a comparison gate. Equal counts must represent the same
    oriented wall sides before a cheaper journey can replace the incumbent.
    Endpoint counts only prioritize bonus-coverage queries; they cannot
    suppress a checked saving for the same wall sides. A skipped pose remains a future target;
    every adopted route still comes from the original router. Reuse its exact
    compass connector and swept-view checks, with no new routing graph.
    """
    incumbent_faces=set(map(int,engine.covered_faces(world,yaw)))
    incumbent_score=engine.policy_score(world,yaw)
    vias=np.empty((2,2),np.float64)
    for i,anchor in enumerate(world):
        count=native.lib.implicit_compass_vias(*anchor,*target,vias)
        for via in vias[:count]:
            test=np.vstack((world[:i+1],via,target))
            test=test[np.r_[True,np.linalg.norm(np.diff(test,axis=0),axis=1)>1e-8]]
            if np.linalg.norm(np.diff(test,axis=0),axis=1).sum()>length_limit:
                continue
            if engine.check(test,yaw):
                observed=set(map(int,engine.covered_faces(test,yaw)))
                if (allow_extra and len(observed)>faces) or (observed and len(observed)==faces
                        and incumbent_faces<=observed
                        and engine.policy_score(test,yaw)<incumbent_score-1e-7):
                    return True
    return False


def select_observation(worker, seen, exclusions, log, home=None, returning=False, compare=False):
    """One private snapshot/model, bounded view ranking, original route queries."""
    total = time.perf_counter_ns()
    result = dict(status='unresolved', target=None, failures=[], deferred=[], counts={}, timings={},queries=0)
    log.event('selection_start',time=worker.last_update_t,returning=returning)
    if returning:
        worker.PATH_INSPECT_WALLS=False
    else:
        # Neighbours of generated viewpoints are already separate candidates;
        # retain the original adjustment policy for manual clicks and home.
        worker.PATH_DESTINATION_ADJUST_CELLS=0
    with log.stage('planner_preparation'):
        worker._planning_maps()
    engine = worker._native_collision_engine
    native = sys.modules[type(engine).__module__]
    source_flags=native.lib.implicit_point_flags(engine.handle,*worker.current_world_mm)
    yaw=_heading(worker)
    if source_flags & 3 != 3 or (worker._initial_escape is None and not engine.check([worker.current_world_mm],yaw)):
        # A newly observed hole may overlap the present footprint even though
        # its centre is still on floor. That requires operator/mover recovery,
        # not blacklisting every destination or declaring exploration finished.
        result.update(status='current_pose_blocked',source_flags=source_flags)
        result['timings']['total_ms']=(time.perf_counter_ns()-total)/1e6
        return result
    if returning:
        groups={'home':np.asarray([home],int)}
        frontier=None
        distances=np.zeros(worker.n*worker.n)
    else:
        cached=getattr(worker,'_exploration_rank_snapshot',None)
        if cached is None:
            with log.stage('exploration_enclosure'):
                potential,enclosure=exploration_envelope(worker,engine)
                worker._exploration_potential=potential
                worker._exploration_enclosure=enclosure
                worker._exploration_excluded=(((worker.cell_state==worker.CELL_UNVISITED)
                    |((worker.cell_state==worker.CELL_UNKNOWN)&ndimage.binary_dilation(worker.cell_state!=worker.CELL_UNKNOWN)))
                    &~potential&(worker.floor!=worker.FLOOR_HOLE)).astype(np.uint8)
            with log.stage('candidate_masks'):
                groups,frontier,known=observation_candidates(worker,seen)
                raw_wall,useful_wall=wall_observation_cells(worker)
                yy,xx=np.indices(worker.cell_state.shape)
                tx=np.floor((xx-worker.c+2)/4).astype(int);ty=np.floor((yy-worker.c+2)/4).astype(int)
                physically_seen=np.isin((ty+worker.n)*(2*worker.n)+tx+worker.n,
                    [(y+worker.n)*(2*worker.n)+x+worker.n for x,y in seen])
                # Match the preserved router's existing isolated30mm-cell
                # reward rule. This colour adds no exclusion or map evidence.
                single=raw_wall&~useful_wall&physically_seen&potential
                single&=~ndimage.maximum_filter((worker.cell_state==worker.CELL_UNKNOWN)&potential,size=7,mode='constant')
                single&=worker.floor!=getattr(worker,'FLOOR_CHECKPOINT',-1)
                worker._exploration_excluded[single | worker._exploration_negligible_walls]=2
                unavailable=getattr(worker,'_exploration_unavailable',None)
                if unavailable is not None:worker._exploration_excluded[unavailable]=3
            with log.stage('coarse_ranking'):
                distances=coarse_distances(worker,known)
            worker._exploration_rank_snapshot=groups,frontier,distances
            result['coverage']=coverage_summary(worker)
            result['coverage'].update(robot_enclosure=enclosure,excluded_remnant_cells=int(np.count_nonzero(worker._exploration_excluded)))
        else:groups,frontier,distances=cached
    digest=hashlib.blake2b(digest_size=12)
    # The preserved planner already hashed every geometry/observation/parameter
    # dependency, including fine physical evidence. Reuse that exact key.
    digest.update(worker._plan_graph_signature)
    # Heading and the exact source connector are dependencies too. A previous
    # failed route must not suppress a newly feasible pose in the same cell.
    digest.update(np.asarray((*worker.current_world_mm,worker.current_yaw_deg),dtype=np.float64).tobytes())
    route_fingerprint=b'route:'+digest.digest()
    assert engine.step == 45
    began_queries = time.perf_counter()
    queries = 0
    for kind, points in groups.items():
        result['counts'][kind] = len(points)
        if not len(points):
            continue
        estimated, direct = _point_distance(points, distances, worker)
        order = np.argsort(estimated, kind='stable')
        ranked = []
        qualifying = 0
        for idx in order:
            worker._check_plan_cancelled()
            point = points[idx]
            key = _candidate_key(kind, point)
            fingerprint = local_fingerprint(worker, point)
            if exclusions.get(key) in (fingerprint,route_fingerprint):
                continue
            if kind != 'home' and direct[idx] <= 18.:
                continue
            target = (point-worker.c)*30.
            with log.stage('view_qualification', kind=kind, point=point.tolist()):
                flags = native.lib.implicit_point_flags(engine.handle, *target)
                if kind!='home' and flags & 7 != 7:
                    result['failures'].append((key,fingerprint,'viewpoint_not_known_or_physically_clear'))
                    continue
                if kind!='home' and not departure_clear(worker,point):
                    result['failures'].append((key,fingerprint,'no_checked_departure_rotation'))
                    continue
                if (kind!='home' and worker._initial_escape is None and not native.lib.implicit_optimistic_check(
                        engine.handle,*worker.current_world_mm,*target)):
                    # An existing native negative proof, never the coarse
                    # ranking graph. Reconsider when measured geometry changes.
                    result['failures'].append((key,route_fingerprint,'native_optimistic_disconnected'))
                    continue
                gain = 1;pose_faces=set()
                if kind == 'wall':
                    gain = 0
                    for heading in (0., math.pi/2, math.pi, 3*math.pi/2):
                        view = np.array([target, target+[math.cos(heading), math.sin(heading)]])
                        if engine.check(view, heading):
                            observed=available_faces(worker,engine.covered_faces(view, heading))
                            pose_faces.update(map(int,observed))
                            gain = max(gain, len(observed))
                    if not gain:
                        result['failures'].append((key,fingerprint,'no_useful_wall_view'))
                        continue
                elif kind == 'frontier':
                    x, y = point
                    gain = int(frontier[max(0,y-3):y+4, max(0,x-3):x+4].sum())
                    if not gain:
                        continue
            qualifying += 1
            # Bound the expensive work, not the set of future candidates.
            cost = max(60., estimated[idx] if estimated[idx] < 1e6 else direct[idx])
            ranked.append((gain/cost, estimated[idx], point, key, fingerprint,gain,pose_faces))
            if qualifying >= 8:
                break
        if not ranked:
            continue
        nearest = min(ranked,key=lambda item:(item[1],tuple(item[2])))
        alternatives = sorted((r for r in ranked if r is not nearest),
                              key=lambda item:(-item[0],item[1],tuple(item[2])))
        ranked = [nearest]+alternatives
        best = None
        baseline_cost = None
        baseline_length = None
        for _, estimate, point, key, fingerprint, pose_gain, pose_faces in ranked:
            worker._check_plan_cancelled()
            # Endpoint gain only prioritizes bonus-coverage queries. Always
            # allow a whole-route proof of cheaper coverage of the same sides.
            if baseline_length is not None:
                length_allowance=min(120.,max(30.,.10*baseline_length))
                # Euclidean length is a necessary lower bound on ANY route.
                # Do not solve an alternative which cannot meet the mission's
                # modest physical-detour limit, regardless of coverage reward.
                if np.linalg.norm((point-worker.c)*30.-worker.current_world_mm)>baseline_length+length_allowance:
                    continue
            if best is not None:
                with log.stage('reuse_incumbent_opportunity'):
                    incumbent=native.nav_world(best[2]['raw_points'],worker.c,worker.current_world_mm)
                    if not route_has_opportunity(engine,native,incumbent,(point-worker.c)*30.,yaw,
                            baseline_length+length_allowance,best[0],allow_extra=pose_gain>best[0]):
                        continue
            if queries >= (3 if compare else 1) or (queries and time.perf_counter()-began_queries > .06):
                if best is not None:
                    break
                result['status'] = 'budget_pending'
                result['timings']['total_ms'] = (time.perf_counter_ns()-total)/1e6
                return result
            _put_goal(worker, point)
            with log.stage('route_query', kind=kind, point=point.tolist()):
                worker._plan_path(worker.current_nav, worker.plan_destination)
            queries += 1
            result['queries']=queries
            if not worker.plan_status.startswith('ok') or not worker.plan_raw_points:
                reason = worker.plan_status
                if 'incomplete' in reason:
                    result['deferred'].append((key,fingerprint,reason))
                    result['status'] = 'budget_pending'
                    result['timings']['total_ms'] = (time.perf_counter_ns()-total)/1e6
                    return result
                result['failures'].append((key, route_fingerprint, reason))
                continue
            route = np.asarray(worker._path_corner_points(), float)
            world = native.nav_world(route,worker.c,worker.current_world_mm)
            with log.stage('final_route_and_gain'):
                valid = engine.check(world, yaw)
                faces = list(map(int,available_faces(worker,engine.covered_faces(world, yaw)))) if valid else []
            if not valid or (kind == 'wall' and not faces):
                result['failures'].append((key,route_fingerprint,'final_route_has_no_valid_wall_gain' if valid else 'final_route_invalid'))
                continue
            actual_cost = float(worker.plan_cost)*30.
            actual_length=float(np.linalg.norm(np.diff(world,axis=0),axis=1).sum())
            # Exploration is not a hypothetical return-home excursion.
            # Useful remnants compete on the same travel estimates as other
            # views; the coordinator alone enforces the actual time reserve.
            if baseline_cost is None:
                baseline_cost = actual_cost
                baseline_length = actual_length
            # Preserve the cheap incumbent: coverage never buys an unbounded
            # journey. This is a selector budget, not a change to route policy.
            allowance = min(120., max(30., .10*max(0.,baseline_cost)))
            if best is not None and (actual_cost > baseline_cost+allowance or actual_length > baseline_length+min(120.,max(30.,.10*baseline_length))):
                continue
            resolved = tuple(worker.plan_destination_corner)
            if kind=='home' and np.max(np.abs((np.asarray(resolved)-worker.c)*30.))>45.:
                result['failures'].append((key,fingerprint,'adjusted_home_outside_start_tile_interior'))
                continue
            target_meta = dict(kind=kind, corner=resolved,
                requested=tuple(map(int,point)), world_mm=tuple((np.asarray(resolved)-worker.c)*30.),
                key=key, fingerprint=fingerprint, predicted_wall_faces=faces,
                route_length_mm=actual_length, trip_seconds=conservative_route_seconds(worker,world))
            if kind=='wall' and pose_faces:
                fx,fy=face_cells(worker,list(pose_faces));labels=worker._exploration_terminal_walls[fy,fx]
                if np.all(labels>0):
                    ry,rx=np.nonzero(np.isin(worker._exploration_terminal_walls,np.unique(labels)))
                    target_meta['remnant_cells']=list(zip(map(int,rx),map(int,ry)))
            if kind=='frontier':
                x,y=point;ya,yb=max(0,y-3),min(worker.n,y+4);xa,xb=max(0,x-3),min(worker.n,x+4)
                py,px=np.nonzero(frontier[ya:yb,xa:xb])
                target_meta['frontier_patch']=list(zip(map(int,px+xa),map(int,py+ya)))
                uy,ux=np.nonzero(worker.cell_state[ya:yb,xa:xb]==worker.CELL_UNKNOWN)
                target_meta['unknown_before']=list(zip(map(int,ux+xa),map(int,uy+ya)))
            gain = len(faces) if kind == 'wall' else 1
            if best is None or gain > best[0] or (gain == best[0] and actual_cost < best[1]):
                best = gain,actual_cost,worker._snapshot_plan(),target_meta
            if kind != 'wall' or not compare:
                break
        if best is not None:
            worker._restore_plan(best[2])
            worker.plan_status=best[2]['status']
            worker._plan_display_points = None
            worker._plan_display_points = worker._path_corner_points()
            world=native.nav_world(worker._path_corner_points(),worker.c,worker.current_world_mm)
            best[3]['gateway_surcharge_mm']=sum(float(np.linalg.norm(b-a))*engine.gateway_rate(a,b)
                for a,b in zip(world,world[1:])) if hasattr(engine,'gateway_rate') else 0.
            result.update(status='selected',target=best[3],queries=queries,
                          baseline_route_cost=baseline_cost,baseline_route_length=baseline_length,selected_route_cost=best[1])
            result['timings']['total_ms'] = (time.perf_counter_ns()-total)/1e6
            return result
        # An exhausted shortlist is not proof that all wall targets failed.
        if qualifying >= 8:
            result['status'] = 'budget_pending'
            break
    else:
        result['status'] = 'no_actionable_view'
    if (result['status']=='no_actionable_view' and not returning
            and getattr(worker,'_exploration_enclosure',{}).get('enclosed')):
        # Colour small leftovers after the COMPLETE viewpoint scan. This is
        # deferred work, not proof of physical impossibility or a map edit.
        remnant=(worker.cell_state==worker.CELL_UNVISITED)&worker._exploration_potential
        remnant&=worker.floor!=getattr(worker,'FLOOR_CHECKPOINT',-1)
        labels,_=ndimage.label(remnant)
        for label,slices in enumerate(ndimage.find_objects(labels),1):
            if slices is not None and max(s.stop-s.start for s in slices)*30.<=120.:
                region=worker._exploration_excluded[slices]
                region[(labels[slices]==label)&(region!=3)]=2
    result['timings']['total_ms'] = (time.perf_counter_ns()-total)/1e6
    return result


class RelayFloorReader:
    """Live station colour and slot occupancy, confirmed across fresh samples."""
    CONFIRM_FRAMES = 4
    SLOTS = {'yellow': (False, False), 'blue': (True, False),
             'green': (True, False), 'orange': (False, True),
             'purple': (True, True)}

    def __init__(self):
        self.reset()

    def reset(self):
        self.candidate = None
        self.count = 0
        self.last_time = None

    @staticmethod
    def classify(rgb):
        import colorsys
        try:
            channels = [float(rgb[k])/255. for k in ('red', 'green', 'blue')]
        except (KeyError, TypeError, ValueError):
            return None
        if not all(math.isfinite(v) for v in channels):
            return None
        r, g, b = (max(0., min(1., v)) for v in channels)
        hue, saturation, value = colorsys.rgb_to_hsv(r, g, b)
        if saturation < .55 or value < .35:
            return None
        hue *= 360.
        if hue < 20. or hue >= 345.:
            return 'red'
        if hue < 58.:
            return 'orange'
        if hue < 80.:
            return 'yellow'
        if hue < 175.:
            return 'green'
        if 200. <= hue < 251.:
            return 'blue'
        if 251. <= hue < 300.:
            return 'purple'
        return None

    def update(self, data):
        if data.get('supervisor_relocated') or data.get('gps_filter_reset'):
            self.reset()
            return None
        now = float(data['time'])
        if not math.isfinite(now):
            self.reset()
            return None
        if self.last_time is not None:
            if now == self.last_time:
                return None
            if now < self.last_time or now-self.last_time > max(.05, 2*data.get('sensor_period_s', .016)):
                self.reset()
        self.last_time = now
        colour = self.classify(data.get('color'))
        self.count = self.count+1 if colour == self.candidate else 1
        self.candidate = colour
        return colour if colour is not None and self.count >= self.CONFIRM_FRAMES else None


class HeroExploration:
    """Stable suggestions for keyboard driving, with existing safe adoption."""

    def __init__(self, bot, log_path=None):
        self.bot, self.nav = bot, bot.navigator
        self.log = ExplorationLog(log_path)
        self.clock = ExplorationClock(getattr(bot, 'receiver', None),self.log)
        if self.clock.receiver is not None:
            bot.receiver = self.clock
        self.emitter = bot.robot.getDevice('emitter')
        self.started_sim = bot.robot.getTime()
        self.started_wall = time.monotonic()
        self.last_clock_query = -math.inf
        self.home = None
        self.centre_seen = set()
        self.exclusions = {}
        self.unavailable_remnants = {}
        self.deferred = {}
        self.target = None
        self.future = None
        self.worker = None
        self.executor = concurrent.futures.ThreadPoolExecutor(max_workers=1, thread_name_prefix='explore')
        self.enabled = True
        self.returning = False
        self.done = False
        # A report return is a repeatable trip, not the terminal home finish.
        # Perception and map/coverage memory remain active throughout the trip.
        self.report_home_cycle = None
        self.relay_floor = RelayFloorReader()
        self.finish_floor = RelayFloorReader()
        self.status = 'observing_start'
        self.expected_request = self.nav._plan_request_id
        self.original_replan = self.nav._maybe_replan
        self.nav._maybe_replan = self._route_tick
        self.installing = False
        self.last_request = -math.inf
        self.last_progress = self.started_sim
        self.last_position = None
        self.travelled_mm = 0.
        self.last_state = None
        self.home_seconds = 0.
        self.home_extra_seconds = 0.
        self.last_home_check = -math.inf
        self.future_kind = None
        self.empty_scans = 0
        self.selection_queries=0
        self.selection_slices=0
        self.selection_seen=set()
        self.compare = False  # One full route query per automatic-selection slice.
        self.coverage = None
        self.finish_stage = None
        self.finish_last_time = -math.inf
        self.finish_error = None
        self.last_drive_state = None
        self.failed_approaches = {}
        self.incomplete_attempts = {}
        self.home_failures = 0
        self.home_failure_evidence = None
        self.home_probe_at = -math.inf
        self.log.event('start', mode='exploration_and_finish', route_search_preserved=True, measured_gateway_policy=True)

    @property
    def reports_allowed(self):
        return self.report_home_cycle is None and not self.returning and not self.done

    def on_report_sent(self, data, token_id):
        """Latch one successful radio send into the existing home-route selector."""
        if not self.reports_allowed:
            return
        now = float(data['time'])
        self.relay_floor.reset()
        self.report_home_cycle = dict(token_id=token_id, phase='returning',
            hold_until=now+2*float(data.get('sensor_period_s', .016)),
            inspect_walls=self.nav.PATH_INSPECT_WALLS)
        self.returning = True
        self.target = None
        if self.home is None:
            self.home = (self.nav.c, self.nav.c)
        # Invalidate outward results before they can be adopted. Their normal
        # completion path still reaps the worker and closes its native engine.
        if self.worker is not None:
            self.worker._plan_cancel_event.set()
        self._clear_suggestion()
        home_key = _candidate_key('home', self.home)
        for records in (self.exclusions, self.deferred, self.failed_approaches, self.incomplete_attempts):
            records.pop(home_key, None)
        self.last_request = -math.inf
        self.empty_scans = self.home_failures = 0
        self.home_failure_evidence = None
        self.home_probe_at = -math.inf
        self.status = 'return_after_report'
        self.log.event('report_home_return', time=now, token_id=token_id)

    def report_home_holding(self, data):
        """Keep the radio's two-step rest and the home wait in the same driver."""
        cycle = self.report_home_cycle
        return cycle is not None and (cycle['phase']=='waiting'
            or float(data['time']) < cycle['hold_until'])

    def _report_home_wait_tick(self, data, keys):
        """Hold while A is full; resume only after a live empty-slot colour."""
        cycle = self.report_home_cycle
        if cycle is None or cycle['phase']!='waiting':
            return False
        now = float(data['time'])
        xy = np.asarray(self.nav.current_world_mm, float)
        # Arrival still targets the centre. Once parked, a small push within
        # the starting tile must not cancel transfer waiting or colour reads.
        # Reuse the home-return route only after the body leaves that tile;
        # an obscured floor sensor below simply keeps the existing hold.
        if not np.isfinite(xy).all() or np.max(np.abs(xy))>=60.:
            cycle['phase'] = 'returning'
            self.relay_floor.reset()
            self.last_request = -math.inf
            self.status = 'return_after_report'
            return False
        # The full stop evidence is available even with DEBUG=False.
        stopped = float((self.bot.vision._engine._latest_payload or {}).get('stopped_s', 0.))
        manual = bool(set(keys)&{315,317,314,316,ord(' ')})
        if (manual or not self.bot.driving._autonomous or not math.isfinite(stopped) or stopped<=0.
                or data.get('gps_filter_reset') or data.get('supervisor_relocated')
                or any(data.get('motor_command_radps', ()))):
            self.relay_floor.reset()
            self.status = 'settling_after_report'
            return True
        colour = self.relay_floor.update(data)
        slots = self.relay_floor.SLOTS.get(colour)
        self.status = {'blue': 'waiting_for_delivery_robot', 'green': 'handover_in_progress',
                       'purple': 'delivery_robot_busy'}.get(colour, 'waiting_transfer_colour')
        if slots is None or slots[0]:
            return True
        # Yellow and orange both verify A is empty. Yellow can also follow a
        # lost/rejected pickup or an already completed delivery; do not assume
        # a scored handover merely from the controller's previous radio send.
        self.nav.set_wall_inspection(cycle['inspect_walls'])
        self.report_home_cycle = None
        self.returning = False
        self.home_seconds = self.home_extra_seconds = 0.
        self.last_home_check = self.last_progress = now
        self.last_position = xy.copy()
        self.last_request = -math.inf
        self.empty_scans = self.home_failures = 0
        self.home_failure_evidence = None
        self.home_probe_at = -math.inf
        self.status = 'resume_after_report'
        self.log.event('report_home_resume', time=now, token_id=cycle['token_id'],
            transfer_colour=colour, reason='empty_slot_verified')
        return False

    def _route_tick(self, now):
        if self.installing:
            return
        if self.nav._plan_request_id != self.expected_request:
            self.enabled = False
            self.status = 'manual_destination'
            self.target = None
            if self.worker is not None:
                self.worker._plan_cancel_event.set()
        if not self.enabled:
            return self.original_replan(now)
        # Scheduling changes only. The existing safety/adoption routines stay
        # responsible for every retained route; no periodic global re-search.
        self.nav._reject_invalid_plan_endpoints()
        if self.nav.plan_path and self.nav.current_world_mm is not None:
            self.nav._refresh_displayed_plan()
        if self.nav._plan_job is not None:
            self.nav._poll_plan_job()

    def _remaining(self, now):
        packet = self.clock.packet
        elapsed_wall = time.monotonic()-self.started_wall
        if packet is None:
            return max(0.,480-(now-self.started_sim)), max(0.,600-elapsed_wall), False
        age = time.monotonic()-packet['received_wall']
        # Clock packets are queried every simulated second. Wall age is exact;
        # local simulation delta supplies a conservative fallback below.
        # A long pause can leave the last reply old in wall time while the
        # referee's running clock was suspended. Wait for the next G reply
        # rather than subtracting that entire pause from the real budget.
        real_left = packet['real']-age if age <= 3. else packet['real']
        return max(0.,packet['game']-max(0.,now-packet['received_sim'])), max(0.,real_left), age <= 3.

    def _announce(self, now):
        state = (self.status, self.target['corner'] if self.target else None, self.returning)
        if state != self.last_state:
            self.log.event('state', time=now, status=self.status, target=self.target, returning=self.returning)
            if getattr(self.bot,'DEBUG_ENABLED',False):print('Explore:',self.status, self.target['world_mm'] if self.target else '', flush=True)
            self.last_state = state

    def _clear_suggestion(self):
        self.installing=True
        try:
            self.nav.clear_plan_destination()
            self.expected_request=self.nav._plan_request_id
        finally:
            self.installing=False

    def startup_ready(self):
        """Wait for first paint; a failed display cannot block prepared maps."""
        if not getattr(self.bot,'SHOW_MAPS',False):
            return True
        bridge=getattr(self.bot,'_map_display',None)
        if bridge is None:
            return True  # Inline drawing completes inside navigator.update.
        if getattr(bridge,'initial_frame_ready',False):return True
        gate=getattr(self,'_display_gate',None)
        if gate is None or gate[0] is not bridge:
            gate=self._display_gate=(bridge,time.monotonic(),False)
        if gate[2]:return True
        closed=getattr(bridge,'closed',None)
        process=getattr(bridge,'process',None)
        reason=('display_closed' if closed is not None and closed.is_set() else
                'display_exited' if process is not None and process.poll() is not None else
                'first_paint_timeout' if time.monotonic()-gate[1]>=30. else None)
        if reason:
            self._display_gate=(bridge,gate[1],True)
            self.log.event('startup_display_fallback',reason=reason,
                qualification='Display gate only; physical pose and original route safety remain required')
            return True
        return False

    def drive_feedback(self, data, result):
        """Public result contract; never reads or mutates driving internals."""
        state=(result.state,result.reason,result.target)
        if state!=self.last_drive_state:
            self.log.event('drive_state',time=data['time'],command=result.__dict__)
            self.last_drive_state=state
        if (result.state=='complete' and self.target is not None
                and self.nav.current_world_mm is not None
                and not self._goal_reached(np.asarray(self.nav.current_world_mm,float))):
            self.motion_interrupted(data['time'],'completed_route_without_goal')
            return
        # Deliberate zero-wheel safety holds pause the driver's motor watchdog.
        # An autonomous mission still needs a decision when the same view never
        # resolves. Reuse local failed-approach evidence; never relax the veto.
        if (result.reason in ('waiting_for_entry_visibility','floor_observation_hold','floor_candidate_sweep_veto')
                and self.enabled and self.target is not None and self.nav.current_world_mm is not None
                and result.left==result.right==0):
            owner=(self.target['key'],self.expected_request)
            at=tuple(self.nav.current_world_mm)
            watch=getattr(self,'_drive_wait',None)
            uncertainty=2*float(data.get('position',{}).get('map_position_sigma_mm') or 0.)
            if watch is None or watch[0]!=owner or math.dist(at,watch[2])>max(1.,uncertainty):
                self._drive_wait=(owner,data['time'],at)
            elif data['time']-watch[1]>2.:
                self._drive_wait=None
                self.motion_interrupted(data['time'],'no_observation_progress')
                return
        else:self._drive_wait=None
        if result.state in ('blocked','recovered'):
            if self.target is not None or result.state=='recovered':
                self.motion_interrupted(data['time'],result.reason)

    def _record_frontier_result(self, target, now):
        if target['kind']!='frontier':return
        before=target.get('unknown_before',())
        if any(self.nav.cell_state[y,x]!=self.nav.CELL_UNKNOWN for x,y in before):return
        observed=[]
        for x,y in target.get('frontier_patch',()):
            if self.nav.current_beam[y,x]:
                observed.append((x,y))
        if observed:
            # One failed view is not evidence against all other viewing angles.
            # The reached viewpoint itself is excluded below, not this frontier.
            self.log.event('frontier_no_discovery',time=now,cells=observed,
                reason='Reached view with no newly known cells; other approaches remain eligible')

    def _refresh_unavailable_remnants(self):
        """Geometry can reopen an inspection; time/robot movement cannot."""
        records=getattr(self,'unavailable_remnants',{})
        mask=np.zeros_like(self.nav.cell_state,bool)
        for key,record in list(records.items()):
            if local_fingerprint(self.nav,record['point'])!=record['fingerprint']:
                del records[key]
                self.log.event('remnant_reopened',point=record['point'],reason='local_evidence_changed')
                continue
            x,y=np.asarray(record['cells'],int).T
            active=self.nav.cell_state[y,x]==self.nav.CELL_UNVISITED
            if not np.any(active):
                del records[key]
                continue
            mask[y,x]=active
        mask.flags.writeable=False
        self.nav._exploration_unavailable=mask
        overlay=getattr(self.nav,'_exploration_excluded',np.zeros_like(mask,np.uint8)).copy()
        overlay[overlay==3]=0;overlay[mask]=3
        self.nav._exploration_excluded=overlay

    def _record_remnant_failure(self,target,now,reason):
        cells=target.get('remnant_cells',())
        if not cells or target['kind']=='home':return
        remaining=[(int(x),int(y))for x,y in cells
            if self.nav.cell_state[y,x]==self.nav.CELL_UNVISITED]
        if not remaining:return
        records=getattr(self,'unavailable_remnants',None)
        if records is None:records=self.unavailable_remnants={}
        point=tuple(target['requested'])
        records[tuple(remaining)]=dict(point=point,cells=remaining,
            fingerprint=local_fingerprint(self.nav,point))
        self._refresh_unavailable_remnants()
        self.nav.last_draw=-math.inf
        self.log.event('remnant_inspection_unavailable',time=now,cells=remaining,reason=reason,
            qualification='Failed inspection only; measured cells and competition export unchanged')

    def finish_tick(self, data, *, manual=False):
        """Send existing corrected export, M evaluation, then E exit, once each.

        Home must be physically reached and settled. No token detection or
        map feature is invented to obtain an exit bonus.
        """
        if self.report_home_cycle is not None or not self.done or manual or self.emitter is None or self.finish_error:return
        if not data.get('position',{}).get('map_physical_pose_ready',False):return
        if self.nav.current_world_mm is None or np.max(np.abs(self.nav.current_world_mm))>=60.:return
        speed=data.get('position',{}).get('speed_mmps',math.inf)
        if not math.isfinite(speed) or speed>5.:return
        now=float(data['time'])
        if now<=self.finish_last_time:return
        slots = self.finish_floor.SLOTS.get(self.finish_floor.update(data))
        if slots is None or slots[0]:return
        try:
            if self.finish_stage is None:
                self.bot.mapper.build_report_matrix(show=False)
                packet=self.bot.mapper.competition_map_packet()
                self.emitter.send(packet)
                self.finish_stage='map_sent'
                self.log.event('competition_map_sent',time=now,bytes=len(packet),sha256=hashlib.sha256(packet).hexdigest())
            elif self.finish_stage=='map_sent':
                self.emitter.send(b'M');self.finish_stage='map_evaluated'
                self.log.event('competition_map_evaluation_requested',time=now)
            elif self.finish_stage=='map_evaluated':
                self.emitter.send(b'E');self.finish_stage='exit_sent'
                self.log.event('competition_exit_sent',time=now)
            self.finish_last_time=now
        except Exception as error:
            self.finish_error=repr(error)
            self.log.event('error',time=now,stage='competition_finish',error=self.finish_error)

    def update(self, data, keys=(), previous_keys=(), *, arrival_ready=True, local_continuation=False,
               inspection_active=False):
        now = float(data['time'])
        with self.log.stage('exploration_tick', time=now):
            if ord('E') in keys and ord('E') not in previous_keys:
                self.enabled = not self.enabled
                self.expected_request = self.nav._plan_request_id
                self.status = 'enabled' if self.enabled else 'paused_by_user'
                if not self.enabled and self.worker is not None:
                    self.worker._plan_cancel_event.set()
            if self.emitter is not None and now-self.last_clock_query >= 1.:
                self.emitter.send(b'G')
                self.last_clock_query = now
            if not self.startup_ready():
                self.status='waiting_for_map_render'
                self._announce(now)
                return
            physical_ready=bool(data.get('position',{}).get('map_physical_pose_ready',False))
            if not self.enabled or self.done or not physical_ready or not self.nav._planning_pose_current():
                if self.report_home_cycle is not None:
                    self.relay_floor.reset()
                if self.future is not None and self.future.done():
                    self._finish_selection(now)
                self._announce(now)
                return
            if self.report_home_holding(data):
                if self.report_home_cycle['phase']!='waiting' or self._report_home_wait_tick(data,keys):
                    self._announce(now)
                    return
            if local_continuation and not self.nav.plan_status.startswith('ok'):
                self.status='finish_checked_crossing'
                self._announce(now)
                return
            xy = np.asarray(self.nav.current_world_mm,float)
            tile = tuple(np.floor((xy+60)/120).astype(int))
            self.centre_seen.add(tile)
            if self.home is None:
                # Hero's map origin is the starting tile; the measured body
                # pose can be offset from its GPS mount and is not a new home.
                self.home = (self.nav.c,self.nav.c)
            if self.last_position is None:
                self.last_position = xy.copy()
            displacement = float(np.linalg.norm(xy-self.last_position))
            if displacement >= 20:
                self.travelled_mm += displacement
                self.home_extra_seconds += conservative_route_seconds(self.nav,[self.last_position,xy])
                self.last_position = xy.copy()
                self.last_progress = now
                if self.returning:
                    self.home_failures = 0
                    self.home_failure_evidence = None
            remaining_game, remaining_real, legal_clock = self._remaining(now)
            # Refresh a shortest known home route occasionally; between checks,
            # conservatively add the observed travel away from that snapshot.
            reserve = 20.+self.home_seconds+self.home_extra_seconds
            if min(remaining_game,remaining_real) <= reserve and not self.returning:
                self.returning = True
                self.target = None
                self._clear_suggestion()
                if self.worker is not None:
                    self.worker._plan_cancel_event.set()
                self.log.event('return_reserve', time=now, reserve_s=reserve,
                    game_left=remaining_game,real_left=remaining_real,legal_clock=legal_clock)
            if getattr(self,'encounter_holding',False):
                # Keep the mission goal. The original A* may refresh its route
                # around a stopped robot, but no new exploration goal is chosen.
                if self.target is not None:
                    self.original_replan(now)
                self._drive_wait=None
                self.status='yielding_to_robot'
                self._announce(now)
                return
            if inspection_active and not self.returning:
                # Keep clocks, physical coverage and home reserve current. An
                # inspection neither completes nor fails the exploration goal.
                self.status='inspecting_token'
                self._announce(now)
                return
            if self.future is not None:
                self._finish_selection(now)
                self._announce(now)
                return
            if self.target is not None:
                if arrival_ready and self._goal_reached(xy):
                    self._record_frontier_result(self.target,now)
                    self._record_remnant_failure(self.target,now,'reached_view_without_covering_remnant')
                    self.log.event('target_reached',time=now,target=self.target,actual_position_mm=xy)
                    if self.target['kind'] == 'home':
                        if self.report_home_cycle is not None:
                            self.report_home_cycle['phase'] = 'waiting'
                            self.relay_floor.reset()
                            self.target = None
                            self._clear_suggestion()
                            self._report_home_wait_tick(data,keys)
                            self._announce(now)
                            return
                        self.done=True;self.status='home_reached';self._announce(now);return
                    self.exclusions[self.target['key']] = local_fingerprint(self.nav,self.target['requested'])
                    self.target = None
                elif self.nav.plan_status.startswith('ok'):
                    self.status = 'follow_committed_route'
                    if not self.returning and now-self.last_home_check >= 15.:
                        self._start_selection(now,reserve_only=True)
                    self._announce(now)
                    return
                elif 'no current pose' in self.nav.plan_status:
                    return
                else:
                    self.log.event('route_blocked',time=now,reason=self.nav.plan_status,target=self.target)
                    self._record_remnant_failure(self.target,now,self.nav.plan_status)
                    # A changed corridor does not mean the observation pose
                    # was reached or is permanently unusable. Requery it on
                    # the new map with all other current candidates.
                    self.target = None
            if now-self.last_request < .25:
                return
            if self._home_waiting(now):
                self._announce(now)
                return
            self._start_selection(now)
            self.status = 'choosing_home' if self.returning else 'choosing_observation'
            self._announce(now)

    def _goal_reached(self, xy):
        target=self.target
        tolerance = 5. if target['kind']=='home' and self.report_home_cycle is not None else 18.
        if float(np.linalg.norm(xy-np.asarray(target['world_mm'])))>tolerance:return False
        if target['kind']=='home':return bool(np.max(np.abs(xy))<60.)
        # Waypoint arrival and physical tile coverage are separate contracts.
        return True

    def inspection_affordable(self, now, world):
        """Optional token work must fit the unchanged home reserve, even wet."""
        if self.returning or self.done:return False
        points=np.asarray(world,float).reshape(-1,2)
        # Stopping on a swamp also consumes accelerated game time. Charge the
        # maximum multiplier if either end's body footprint may overlap one.
        wet=False
        for point in (points[0],points[-1]):
            low=np.floor((point-self.nav.ROBOT_RADIUS_MM)/30+self.nav.c).astype(int)
            high=np.floor((point+self.nav.ROBOT_RADIUS_MM)/30+self.nav.c).astype(int)+1
            low=np.clip(low,0,self.nav.n);high=np.clip(high,0,self.nav.n)
            wet|=bool(np.any(self.nav.floor[low[1]:high[1],low[0]:high[0]]==self.nav.FLOOR_SWAMP))
        required=(20.+self.home_seconds+self.home_extra_seconds
            +2*conservative_route_seconds(self.nav,points)+3.*(10 if wet else 1))
        return min(self._remaining(now)[:2])>required

    def motion_interrupted(self, now, reason):
        """Requery after a stopped drive; this is not global unreachability."""
        if self.worker is not None:
            self.worker._plan_cancel_event.set()
        if self.target is not None:
            self._record_remnant_failure(self.target,now,reason)
            if ('progress' in reason or reason in ('excess_rotation_for_leg','completed_route_without_goal')) and self.nav.current_world_mm is not None:
                # A failed actuator approach is local evidence, not proof that
                # the destination is unreachable from every other direction.
                point=tuple(self.nav.current_nav)
                self.failed_approaches[self.target['key']]=(tuple(self.nav.current_world_mm),
                    point,local_fingerprint(self.nav,point))
            self.deferred[self.target['key']] = (local_fingerprint(
                self.nav,self.target['requested']), now+2.)
        self.target = None
        self._clear_suggestion()
        self.last_request = -math.inf
        if getattr(self,'returning',False):
            self.home_failures = getattr(self,'home_failures',0)+1
        self.status = 'drive_requery'
        self.log.event('drive_interrupted',time=now,reason=reason)

    def _home_waiting(self, now):
        """Three failed home attempts per unchanged physical map/approach."""
        if not self.returning or getattr(self,'home_failures',0) < 3:
            return False
        if now < getattr(self,'home_probe_at',-math.inf):
            return True
        self.home_probe_at = now+1.
        fingerprint = local_fingerprint(self.nav,self.nav.current_nav,radius=self.nav.n)
        position = tuple(self.nav.current_world_mm)
        old = getattr(self,'home_failure_evidence',None)
        if old is None:
            self.home_failure_evidence = (fingerprint,position)
        elif fingerprint != old[0] or math.dist(position,old[1]) > 18.:
            self.home_failures = 0
            self.home_failure_evidence = None
            self.log.event('home_retry_evidence_changed',time=now)
            return False
        self.status = 'home_waiting_for_change'
        return True

    def _start_selection(self,now,reserve_only=False):
        self._refresh_unavailable_remnants()
        self.last_request = now
        self.request_started_ns=time.perf_counter_ns()
        self.selection_queries=0
        self.selection_slices=0
        self.selection_seen=self.centre_seen.copy()
        self.log.event('selection_requested',time=now,kind='reserve' if reserve_only else 'observation')
        with self.log.stage('snapshot', time=now):
            self.worker = self.nav._planning_snapshot()
        if self.report_home_cycle is not None and self.returning:
            self.worker.PATH_DESTINATION_ADJUST_CELLS = 0
        self.worker._plan_cancel_event = threading.Event()
        self.worker._map_display = None
        self.worker._exploration_rank_snapshot=None
        self.selection_request = self.nav._plan_request_id
        self.selection_returning = self.returning
        self.future_kind = 'reserve' if reserve_only else 'observation'
        if reserve_only:
            self.last_home_check = now
            self.future = self.executor.submit(estimate_home,self.worker,self.home,self.log)
        else:
            excluded=self.exclusions.copy()
            for key,(position,point,fingerprint) in list(self.failed_approaches.items()):
                if local_fingerprint(self.nav,point)!=fingerprint:
                    del self.failed_approaches[key]
                elif math.dist(self.nav.current_world_mm,position)<=self.nav.ROBOT_RADIUS_MM:
                    excluded[key]=local_fingerprint(self.nav,key[1:])
            for key,(fingerprint,until) in list(self.deferred.items()):
                if now<until and local_fingerprint(self.nav,key[1:])==fingerprint:
                    excluded[key]=fingerprint
                else:
                    del self.deferred[key]
            self.future = self.executor.submit(select_observation,self.worker,self.selection_seen,
                excluded,self.log,self.home,self.returning,self.compare)

    def _finish_selection(self, now):
        if not self.future.done():
            return
        future, worker = self.future, self.worker
        self.future = self.worker = None
        continuing=False
        try:
            result = future.result()
            self.selection_queries+=result.get('queries',0)
            if result.get('coverage') is not None:
                self.coverage=result['coverage']
                self.log.event('coverage',time=now,**self.coverage)
            self.selection_slices+=1
            if (self.selection_request != self.nav._plan_request_id or not self.enabled
                    or self.selection_returning != self.returning):
                self.log.event('discarded_selection',time=now,reason='manual_or_newer_request')
                return
            if self.future_kind == 'reserve':
                if result['status']=='home_estimate':
                    self.home_seconds=result['seconds']
                    # Movement during a background estimate remains charged.
                    self.home_extra_seconds=conservative_route_seconds(self.nav,
                        [result['position'],self.nav.current_world_mm])
                    self.log.event('home_reserve_estimate',time=now,**result)
                else:
                    self.log.event('home_reserve_blocked',time=now,**result)
                    if 'incomplete' in result.get('reason',''):
                        self.last_home_check=now-13. # Retry after2simseconds.
                    else:
                        # Do not keep selecting outward goals after losing the
                        # only measured return-route certificate.
                        self.returning=True
                        self.target=None
                        self._clear_suggestion()
                        self.status='home_route_unresolved'
                return
            if hasattr(worker,'_exploration_excluded'):
                self.nav._exploration_excluded=worker._exploration_excluded
                self._refresh_unavailable_remnants()
                self.nav._exploration_geometry_cache=getattr(worker,'_exploration_geometry_cache',None)
                self.nav._exploration_distance_graph=getattr(worker,'_exploration_distance_graph',None)
            for key, fingerprint, reason in result['failures']:
                self.exclusions[key] = fingerprint
                self.log.event('candidate_blocked',time=now,key=key,reason=reason)
            for key,fingerprint,reason in result['deferred']:
                attempts = getattr(self,'incomplete_attempts',{})
                previous = attempts.get(key)
                count = previous[1]+1 if previous and previous[0]==fingerprint else 1
                attempts[key] = (fingerprint,count)
                self.incomplete_attempts = attempts
                if count >= 3 and not self.returning:
                    point = tuple(self.nav.current_nav)
                    self.failed_approaches[key] = (tuple(self.nav.current_world_mm),
                        point,local_fingerprint(self.nav,point))
                    self.deferred.pop(key,None)
                    self.log.event('incomplete_approach_suspended',time=now,key=key,
                        reason='Three incomplete queries; reconsider from a changed approach or geometry, not globally unreachable')
                else:
                    self.deferred[key]=fingerprint,now+2.
                    self.log.event('candidate_deferred',time=now,key=key,reason=reason,retry_after_sim=now+2.)
            if result['status']=='budget_pending' and result['failures'] and not result['deferred']:
                # Keep this immutable source/map and its completed negative
                # proofs. Restarting at a slightly jittered GPS pose discarded
                # all progress and repeatedly solved the same failed target.
                excluded=self.exclusions.copy()
                for key,(fingerprint,until) in self.deferred.items():
                    if now<until and local_fingerprint(worker,key[1:])==fingerprint:
                        excluded[key]=fingerprint
                self.worker=worker
                self.future=self.executor.submit(select_observation,worker,self.selection_seen,
                    excluded,self.log,self.home,self.returning,self.compare)
                continuing=True
                # A partial batch is not a new completed scan. Keep prior
                # exhaustion evidence until selection succeeds or is unresolved.
                self.status='choosing_observation' if not self.returning else 'choosing_home'
                self.log.event('selection_continued',time=now,slices=self.selection_slices,
                    route_requests=self.selection_queries,elapsed_ms=(time.perf_counter_ns()-self.request_started_ns)/1e6)
                return
            if result['status'] != 'selected':
                if result['status']!='no_actionable_view':self.empty_scans=0
                self.status = result['status']
                self.log.event('selection_pending',time=now,**result)
                if self.returning or result['status']=='current_pose_blocked':
                    self.empty_scans=0
                    if self.returning:
                        self.home_failures = getattr(self,'home_failures',0)+1
                    self.status='home_route_unresolved' if self.returning else 'current_pose_blocked'
                    # Keep reporting the blockage without spending a full
                    # request every control tick on unchanged home geometry.
                    self.last_request=now+(0. if result['status']=='current_pose_blocked' else 1.75)
                    return
                if result['status']=='no_actionable_view' and self.deferred:
                    self.status='waiting_for_incomplete_queries'
                    self.empty_scans=0
                elif result['status']=='no_actionable_view' and not self.returning:
                    self.empty_scans+=1
                    if self.empty_scans>=2:
                        self.returning=True
                        self.log.event('return_no_actionable_view',time=now,
                            qualification='No available observation in the current measured map, not proof of a fully explored world')
                return
            self.empty_scans=0
            target = result['target']
            if not self.returning and target['kind'] != 'home':
                game, real, _ = self._remaining(now)
                # Return via the known source/home estimate; no extra search.
                reserve = 20.+self.home_seconds+self.home_extra_seconds+2*target['trip_seconds']
                if min(game,real) <= reserve:
                    self.returning = True
                    self.target = None
                    self._clear_suggestion()
                    self.last_request = -math.inf
                    self.status = 'return_trip_budget'
                    self.log.event('return_trip_budget',time=now,reserve_s=reserve,
                        game_left=game,real_left=real)
                    return
            previous = self.nav._snapshot_plan()
            self.installing = True
            try:
                self.nav.set_plan_destination_corner(target['corner'],
                    inspect_walls=False if target['kind']=='home' else None)
                self.expected_request = self.nav._plan_request_id
                worker._plan_committed_previous = previous
                worker._plan_request_id = self.expected_request
                worker._plan_preview_queue = __import__('queue').Queue(maxsize=1)
                completed = concurrent.futures.Future()
                completed.set_result(None)
                key = (worker._planning_input_signature,worker.current_world_mm,worker.current_yaw_deg,
                       target['corner'],None,self.expected_request)
                self.nav._plan_job = completed,worker,self.expected_request,key
                with self.log.stage('existing_current_map_adoption',time=now):
                    self.nav._poll_plan_job()
            finally:
                self.expected_request=self.nav._plan_request_id
                self.installing = False
            if self.nav.plan_status.startswith('ok'):
                self.target = target
                if target['kind']=='frontier':
                    x,y=target['corner'];ya,yb=max(0,y-5),min(self.nav.n,y+6);xa,xb=max(0,x-5),min(self.nav.n,x+6)
                    uy,ux=np.nonzero(self.nav.cell_state[ya:yb,xa:xb]==self.nav.CELL_UNKNOWN)
                    target['unknown_before']=list(zip(map(int,ux+xa),map(int,uy+ya)))
                self.status = 'follow_committed_route'
                self.log.event('target_committed',time=now,target=target,
                    points=self.nav._path_corner_points(),bends=self.nav.plan_annotations,
                    selection=result['timings'])
                self.nav.last_draw = -math.inf
            else:
                self.status = 'adoption_pending'
                self.log.event('adoption_blocked',time=now,reason=self.nav.plan_status)
        except concurrent.futures.CancelledError:
            self.status = 'selection_cancelled'
        except Exception as error:
            self.status = 'selection_error'
            self.enabled = False
            self.log.event('error',time=now,error=repr(error))
        finally:
            if not continuing:
                self.log.event('selection_completed',time=now,kind=self.future_kind,
                    elapsed_ms=(time.perf_counter_ns()-self.request_started_ns)/1e6,
                    slices=self.selection_slices,route_requests=self.selection_queries)
                engine = getattr(worker,'_native_collision_engine',None)
                if engine is not None:
                    engine.close()
                    worker._native_collision_engine = None

    def close(self):
        if self.worker is not None:
            self.worker._plan_cancel_event.set()
        self.executor.shutdown(wait=True,cancel_futures=True)
        if self.worker is not None:
            engine = getattr(self.worker,'_native_collision_engine',None)
            if engine is not None:
                engine.close()
        self.nav._maybe_replan = self.original_replan
        if self.clock.receiver is not None:
            self.bot.receiver = self.clock.receiver
        self.log.close()

# END EMBEDDED EXPLORATION


class EncounterYield:
    """A-only early yield from ego-compensated live LiDAR; no steering.

    A surface must enter previously observed free space and move coherently
    before it can stop the mission. A latched encounter survives brief lost
    returns. Mapping, clocks, manual control and the original driver remain
    authoritative; this class never writes a motor or a planner destination.
    """
    SAMPLE_S = .032

    def __init__(self):
        from scipy.optimize import linear_sum_assignment
        self._assign = linear_sum_assignment
        self._projections = {}
        self._reset()

    def _reset(self):
        self._history = deque()
        self._tracks = []
        self._watch = None
        self._last = None
        self.holding = False
        self.reason = None

    def _scan(self, data, bot):
        """Use the same calibration and self mask without touching mapper caches."""
        position, mapper = data['position'], bot.mapper
        if not position.get('map_physical_pose_ready') or mapper.origin_x is None:
            return None
        yaw = float(data['orientation']['yaw_raw'])
        heading = math.pi/2 + math.radians(yaw)
        xy = np.array([position['map_body_x_float']-mapper.origin_x,
            mapper.GPS_Y_SIGN*position['map_body_y_float']-mapper.origin_y], float)
        now = float(data['time'])
        if not np.isfinite([now, *xy, heading,
                data['orientation'].get('roll',0.),data['orientation'].get('pitch',0.)]).all():
            return None
        raw = np.asarray(data['lidar_raw_m'], float)
        hres = int(data['lidar_hres'])
        if hres < 128 or raw.ndim != 1 or len(raw)%hres or data['lidar_fov'] < 1.99*math.pi:
            return None
        layers = len(raw)//hres
        if not layers:
            return None
        first = min(mapper.FRONT_INDEX//hres, layers-1)
        clouds = []
        projection_data = dict(data, lidar=raw)
        for layer in sorted({first, min(first+1,layers-1)}):
            probe = self._projections.get(layer)
            if probe is None:
                probe = type('EncounterProjection', (), {})()
                self._projections[layer] = probe
            probe.FRONT_INDEX = layer*hres
            probe.LIDAR_VERTICAL_FOV = mapper.LIDAR_VERTICAL_FOV
            probe.LIDAR_MOUNT_ROTATION = mapper.LIDAR_MOUNT_ROTATION
            probe.LIDAR_MOUNT_MM = mapper.LIDAR_MOUNT_MM
            ranges = raw[layer*hres:(layer+1)*hres]*1000
            if np.any(np.isnan(ranges) | (ranges<=0)):
                return None
            indices = np.flatnonzero(np.isfinite(ranges) & (ranges<=500))
            offset, rays = type(mapper)._lidar_projection(probe,yaw,projection_data,indices)
            body = (probe._lidar_sample_vectors[indices] @ probe.LIDAR_MOUNT_ROTATION.T
                * ranges[indices,None] + probe.LIDAR_MOUNT_MM)
            keep = ~(body[:,2]**2+(body[:,1]-55.5)**2+(abs(body[:,0])-30)**2 < 7**2)
            clouds.append((xy+offset+rays*ranges[indices,None])[keep])
        cloud = np.concatenate(clouds)
        if not np.isfinite(cloud).all():
            return None
        return now,xy,heading,cloud

    @staticmethod
    def _clusters(points):
        if len(points)<3:
            return []
        tree = scipy_cKDTree(points)
        visited = set()
        result = []
        for i in range(len(points)):
            if i in visited:
                continue
            todo, group = [i], []
            visited.add(i)
            while todo:
                j=todo.pop();group.append(j)
                for k in tree.query_ball_point(points[j],12.):
                    if k not in visited:
                        visited.add(k);todo.append(k)
            if len(group)>=3:
                cluster=points[group]
                if np.linalg.norm(np.ptp(cluster,axis=0))<110.:
                    result.append(cluster)
        return result

    @staticmethod
    def _fit(history):
        if len(history)<3:
            return np.zeros(2), math.inf
        times=np.array([row[0] for row in history]);times-=times.mean()
        centres=np.array([row[1] for row in history]);centres-=centres.mean(axis=0)
        variance=float(times@times)
        if variance<=0:
            return np.zeros(2), math.inf
        velocity=(times[:,None]*centres).sum(axis=0)/variance
        residual=float(np.sqrt(np.mean(np.sum((centres-times[:,None]*velocity)**2,axis=1))))
        return velocity,residual

    @staticmethod
    def _approach(points, xy, velocity, intent):
        relative=points-xy
        rate=velocity-intent
        tau=np.clip(-(relative@rate)/(rate@rate+1e-9),0.,1.)
        return float(np.min(np.linalg.norm(relative+tau[:,None]*rate,axis=1)))

    def _step(self, now, xy, heading, cloud, velocity, route_velocity=None, *, require_route=False,
              lost_route_clear=False):
        """Causal, bounded 32 ms detector; also used by recorded-data tests."""
        if self._last is not None:
            dt=now-self._last
            if dt>=0 and dt<self.SAMPLE_S-1e-8:
                return self.holding
            if dt<=0 or dt>.064+1e-8:
                self._history.clear();self._tracks.clear()
                if self._watch is not None:
                    self._watch['safe_since']=None
        self._last=now
        while len(self._history)>1 and self._history[1][0]<=now-.192:
            self._history.popleft()
        clusters=[]
        if self._history and .16<=now-self._history[0][0]<=.25 and len(cloud):
            _,old,oldxy=self._history[0]
            if len(old)>=2:
                distance=scipy_cKDTree(old).query(cloud)[0]
                mask=(distance>6.) & (np.linalg.norm(cloud-xy,axis=1)<350.)
                rays=old-oldxy
                angles=np.arctan2(rays[:,1],rays[:,0]);order=np.argsort(angles)
                angles=angles[order];ranges=np.linalg.norm(rays[order],axis=1)
                angles=np.r_[angles[-1]-2*math.pi,angles,angles[0]+2*math.pi]
                ranges=np.r_[ranges[-1],ranges,ranges[0]]
                relative=cloud-oldxy;angle=np.arctan2(relative[:,1],relative[:,0])
                ix=np.clip(np.searchsorted(angles,angle),1,len(angles)-1)
                was_free=((angles[ix]-angles[ix-1]<.035)
                    & (np.minimum(ranges[ix],ranges[ix-1])-np.linalg.norm(relative,axis=1)>8.))
                clusters=self._clusters(cloud[mask & was_free])
                clusters.sort(key=lambda q:float(np.min(np.linalg.norm(q-xy,axis=1))))
                clusters=clusters[:16]
        self._history.append((now,cloud.copy(),xy.copy()))
        self._tracks=[tr for tr in self._tracks if now-tr['last']<=.256]
        assigned={}
        if self._tracks and clusters:
            costs=np.array([[np.linalg.norm(q.mean(axis=0)-(tr['centre']+tr['v']*(now-tr['last'])))
                for q in clusters] for tr in self._tracks])
            for i,j in zip(*self._assign(costs)):
                if costs[i,j]<35.:
                    assigned[j]=self._tracks[i]
        confirmed=[]
        for j,points in enumerate(clusters):
            centre=points.mean(axis=0)
            tr=assigned.get(j)
            if tr is None:
                tr=dict(centre=centre,v=np.zeros(2),last=now,hist=deque(),warning=None,count=0)
                self._tracks.append(tr)
            tr['hist'].append((now,centre))
            while len(tr['hist'])>2 and now-tr['hist'][0][0]>.256:
                tr['hist'].popleft()
            tr.update(centre=centre,last=now)
            tr['v'],residual=self._fit(tr['hist'])
            speed=float(np.linalg.norm(tr['v']))
            if now-tr['hist'][0][0]<.128-1e-8 or not 25.<=speed<=220. or residual>6.:
                continue
            dist=float(np.min(np.linalg.norm(points-xy,axis=1)))
            predicted=self._approach(points,xy,tr['v'],velocity)
            closing=float(-np.dot(centre-xy,tr['v']-velocity)/(np.linalg.norm(centre-xy)+1e-9))
            if dist>=170. or predicted>=65. or closing<=10.:
                continue
            old_warning=tr['warning'];coherent=False
            if old_warning is not None:
                previous_speed=float(np.linalg.norm(old_warning[1]))
                coherent=(now-old_warning[0]<=.064+1e-8
                    and np.dot(old_warning[1],tr['v'])/(previous_speed*speed+1e-9)>.85
                    and .4<speed/(previous_speed+1e-9)<2.5)
            tr['count']=tr['count']+1 if coherent else 1
            tr['warning']=(now,tr['v'].copy())
            if tr['count']>=3:
                confirmed.append((dist,centre.copy(),tr['v'].copy()))
        self._tracks=sorted(self._tracks,key=lambda tr:tr['last'],reverse=True)[:32]
        if self._watch is None and confirmed:
            _,centre,rate=min(confirmed,key=lambda row:row[0])
            self._watch=dict(start=now,last=now,centre=centre,v=rate,hist=deque(),
                intent=velocity.copy(),safe_since=None,
                background=scipy_cKDTree(np.concatenate([row[1] for row in self._history])))
        if self._watch is None:
            self.holding=False;self.reason=None
            return False
        watch=self._watch
        gap=now-watch['last']
        prediction=watch['centre']+np.clip(watch['v'],-160.,160.)*min(gap,.192)
        search=min(70.,25.+125.*max(0.,gap-.032))
        local=np.linalg.norm(cloud-prediction,axis=1)
        background=watch['background'].query(cloud)[0] if len(cloud) else np.empty(0)
        points=cloud[(local<search)&((background>6.)|(local<10.))]
        safe=False
        self.reason='moving_obstacle_yield'
        if len(points)>=2:
            weights=np.exp(-np.linalg.norm(points-prediction,axis=1)**2/200.)
            centre=(points*weights[:,None]).sum(axis=0)/weights.sum()
            watch['hist'].append((now,centre))
            while len(watch['hist'])>2 and now-watch['hist'][0][0]>.192:
                watch['hist'].popleft()
            rate,residual=self._fit(watch['hist'])
            if len(watch['hist'])>=3 and np.linalg.norm(rate)<=220.:
                watch['v']=rate
            watch.update(centre=centre,last=now)
            # Include the rest of the observed body, rather than mistaking
            # a distant camera housing for the nearest surface during a turn.
            body=cloud[(np.linalg.norm(cloud-centre,axis=1)<90.)
                & ((background>6.)|(np.linalg.norm(cloud-centre,axis=1)<25.))]
            distance=float(np.min(np.linalg.norm(body-xy,axis=1)))
            radial=float(np.dot(centre-xy,watch['v'])/(np.linalg.norm(centre-xy)+1e-9))
            intent=watch['intent'] if route_velocity is None else route_velocity
            path_clear=self._approach(body,xy,watch['v'],intent)>80.
            away=distance>110. and radial>10. and residual<6.
            stationary=(route_velocity is not None and distance>90. and np.linalg.norm(watch['v'])<25.
                and residual<4. and len(watch['hist'])>=5)
            safe=(path_clear and (away or stationary) and not confirmed
                and (not require_route or route_velocity is not None))
        else:
            self.reason='moving_obstacle_tracking_lost'
            # Missing returns are not evidence that B has passed. A newly
            # confirmed surface can reacquire it without an unbounded jump.
            nearby=[row for row in confirmed if np.linalg.norm(row[1]-prediction)<90.]
            if nearby:
                _,centre,rate=min(nearby,key=lambda row:np.linalg.norm(row[1]-prediction))
                watch.update(centre=centre,v=rate,last=now,hist=deque())
            # Lost tracking alone never releases a hold. A fresh raw-sensor
            # sweep of the committed route can replace the missing track;
            # require the same continuous clear interval as a visible B.
            safe=(not nearby and not confirmed and gap>=.256
                  and route_velocity is not None and lost_route_clear)
        if safe:
            if watch['safe_since'] is None:
                watch['safe_since']=now
        else:
            watch['safe_since']=None
        if watch['safe_since'] is not None and now-watch['safe_since']>=.192-1e-8:
            self._watch=None;self.holding=False;self.reason=None
        else:
            self.holding=True
        return self.holding

    @staticmethod
    def _route_velocity(bot, xy, now):
        """Only a fresh, committed route can authorize passing a stopped B."""
        nav=bot.navigator
        if (getattr(nav,'_plan_display_checked_t',-math.inf)<now-.032-1e-8
                or not nav._planning_pose_current()):
            return None
        route=bot.driving._planner_route()
        if route is None:
            return None
        for point in route[1]:
            delta=np.asarray(point,float)-xy;distance=float(np.linalg.norm(delta))
            if distance>2.:
                speed=min(bot.driving.driver.radius*bot.driving.driver.max_speed,distance,100.)
                return delta/distance*speed
        return np.zeros(2)

    @staticmethod
    def _lost_route_clear(data, bot, xy, cloud, route_velocity):
        """Release only onto a freshly visible, full-body clear route prefix.

        This proof cannot command motion or change a goal. The original driver
        still checks every subsequent wheel command and all mapped floor vetoes.
        """
        if route_velocity is None:return False
        length=float(np.linalg.norm(route_velocity))
        if not math.isfinite(length) or length<=2.:return False
        speed=float(data['position']['speed_mmps'])
        motors=np.asarray(data.get('motor_command_before_control_radps',()),float)
        if (not math.isfinite(speed) or not 0<=speed<=5. or motors.shape!=(2,)
                or not np.isfinite(motors).all() or np.max(np.abs(motors))>1e-6):
            return False
        driving=bot.driving;frame=driving.frame(data)
        if (frame is None or not frame.scan_valid or frame.relocated
                or frame.time!=float(data['time']) or frame.free_polygon is None
                or not np.isfinite([frame.x,frame.y,frame.heading,frame.position_sigma_mm]).all()
                or frame.position_sigma_mm<0):
            return False
        local=HeroDriving._rotate(cloud-xy,-frame.heading)
        # Both freshly scanned layers veto the sweep. Bound each beam's range
        # from its body-relative return and mount offset; retain GPS uncertainty.
        reach=np.linalg.norm(local,axis=1)+np.linalg.norm(bot.mapper.LIDAR_MOUNT_MM)
        margin=.25+reach*math.sin(data['lidar_fov']/data['lidar_hres']/2)+2*frame.position_sigma_mm
        proof_frame=HeroDriving._Frame(frame.time,frame.x,frame.y,frame.heading,
            frame.left_encoder,frame.right_encoder,local,True,margin,False,
            frame.free_polygon,frame.position_sigma_mm)
        point=HeroDriving._rotate(route_velocity*min(1.,100./length),-frame.heading)
        return HeroDriving._Maneuver(reports=False).clear_entry(
            proof_frame,point,driving.driver._normal_clearance)

    def update(self, data, bot, exploration, keys=(), previous_keys=()):
        cycle=exploration.report_home_cycle
        station_wait=cycle is not None and cycle['phase']=='waiting'
        bypass=(bool(set(keys)&{315,317,314,316,ord(' '),ord('G'),ord('E')})
            or not bot.driving._autonomous or not exploration.enabled or exploration.done
            or not exploration.startup_ready() or bot.driving._recovery is not None
            or data.get('gps_filter_reset') or data.get('supervisor_relocated'))
        if bypass:
            self._reset();exploration.encounter_holding=False
            return False
        old=self.holding
        try:
            now=float(data['time'])
            if self._last is not None and 0<=now-self._last<self.SAMPLE_S-1e-8:
                exploration.encounter_holding=self.holding and not station_wait
                return exploration.encounter_holding
            scan=self._scan(data,bot)
            if scan is None:
                if self._watch is not None:
                    self._watch['safe_since']=None
                exploration.encounter_holding=self.holding and not station_wait
                return exploration.encounter_holding
            now,xy,heading,cloud=scan
            motors=data.get('motor_command_before_control_radps',data.get('motor_command_radps',(0.,0.)))
            speed=bot.driving.driver.radius*sum(motors)/2
            velocity=speed*np.array([math.cos(heading),math.sin(heading)])
            route_velocity=None
            if self.holding:
                try:route_velocity=self._route_velocity(bot,xy,now)
                except (AttributeError,TypeError,ValueError):pass
            lost_route_clear=False
            if self.holding and self._watch is not None and now-self._watch['last']>=.256:
                lost_route_clear=self._lost_route_clear(data,bot,xy,cloud,route_velocity)
            self._step(now,xy,heading,cloud,velocity,route_velocity,
                require_route=getattr(exploration,'target',None) is not None,
                lost_route_clear=lost_route_clear)
        except (AttributeError,KeyError,TypeError,ValueError,IndexError,ZeroDivisionError):
            # The ordinary driver's existing invalid-sensor veto still applies.
            # An active encounter additionally cannot clear on malformed data.
            if self._watch is not None:
                self._watch['safe_since']=None
        # Continue observing while the transfer's own colour wait owns the
        # stop, so a nearby B is already tracked when A becomes empty again.
        exploration.encounter_holding=self.holding and not station_wait
        if old!=self.holding and not station_wait:
            exploration.log.event('encounter_yield' if self.holding else 'encounter_resume',
                time=data['time'],reason=self.reason)
        return exploration.encounter_holding


class EncounterObservation:
    """Bounded, asynchronous sensor/decision recording for A encounter tests.

    No actuator, planner or supervisor calls. Full raw LiDAR layers and the
    driver's actual filtered returns distinguish sensing from control issues.
    File errors and a slow writer only disable/drop observation, never driving.
    """
    def __init__(self, enabled=True, directory=None, *, queue_size=128,
                 max_bytes=64*1024*1024, max_files=8):
        self.active = False
        self.observed = self.written = self.dropped = self.errors = 0
        self.max_capture_us = 0.
        self.path = None
        self.paths = []
        self.discarded_parts = 0
        self._thread = None
        if not enabled:
            return
        try:
            import queue
            import threading
            self._queue = queue.Queue(maxsize=max(1, int(queue_size)))
            self._full, self._empty = queue.Full, queue.Empty
            self._stop = threading.Event()
            self._limit = max(1024, int(max_bytes))
            self._max_files = max(1, int(max_files))
            root = Path(directory) if directory is not None else Path(__file__).resolve().parent/'encounter_logs'
            stamp = time.strftime('%Y%m%d_%H%M%S', time.gmtime())
            self.path = root/('A_'+stamp+'_'+str(time.time_ns())+'.jsonl.gz')
            self.active = True
            self._thread = threading.Thread(target=self._write_loop,
                name='a-encounter-observation', daemon=True)
            self._thread.start()
        except Exception as error:
            self.active = False
            self._thread = None
            self.errors += 1
            self._notice('disabled: '+str(error))

    @staticmethod
    def _notice(message):
        try:
            print('A OBSERVE:', message, flush=True)
        except Exception:
            pass

    def record(self, data, result, bot, exploration, *, loop_started_ns=None):
        if not self.active:
            return
        started = time.perf_counter_ns()
        try:
            # Sensor arrays are reused on the next robot step. Own every value
            # given to the writer; never hand it live controller/map objects.
            now = float(data['time'])
            raw = np.asarray(data.get('lidar_raw_m', ()), dtype='<f4').copy()
            driving, mapper, nav = bot.driving, bot.mapper, bot.navigator
            frame = driving.last_frame
            fresh = frame is not None and abs(frame.time-now)<1e-8
            measured = None
            if fresh:
                measured = {k: getattr(frame, k) for k in (
                    'time', 'x', 'y', 'heading', 'left_encoder', 'right_encoder',
                    'scan_valid', 'relocated', 'position_sigma_mm')}
                measured['hits'] = np.asarray(frame.hits, dtype='<f8').copy()
                measured['hit_margin'] = np.asarray(frame.hit_margin, dtype='<f8').copy()
            hres = int(data.get('lidar_hres', 0))
            layers = len(raw)//hres if hres>0 else 0
            cycle = exploration.report_home_cycle
            inspection = getattr(bot.vision, '_inspection', None)
            self.observed += 1
            packet = dict(kind='sample', sequence=self.observed, time=now,
                sensor_period_s=data.get('sensor_period_s'),
                loop_before_observation_ms=None if loop_started_ns is None else (started-loop_started_ns)/1e6,
                sensors=copy.deepcopy({k: data[k] for k in (
                    'position', 'orientation', 'encoders', 'gps_velocity_mps', 'color',
                    'distance_raw_m', 'gps_filter_reset', 'supervisor_relocated') if k in data}),
                raw_lidar=raw, lidar_hres=hres, lidar_layers=layers,
                lidar_fov=data.get('lidar_fov'), lidar_vfov=data.get('lidar_vfov'),
                reference_ray=int(mapper.FRONT_INDEX)%hres if hres>0 else None,
                driving_layer=min(int(mapper.FRONT_INDEX)//hres, layers-1) if layers else None,
                mapper_origin_mm=[mapper.origin_x, mapper.origin_y],
                gps_y_sign=mapper.GPS_Y_SIGN,
                lidar_mount_mm=np.asarray(mapper.LIDAR_MOUNT_MM).copy(),
                lidar_mount_rotation=np.asarray(mapper.LIDAR_MOUNT_ROTATION).copy(),
                driver_frame=measured,
                motors=dict(before=list(data.get('motor_command_before_control_radps', ())),
                    after=list(bot._motor_command), requested=[result.left, result.right]),
                drive=copy.deepcopy({k: getattr(result, k, None) for k in (
                    'state', 'reason', 'target', 'remaining_mm', 'lateral_mm', 'route_revision')}),
                navigation=dict(status=nav.plan_status, request_id=nav._plan_request_id),
                mission=dict(status=exploration.status, returning=exploration.returning,
                    done=exploration.done, report_phase=None if cycle is None else cycle['phase'],
                    inspection_phase=None if inspection is None else inspection['phase']))
            packet['capture_us'] = (time.perf_counter_ns()-started)/1e3
            self._queue.put_nowait(packet)
        except self._full:
            self.dropped += 1
        except Exception as error:
            self.errors += 1
            self.active = False
            self._stop.set()
            self._notice('recording disabled: '+str(error))
        finally:
            self.max_capture_us = max(self.max_capture_us, (time.perf_counter_ns()-started)/1e3)

    @staticmethod
    def _json_value(value):
        if isinstance(value, np.ndarray):
            return EncounterObservation._json_value(value.tolist())
        if isinstance(value, np.generic):
            return EncounterObservation._json_value(value.item())
        if isinstance(value, float) and not math.isfinite(value):
            return None
        if isinstance(value, dict):
            return {k: EncounterObservation._json_value(v) for k, v in value.items()}
        if isinstance(value, (tuple, list)):
            return [EncounterObservation._json_value(v) for v in value]
        return value

    @staticmethod
    def _encode_sample(packet):
        import base64
        row = dict(packet)
        raw = row.pop('raw_lidar')
        # Little-endian float32 bytes preserve infinity (valid no return), NaN
        # and every layer exactly without nonstandard JSON numeric literals.
        row['raw_lidar_f32_b64'] = base64.b64encode(raw.tobytes()).decode('ascii')
        row['raw_lidar_count'] = len(raw)
        hres = row['lidar_hres']
        summaries = []
        if hres>0:
            for start in range(0, len(raw)-hres+1, hres):
                scan = raw[start:start+hres]
                valid = scan[np.isfinite(scan)&(scan>0)]
                summaries.append(dict(nearest_return_mm=float(valid.min()*1000) if len(valid) else None,
                    invalid_rays=int(np.count_nonzero(np.isnan(scan)|(scan<=0))),
                    reference_ray_m=float(scan[row['reference_ray']])))
        row['raw_layer_summary'] = summaries
        measured = row['driver_frame']
        if measured is not None:
            measured = row['driver_frame'] = dict(measured)
            hits, margin = measured.pop('hits'), measured.pop('hit_margin')
            measured['hits_shape'] = list(hits.shape)
            measured['hits_f64_b64'] = base64.b64encode(hits.tobytes()).decode('ascii')
            measured['hit_margin_shape'] = list(margin.shape)
            measured['hit_margin_f64_b64'] = base64.b64encode(margin.tobytes()).decode('ascii')
            if hits.ndim==2 and hits.shape[1:]==(2,) and len(hits) and np.isfinite(hits).all():
                allowance = np.broadcast_to(margin, (len(hits),))
                # Same body/front-device geometry, for diagnosis only. A gap
                # is to a measured surface; it does not identify another robot.
                gap = np.minimum(np.linalg.norm(hits, axis=1)-37.,
                    np.linalg.norm(hits-[37., 0.], axis=1)-3.)-allowance
                front = hits[:, 0]>=np.abs(hits[:, 1])
                rear = -hits[:, 0]>np.abs(hits[:, 1])
                left = hits[:, 1]>np.abs(hits[:, 0])
                right = -hits[:, 1]>np.abs(hits[:, 0])
                measured['nearest_surface_gap_mm'] = float(np.min(gap))
                measured['sector_surface_gap_mm'] = {name: float(np.min(gap[mask])) if np.any(mask) else None
                    for name, mask in (('front', front), ('rear', rear), ('left', left), ('right', right))}
        return EncounterObservation._json_value(row)

    def _write_loop(self):
        import gzip
        try:
            self.path.parent.mkdir(parents=True, exist_ok=True)
            source = globals().get('__file__')
            digest = hashlib.sha256(Path(source).read_bytes()).hexdigest() if source and Path(source).is_file() else None
            part = 1
            while True:
                path = self.path if part==1 else self.path.with_name(
                    self.path.name.removesuffix('.jsonl.gz')+f'_part{part:04d}.jsonl.gz')
                reason, part_written = 'closed', 0
                with gzip.open(path, 'wt', encoding='utf-8', compresslevel=1) as stream:
                    self.paths.append(path)
                    # Retain the newest parts of THIS run only. Older test
                    # runs and unrelated files are never selected for removal.
                    while len(self.paths)>self._max_files:
                        self.paths[0].unlink(missing_ok=True)
                        self.paths.pop(0)
                        self.discarded_parts += 1
                    pending_bytes = 0
                    def write(row):
                        nonlocal pending_bytes
                        text = json.dumps(row, allow_nan=False, separators=(',', ':'))+'\n'
                        stream.write(text)
                        pending_bytes += len(text)  # json.dumps defaults to ASCII.
                    write(dict(kind='metadata', schema=2, role='A', source_sha256=digest,
                        run=self.path.name, part=part, max_part_bytes=self._limit,
                        retained_parts=self._max_files, discarded_parts=self.discarded_parts,
                        observation_only=True, lidar_encoding='base64 little-endian float32 metres; layer-major',
                        driver_hits_encoding='base64 little-endian float64 millimetres; body forward/left',
                        surface_gap_note='Measured obstacle surface; no robot identity or contact assertion.'))
                    self._notice('recording '+str(path))
                    flushed = time.monotonic()
                    while not self._stop.is_set() or not self._queue.empty():
                        try:
                            packet = self._queue.get(timeout=.1)
                        except self._empty:
                            packet = None
                        if packet is not None:
                            write(self._encode_sample(packet))
                            self.written += 1
                            part_written += 1
                        if time.monotonic()-flushed>=1. or pending_bytes>=self._limit:
                            stream.flush()
                            flushed = time.monotonic()
                            pending_bytes = 0
                        if stream.buffer.fileobj.tell()>=self._limit:
                            reason = 'part_size_limit'
                            break
                    write(dict(kind='summary', reason=reason, part=part, part_written=part_written,
                        observed=self.observed, written=self.written, dropped=self.dropped,
                        errors=self.errors, discarded_parts=self.discarded_parts,
                        max_capture_us=self.max_capture_us))
                if self._stop.is_set() and self._queue.empty():
                    break
                part += 1
            self._notice('saved '+str(self.written)+' frames; dropped '+str(self.dropped)
                +'; retained '+str(len(self.paths))+' parts in '+str(self.path.parent))
        except Exception as error:
            self.errors += 1
            self._notice('writer disabled: '+str(error))
        finally:
            self.active = False
            self._stop.set()

    def close(self):
        self.active = False
        if self._thread is not None:
            self._stop.set()
            try:
                self._thread.join(timeout=2.)
            except Exception:
                pass










# BEGIN GENERATED NATIVE PLANNER BUNDLE
# Readable sources are in robot_planner/. Rebuild this section with
# tools/build_hero_bundle.py. Cache files stay beside this controller.
def _install_native_planner():
    import base64,hashlib,importlib.util,io,os,sys,zipfile
    from pathlib import Path
    encoded=b"""P)h>@6aWAK2mk;8AzUy?EKyhh004FX000>P0044tVsCU`aBN|2Ze?;WUtei%X>?y-E^v8WjIj-
XFbu%<UJ>O0?qJ~t6lhvSlmw*(iSOyqiPg4@hznD_og*kUc^Ixjs-hV-+7H$U$sA^On-DQ1w-Y9%M{>_iLC(v(foN@1%J5NY-
@HG$4jevEO9KQH000080000XT!#BvQ*{CW0Gk8=02TlM0CI0)Z**U9Y+-
J0WpXcKb!lv5E^v8`R85bYFc7`xS6I2&2;ye9?N*A!Wj|E)vRzf}X|+NQ48dBk>DcZj|Gr~PNZV66fEi}qym|AI^?H39bl)&*YG5
NEb2eZN@5WyF*5XjGJ<t=F1LG{R5p505X>(xohBkO$2tNZj@8o*DUg=@9=vk*+*Xw;@U9mv9E_mG+F6?KNRf~3Dqw+D8&6Q8|x>_
~RG76%?Qxy|Lwp%f}61_4eiK=RK4^@@PH|@L1r~w2AbI=B4J&rt^Dqi7K&TORX)b+<3kw7GZYO4LogA-XMJ2*D_Ky!%fd3Sa5lG=
r~)yI;{=0x>ex9mKPbSLLw5LsFBdxdY>aOfRy=`5d0Nt!YEa_;p9!zxj%cuv%EPR|8%P<2P2M3$$<X#(n+z@&E5$Rq}W4`}B2lvU
edq;FUnIC>$H*z&E@btjG)!LuXNh6!HhIWKtKD|47m@t#qSe=4%#(HdA7oZ*>vEPmLb(hk_SV7wmS0nwu1kIL&`5H&J}Qll|6f@6
x^<@wd83rM7+y(8PfCf2ikbfKJAM!J#}X!Yeh_eH%q5YIW6Ja-
<2b@Bjy)D5kXf_^x!p4tt{H0ATTZ^1XtH)S2k(Bzx1@8kXc@&4iV$LDx#PK2!MdDrvym-SBz-jIn9nwZ@_eYxXBq>#3pQOu^mGD4
P&<nfdY950e2^kj|YhafQ+F|#ddHPUXOnho?F0~DTiI-
f`_`cht9Z?grExF$@Qd{NuxzZNv9&RRv<G&{!g8H6+f#OKY2?RL9+xwIjn5vaC?%X~$mnUY#<RNwzGDG^&TB^)*VOOSalp<sG#UP
Y?@+ta@RP)h>@6aWAK2mk;8AzX?~A!_&(007WR000;O0044tVsCU`aBN|2Ze?;WWNCA7Y+-
pWaCx;nYjfK;lHdI+aLET!S%zaLaUM?b>XO(qle)>{^0?HN%f+HdNaC6zcqC;>vo-(yx*HFIBq-ZEv#d(Q2hafe-HnDHJb3U{Y|4
yhEX$j!NcOC%MarAzH7f=C!RMlFSeg_?!$i$$v1vJLcv7b;mY1wuah9&~A`6d>zBjyKe3v(EUM^Y90cg!@=vD&A{3gMUWl}XOfEc
n*WwB=oG)uC*X$&|2x8Y6O9A!zHgzOuF7iC+EqJXa@zk&a^Ne)w5;B2THjs_2nNUwRzwsqd}CSXaK$@WL{4a|>b53m6`O}gZ}mM!
XJ&Dj>_&*e%sHGtIb^M=hi0?+DXTZTsu9y~b8*OjPSmNc!tO+~fW_lr%Lwn7w*ej%EpMJ?8>O4?PC&zXAt86I_$Do?KqE}PN(bfb
%P)$RlK^QI^Q_CNSVcabYKPn)@-n0d2T!?r6#FD^mDyn*EffL44Tu-^cX?$qXMespw{@dYb@R88yCYyt-
8T+hyq7+h_=mp6FP=djweaVd}_J~cF-Yu;|^l6|XT-uTRSDX&^uP6wk0umif}251LR0U)&WJ5>p+{T7gu^K{L1a=upMpOd1|2tx*
m`K~HNowT_qV*z{zqU4v!jY1tT&pEK_4aiv~iagx|;}l{;;!*5hvo%keO$}dD4x;uG03$xDxnlaY$nr(bvk<uu0V=YUn{@?p)UdL
m*XyLYjsSm2+txUI;Poa3t131i>|XFulw4vVUr;aPcYKM00v*FezKr2__&P3t`QuH)vv@uiJN@}gqJ;qNj)UDvus;s=Cy}EIwMlD
HRXmGu<jei>+5RNh9iQz^uIOvMoF`FPg}?J!G`{~xwGNwAQt{x~ELapGX=kSGr3M$BTt$9(dK!dJp9JBH7eV;)*wyR_HhTrlp2NQ
<$5YLoVzU<j=?VUO*);?00g@E?kHb?~>*L)q-
r&Fe$>Tjf$Yx31f;v1v55*|4Y&f8bd}%OjxR7u<4TW%??2ve2z6N!>^>I~!sPkoBMqVMdy!HaM1o;!>Wy@>eOad5rC1`k#6Z&1$p
kgDvAP!7_CWi?dkWy6B1bz0}dMjc2@@HlX$xYtigx8`KwOdNwiFK8vpchw3!yii_0kII<5^TvTufXbLd741~*Ge?fRZ=cFBbZGm9
}8xqoq39%y@0Se8>7|Y;HE`u6%8QtXOC?*jRt3t`<8y1)T~GAQnYf^ZazH@!0-
L(b^{X5ag2GvebP%qH7rGra0#>frw4YvEWs{#XWqk?$EVNcPhmb@Qh^~!(iOhVo}Th2$M_b2w*|*{uU@@4J%)F^DKO|)<jWEpB+s
86C(p4H&?R5&@!gY?m-EwC-XEyceOlqAN3`{>pxwil^OG0P=iaS6Pxfq`w7~Q&Ske$IYPtejuGHYWyiSWwc^g8Is5sCMpJW%`509
Uo1}uE_{B#CC$0twv>uc+zY{1!A+hRp8-
|W!OSR^S&%+`^_Z@_rARBt?REm}T6Sdo`}n`iARItiZyObOWO@vJA`YM>;`Vp^&%=WR*hV6nsmW-
`8<%*@qDa|)U}Eu7jh^Vh%x92P;G+n_gAro59*^4g@^1%e1hRl14QYTtlR7HnDP8TKn1Fc;)Tk>jNt#xy~i84;CZ#bDOZKZ}^}mB
>3O{`bq5_vi744|rRMrO2{|*b1iJqV*cEpytFFoW(_6uffuYIWQX#I7SPDec{`Bowv|E2I~Qw(1P37901j*9&sk+)iWSfPmqlk@J
f$&nNkC6lP_0oyhV2!`_~+~7pB<O`A!J|VBU=qnl(Lvi$Lm!)01>s<2g^(_>92g8*{gE&dl_Z8qt1?cYFKIR@6lnuh)mc>_N9@eA
(dy;VO$N{UuP^t$z*5JMuKMc^!vE`X&Ib$aq{f5jvzH@W%o$xIya#Fa6ng7Ltpie_r$b8F}Mj#(9M|zE;DUsiDbiMv(*DVffPHz(
2;YVHF1fla!30b`*4gMAaMhp^=i(NQtq5KXWWp#b<FPz*sjf7}z!LH*>&ocn;C@@(%is4(JXWc@VnOrb^&iIaLX)BqnXr*H2Z^cB
<)nm7&oFo@eNlD2<0P9FT^yJrV>%p;l0=_sjS95VK-m3^Ju}1PZJRTdM>-
R`ND(@@xZO_ZW{RE#gl!DvTtz31mBmp|$kpOTrZ^_!5AzZQiaRYGxR;D%T$*rp`G<#^}N9rKSuJVkuWE;~98fvw4zUvus0QE)EF-
c@6$sgI&w^F(`#f&h?Bb&CV7u3bY9|YUr2RNIwt!D~LoQKwl`|;vMwuzNxs2-iyV;dZZ3fk*w!g!X72{vI!nNy58c=8Io-
rC`yur<o~Te|Ha^0ief|j1mCO>&u$62x8iNCTA_X%G)9i6mL19mEe?id{Z^BHDK%RlYZTU!Iz-U_w##NA@l2wxR8v9?0Zu5mv#2B
#CsUZU$^+{Oj6z@qV6&b_r~z0Wp?nplNqLhr5+!+7APJ9-
QV6{ob}m)IyCMgUk>F6S;y5qCyv2UQiv{Rna2QYlBg1jj({Gkb&l(+DfkG}xiXxekfK}|8{C6Im%=+z!Ih#?l=g$W1q!STX4{;%Y
;lWG&LnbKvK@XCZSS5;}onbn63y2*enfDk1N(&I<$*<ZV!T00glz}UgBG$uT0xMGa0K*_jO_>(Tjlox$vI*r6NO(OOPG+wn)w4@O
lsE8_jHhTqc*<*02JFM<-`>7?_um&E&m%>yfPMb^w_iVfJpc6Z+mA2rzg@ij`?rgCpUy8L@5CDnrt(ENFOxh9KMSC(-
_z%p%^t?w@pRKBpjUuOptdo2<;P&)z!8dPAqg`o4Dg=4$B&hE8rTKwC)0%3trXPS0F#VogSzVX#{^Hj$eL&XBmgLYk_i5FE0j%$x
MP}<;~J?1m=B%`7^8^3#SI~`1e!)3NaJH=M<3&R@KmyduOTv)PIC`x%~PJ=Ou|*4U7)3q90V?>9FkKtT#g>teC;F~yOCJW4kX*qG
TGc0u9Ityxb?j)gk+di0vdr^+3!YODQ8MSYbQ!zLSSH+$hf_8eEY(r9P*>3V<=)oFA%I}6ds?>oSmoBg7$?;<h-=fd-
f$$uwk}YSCkwK)UpFig|`Aisc2$w*)CiZn`SjmRr;U4UevV!Z{>fJV#C$_#iyU?Zh$2lkAy^vnu^eWhM@JofuO{`sInyf3IVJy;k
&T~=8L*qdKd?IS1gJc%7fIQ-Zb?NsNG)9c6g~owq=k4`kCOQ!Z|@@N94E%CfOmsGh@@O&9a8(j3#i;ZiZ#<xGZ!(JgKB8u9g0dH$
i5+9<tvEmn=osk~UXVFO@4x$g9aX$}S8QvNs1{k<*mfJ3H$Qz((#vzGENINl(;R^G)5f;a`n*>33ZA@N#t!{r#lN@N%#k8}Qm+tQ
D%wyvRZANaxLpi~0_uOPpm&rgH>Bh)W88Hl1he?Ri+hjlLMNqg!z}!Q4*3PB_M6^%}yAblXt^@!%W`st-Ds?p7R~4W5iWI_*IIEn
+8@W2pSRsf?S|b2jL8X>?Ie%1JM_|9+|y&*HLic2i$Q9i?9uqY0*d7?jAZ916AL4*3t+8wTOy5=;mfYtRTXNYHp(^Q6Rh8Hmypf{
3%IIS<)Kv!M&Iumz3stC$*-LPUvSwn)H4Wj{f9R3xdjUwxAcl0Fy!qVpW0pgK=VOJ$7gWY5d6b_V6bM-
n^U1H7BkadMQ4ZzmaRtks?DYwurn`T=EHDu=#OxP9^_cmoBabehzc;SH*+20ZjJK2?jMEMU|dqs6Ilm&S(xus@d5E7YEarB2<O!W
NY#Jis}43fGR+g(!C5BoCA{PFIeem#XF2P{)jY1Z7Ldqoc@R&Ru5aIF?4pR75K3lGlE_slYo^0OdRH%8^yO^{Q&V;C0Qjs0Y%#2B
-t9xxF3j3rs9!CAP8Ns*aajtU1sR2sIa?&|~rvgGC&1$e<FR9gE5!ZQ#|8O`iVZmSY`SuQqKawxzG0zyA9DTm0qx$Cr^fEBkw(hQ
N-z{vq<xL?Od0hl#)rwo)@1hNKf}Ntt9~bPe&zXc1Q6{!KQz*i|`D<p4-bRU-fKfdDy0@J^Gt|Aa=MuXA)g?l-(iY<&@P(W>m0-
^+5%CevKdL+84NT&GPjJz(tvGIncT<Go5YirVy4`4`ky`zBnAY*X;&(zEv}_ju=BkqT4IZEs-%(EVB1&2x~5Q@^-
wy(Z=S#rdMM7DGYO&fmR!Gl_T0wHzU3B-
M5$OkRTVFABX0;nzr@n26OS7?pyM=?;^>|6KEi*Eg~f%aoC=6VSt$<9^MOwoTF%)yrUf0D8S(1YeE{4pX<ah+8l;YGlp8U>I(eg_
y&<;U2RL-ba%|{y@v^Hlz(C+dG!kW$<9_WvErU>pK))aN|U7(09nEgst9xhzuZCZrk+vFH>n2Zfo!({T>i2x2@|g(B{W-
_ks3Z`ic7yD>R#;je0O$y?UMQmTWgI4=3CQ9404Zz8u6+G+eKv+hGz#Ca0hx9L28~HEDYH_}Yt=qB47L7P;Aw-
d071MlP`>`i2f(l`=mfc(3cs=mmW8NyAB6F$alzUEPg`F&=8ZufKl!5r25|`KwEBjFGHEQYAbQYxfoRdn$AmbT4J&(ZFN*a$tKU#
C7AqG+r`PIS!<+0l&x|$1<2vn3hcf>kYLfpRH7p*#VjC4L72b8h;=GKq>h_?jS!(B<>vv7|HrDUj=B;!Qt-
a+0s$EglRT~uU_eqeF4r@_o1qjuR~Q12tV#bh-yZW$4Hikja(LN+@r4n4&z#qc#LZ4c-
@BSKZCi+)b^i7sfDqj|ERCg?xcN5oD8|<Fdj)n#%RRX|5hyiX=Dv_QUA4+{DYYIt8{`9SEY8O)|nW}t<Hw2?SVeDb?bdqBGM8GZE
T2xjFa|eOf|zA+EF~f9Zn9>{qDvj3K@qPMS{ey@@`tTCRnEXWX$DweTNvW_iaJ7(`&i!?@bj#U@HDV>4Pgh-
#FQ!q+${xv72A8!EBHZuv^@bCk-WgVVE6Xbxpy<+vM#<R<19`Nf>>kL8aWf{3U3DH{<V<KAK*ZI{YA#%(OxGr15*fVq0@ua?)T0N
DT^ku!Rl1TfS>`imVhFN5oy?jbsG8!J{*n0iBC%-
=`{;kTg1=@v(8hd>rllS|&uh`=4vEme22SiRXMVps5K{{1M1Ll$mOoaTUN)E~wj|@n%;ltXlJ?N~kJ=Y7)*b-
hTfjA}tdz;~_>?5i*t!_{h<?4T+<EpaFbbc|_QeUSOOrO928AU0$i$0EJVK3WVX!f^PU(e>Rt1RbYHwr=JrDb|Ly!H8FLPgVNsSW
hS<i!tU~>N>&XFMSfYo;4iX4j-
Zi6DZM%lU)CW+i%dm}C9Ly6aZJ<Fu>z~&#Q5!0HgNmaE)4sx?di&qZ`sc^dlUEA(A9edRtnLSciy^NrV$pfiOjpNx5WbH+%;1Z{a
(@yKyvP(sl{9SXNYb`!G=5)HLlZ8X+Bq>hJ@6s&nPK#KB-2ijWSRWh5)mEO4CgRvbk5ehbvHKCEv!e@BC^*P~~F)NIfUJ-ebO2pU
p;AztF7LvG<HU<qOkQX2ft#qjbp?i1xlrV|}o|)&Qn76+gsg<?@DFR21PLV3OmbDTdcoDPq%Bo7PI9U<G@Z%pYWSt?CXg&%eq3)8
!rVyD97FjU!pbm$~}D%Da=h(jU4MWg)OE9#0$UFegs>n?E#ht2)~3%bVJ0U1V))q$lJ`&M6vC&D;&_!(dui>K1mZy=CtLLwcI3>F
|)fqjEPqEs8nI1`w7?YX|70#2p>!+_OxOT7nsYzXP_1i8**eyyX@wtwwYKZ`(f$x&l5}fz?6-
t~UOVW(EKi4;<?wATVp`al^|#J?WK`acFMQRGsP2o1cgSWTEWYcq7>vGRjF0_oVQe4DzOjCO5Q4%^n({g##VeeD6fYomg4#D1}%E
PsKg=-
A6*Mm~P<kfBUkdcdxn}^E9xbuf*;m4;q8CKt$Hj)Kp|m6Pl<j60~~>cfRd)55>AyX?m9;({9(l*VWpApQ>bVE>wRU`yTi3BY*{5<
Lg7D=w{Nv4zQx2d&@laXY4<t;e@8bd$ZnNO@)2A7vJ@Cb{f;pF8K<q?ooGp+a|4DTrqW+g(VBTpVagY9-T}z8ShPdRnt4>c-
>w%X#kFN7hcBqo30wt*F$!(t3cJ{Ed&h-xcOj8QghUwO2_DsREerw$T|^-
dXvIk^<{aTy?U7ch+NeF7i&WAAVl^h#gx22a`1;d?#|7P6@V$dQLn73oZfIMp%5@C$iQ$?_grfq5-
VAoDOVq_W{`ie0J`UUU{pNrb&nw_33uywqjS#s^id>-
m0Nd?y&UfVJlFXo8=UJr(gARIN*cTqd1^~z10U@2hfy!bK`o2;{SOl?K)j|oZKPQy1bTFi3JHOysND3XAi9a}epF>jUj34GX~!mw
(hdM%c-
qibDhKr}0szOzAD@dEoVQVnnH?;0eWU&!iOd3MXo<ue2sSJ|WN*@I(5eL<^Ch4506bFf>Wr6KK9lEk$bU`JmJXKbzc4Zd$hnfkMX
uvRK@&{j?m(eWEbDa+bQ~Jau9!Qxcl6SO+f^P$&T2%5Vbs?>>%<k&3#?4sc7KS*6eMRH^NxeyL)8Yk1$^XNt&^QUGzh`KWc-
c9;QY&*AMxjlFYiB{$6uWrBqN)lQ+#;V3=KZevD{2+;c+>f0zB$Zko8+rwGz@_kASaWZM|f<oJjJR19E<NeBy9ECTpn)Pffvj)$i
$yfBKo79uZKd*gH0Z?>IlXpXLGBfr59}Lt!f74n=0PyaR=1G`d}EhL_4BO;wSZ@%0wq(NT_dO=v+e;|-
*UW2}^kV?_@bfP;A`I81flgb=6S%&^@vqN;v%c_K4*=GfNJX!$B13hK`Qx_m+zTK)9&N+)_{%I<9~fK@){l}^&V6F`6z?eiB7lp3
b!9%2J6ShT>OYL4kM>kl(w)v@}kO!9cFRL%9;_7MrI{|``00|XQR000O8001Fe9A9#446XnGb9Vv&7ytkOa&Ka9bYF06VQy|^axZ
0WXK8L_E@N<T)V*tS<3^Gw`h9){v^UHlNRT3_R=Z6SRP@+AvyS7n6}ETJ-RRINKmrosrU*6wO2A0^?>8Uy21u#xnK}13W?Dp{koC
&S%F5^IsrO5=NmgYNd)uf??-OsgiMHD$pL**gij(+kba&^y?|Bbtx%LXG3$@OnFi&pMZE}}v%i*6+Prbh-
*<DiRb>OYCqAUWh$oBavInA>Y0B2k8HY$_Ci?*>>KuH4M?~BBX^2GbRk2dK|3Vm4CUb#*XTvC-
uzKu3e4SH02>nPtQMKSb#%)BDK+i#*W%drnho@`f%SLS=@Aao|l3xJUp-ZEN!O1AOvPybK4U2XPp;>}k@nWx*^g;5l3ZnGT5c4rm
lc~qMPt971jGppn-D%T4ecARcYvuKmvrDb83q|iO=xLJ0W+-
14d+UI?;Ps{=Uom8u2XF=a@vpj~;$Jck!uKh)k*k6?CCM~U|?qNN#JKN|kDRvP|<l~=K*|sQ?YL|O)wqI@%@ACWbY&d>1KL2L&_W
bPp-
8Wx<b2d49_vUTj{pF|c{yRMTdKh?r{rRUKf90RQeE9K~pMHjKX1J0c@?^O$y#Go8bMA(#b+Y<&y~~P}kRncQqWz|v;xcD>35x^2K
JU{!Ddyp5IQr&p*zC{GA1;6T`Pb{8KfM3;-@|W4&<w2WepPyx-lM2e1@-
Kin*TPt+eJm;<=Gzb4{<KrZfY;8(&F^Z`5;bi^Ca;$$t@t)uj@qM<jp3FusefY2CIsI0KhiRZf?9X+rTOU(iL6`Xw7(^uHZAoqoh
n%fw#>{*e%-
va5P%s%3*!p0>eKovup#qg!<LJ^rGO>Tl$ZeUi`QmR)c6*1<PR#f9vO2o|OB13p*yBPNQYfjfa&#A18xvo}P@dx>06*FtDm}-
P!Y>U|g~n*Y|0px)N}8+ynE=V6y0V@Ya9)lL!B$H(lBBC|LUbBfR=Jx(b)Fr(*z&73DgFa!7S31{FkAs5(^nK~(F<+Mmg0qG}P<3
jQO2=u@>-wyTTucH52W0KQ)0EMcWFRKm8}D$!JOzmiRnD4<Xu-
?@OHv%Ur@*Q2!(0~KSR%>MK&QSmQXrIB$I)T5vp2laSHYrjj&@Fs$uAdZSkaaL+v5CEa+9`vyfeT2V&(SN%1iv6-
t5BoR}gBdJ&umUWh?*zWs1+<T_V;%fdEf9_CE~>gX9izbx!0Z^@I2a9|{h+q0)L5m4D%28Jk&9pTs(#(8b?ZfPiwDF+p_b8yT%#0
pKSV_7bYKAZtM&y%(fugM1!i1&_aRQfGKl<{Xr&y5qo5pzW3F<O<z2*W+~MJ9hJWUhS-
+1z)VOycK3+h3Q(WMfAMfGu9v(LtKj$#qX%5p8tW#Ao#uCw%7(g6`BmWV8?B<*7>1nrEr~V^!YdnK)jb_U{i9XGqpVcRbg&IA=?j
{@T{L>hwy1P#4=L~>Vi`u8DFCL;@7ZGj-Fx6s|fKxs;z$NLnTaK{~H}fnfw;avGax5nlsim<$mq}6T?~U0G#P22i_JjNT5b^Uqxc
BuO%kEDfX@+o8o_*s*jW23^QNxQzHF&vd;r;yuZ>mjnx9fJhL@4|oEI_|Yqym4}{^|WaU|BF8`cv6^158|nNE=H0Yyb;chq$Bhu^
!Yjdwlp5#8oe@`f=ThCAE1LbAzvO3k%2(In28F9jU!W!1ClSO1E&x!dy*ym*3;0)6UMe*HOMwUu|zUNuZ`0=0-
iO*6=b>=op988htR}nrc^Z)kQeE=*Ek10MvUt>2%_G$S?X-;7_|yBHt~<JFe{O4GEpY&6u@!D!Kvw#Gw-
=wlGl9Rjez!8t8f6?(Y&f4X>H`ne&&6=``H}y9nGzS4?`eFM*STh2JFM$Ig{CWwe#VRPr2(eu`Q1Tb5Pe>thDUK)gr~_)z}Z4>CX
rE-auv7e3?pg#;LT!yW0$nYSMMqE(x^q_D5k`5T}ssV}OhfC;CQ)*XVRHCVRn5G1I0yub?qB?4Yy0{9Po4l4M!@?U}olcf(-
`eGi^Xq%fZ3L~5rbKCJ3r}RNE7}G}c^)@qFEvJuBFo>QN*4Cjtv89h>t6m@lA9XtQ9Dj^E9kG<9yNsG9nKbL*1exQc`8hgH?2tK5
{^Hf+y!_rAX%*Ud#Urh7q!o;`!jV>AGE#@+2(IDB44Cpq<Q<mCZ5A`cv`unT!nP0c^ma`@mRSXV5OpQ9@n8S$@f6U2nW>)xm<7P8
l452SV;c)#P-7?;qeH&*A*!#V%_e(5PBoN-
7TaI$(@oqpLEt^7<E3#CQ2#RegGZh|6LE(CfviIK)uqOUMf&%o3scTdyM3{~#yeW~5f}&>a!8&12yj1cva2h5ZFtXZvi=yh-
8!Xx_eezKv+-
RDpW%`i!~yroXZXjTAu5?KYJ5@Ci)yUTRpNZ$i!ps+y{Pd;J(g!I{zs$vNMpSk@~QzhY%&LMq60q>j{QfcE=&#&RZ@C-
W5A{(g;?k@@?#^~SopIZ5oX+_G4c+5nEKO+Zj<S7xxR+$I<j9~XlA%fZeh@VA6LtG1Q6EqIt`E-6aPfZx>6@!b;0u-
T|rm+`uBLwzgI>%{vJ<No!Y2h8})0WJ}}46#(pHs3K{nh)1cqbS3k&&GVDBzeP+!oZI_K*##<-
1=h+}R_gNZycs>>DE*(D9t6_4t!vjm~UV*r9HBIvSq&w+xC5hJV2EqN&wW}Z=<L_&EeBjLUOTuTyVA?V;Dm~6okcw3lkR$?@(8CR
E-Z{`Xxa0nH!6<W|!u4A5++vgGQJn6J;IHseQTlYqKbmODqqB#MVwGfO+y|i_!Fcj+fij%crx{-
%_uCRUT`IwA)krVm!#af%jcV{_?ReXeSB9$(2K3V0>^5(wuO3^myYP~tO8IgIS3I??WywI-
7nhXQ7V5SZYO94Bj~}ZAF3~O2Z7txnK(m?)p8g*HL7wd$u0fe@VS?T75HOrAe5E+S^<qT&NH4C`>WDj}^*LQ|>6#An+Mg^Cb7-
y=?v(31dk}mq)JgA>>m<(sHUACIkvQ3v>wojbax#?uRk)>n)hwl*Wpv@$L>yy>5niJBZB<^cHc6DXqo=@20fg~53g8mL$cv=OU{6
15u3TO;vGQks5n9uYZ16jzE=FI}nTH!?*RmfPhvFO-
#F}`U;EJt(340^Ov(={JMRb#dJf2;!3s7#>XE|BRpjt6C6ddjj#@E1@&%<3>%H9F{kMMi=4-
dT!AdD)>sp6n9)Fvj+sd|t8C71`MLe&qu#u)vc_2i5Y|ATw|qrt7A6ymcuxyL>UC9JnKo!opFqdeky)_{n;Sn&Oa7R2N@;I4RDe9
p@*94K&6l)v3A+(c$7^{wOm1ef-
}Y%~Zy_rxIiMvdQ~#&2`vY<}|_^9i?a`WDWIfPegU1?=x&_|9+bWV1g8?!UYN%>i1wR&PeXy<zx(-
=7d}Jw1KFK57&07@~*Zbtu~)9{9nlcbP*;W`su0erg}N7(H#pr>8I4F(@X#Px9JhF@Kt&=Hm{An5Pkninl;^OK+PbF=|k}Y=;8$?
X7p0M8!T&Viy<`-k>l)0=z2=fQp~5Hdz5jiMN8`<;l={4<ullcn>gz02-
QCikM<(0R%)TLnRyf3}~l>L!bSNXu6y!X+ohZmJK>(UtrbtcCW;0XFs>liGPsrNDpoIsKiHsk{mtHUZ%QYx#KyG+JB_6$1|KM=R}
rN#5i%H(r?leOK?uR4e^>px3VzWg>Oegu}95Ghc`X=|4g4S@YI6yhXp4%KqYb_#MW|MB9Ao8*}+a)v;-
PI(n&RPy+vQ$TW5Q|s>grQH|1Y}+sXFjqb~U_EuhgY=|CS@W%OOL+@l_9hphfqs4t6Eo+R6;m)_m&%ZP5xLgVS}@+`Y4ymfS+0Mi
d&q^oFyfP{9d2;dHljB=D0$$+#^1x(y(ofNdENn3=3Y=wH!ga`q0_1hBltgB?R8FII_MFPA=mPfa!K1C5E>N*o55P7y4dYAB~ApK
SLuw{KJZgHX7O}22yONM`PZVkUK#8mt(Pvc7*ix4KTt^z>DLR;~xhax;6Nib0+&-
3d7hPstR&3r?@_uKUIKDpkNQ1j!}VnHwkgG4tk=mqLj#5qXZW$>$aA8qz13gjk3dbG|N>YD#ZO%M5a@@E9wd4hGs6IbxW=i{+|dd
^}|35^8u3`lf_d%=A$_vf!2Aiu1|{dlDg_n29A?vZ4}5ZuQ!zMApP2VaKNAt+odzPYm52z=u|QYCTu_>^4p)X}tcsmt{0Cf#hVw>
19YZBlmq0Nw>+qRe4QEeceGHajr#D>)Xu1Aaku{I?nGyGLA&9_#04qyZ&mK1!Oj5*F`#zx?#We|`8a90foAbPd1%^Vjcx`VrMHQV
+{E1g1=iua8U(g7XCnm9FoKN5Q$_)k7Y5!7_{Mpm+enF6l5mTw`a#^C2tIR(seAcT`pg{4_Cd5B+?14cBlON0tXPCg5BiZeSZX@l
=SZX|oZ?c39gizl%1^3N?YKYIvkx-
v{((4eL_u&}<^xUL$y08VA>fRR%jAR_GdLO>T>ZFhNz?q1{DgDRI=hhlJ!DaL|$5E{X200av%-B%t5epYV)-Z-5w-
>u|zzDY{5CijXh;jM@7wK~LgqWGD)GxfculUup923sYpJ&Whk1g$pv=!fPP~<u*u!Vqc2nMhZhg_zid+bt#j|&?*df!w8jxHT|kP
oh6kmp=?QIODv-
{>4|=II`}z0QxBe>8zcDls91ZCV!*ObUH@30MCCPa)inT^<A@fI&s}b(U9x;XeZ(OI*!uuGj{ndv>guz<{vQLPKuUml@ApU5H_6%
8Uw^awrr%x9=Wl)Zvsg^jiMLiGGmLTMVZ$r5O7areJ3M9JNZRG;J)SpZMrVXR*U$h1PR^l5M9b7<ZcIqf+}o2yAm<dZ_727ccZUc
LJ6GBam>F%#TFIJ)S%ko4`a~W)vTLvDjj?O2o(Q;NGDMe5RQMS!M2_Qvy>na8VJGi)*6PE(X1X)E+<1ytP`r8`T=Mu!mfRe{2jt?
tM{9!}96%JbmoHoHfsq*7Nw}1<B<n`gxa>xjY!d*$<#kxW=k)jKiJoimT-(4bwoW7VcOU-ppMUdy`tgVVo&h_9I3bP3yvw>=1E0n
<mgzQz3sN91Ho~Te>N4v!G&r1WldkIPIE`+%K>5>^I8FlbV!4mukY$CY!pJ7#zgb8~C{n|NO)NGa8fig%B!l@fRjIu5z=ZG-IDB`
`+93Uibe^By41*Qf!Cfz-
Z5$3q?}8oRmO34V<9C5#+;!nZD}1)WD#*7%zLF!zZ|Oh*24EMS2ipn`{w?g=phClo8vlutGD`8}L|O1?b}6j?9-
o8TjMznay^9{u9s;L5ilZH}rDC#$Ff`lC*<S)T3cQkfjm+~k8i~PkXxyO{*E*NSFR~BUpSHjP2RC~-
f{E_yYIsbaO|W7D4Ty^L6S%!JZa?{XfCiyTaf)FY?<GnEIQ!RR)hG%`%|K>&h;gD2rQcR9x__A^t|q}~pF3-jOPZDp*o%z>h-
R7Bt95jDljLU62JlMGZB(R%)r*@fvpY{_UceG?!+MdnWC04;Y1PhA&1sd8XdKywG}*+e*$pCHV@BXZ;#2rJTatn$ZtV{7aiZ)ltr
TA(Mgh#-b)4bN$m!n0HA2`V>SJOH-
9o&AZLZm}mDr#lfinYkl7)+~Yhkdk`QZSSxEKJLNpI?FB%3L0l<le(FJd4(&~dnM0clBikMbMb8p!m;cTxUHltuAf3KtOAE7y2-
i_&$CJ-Fu8ws(ZCCHELfaY{+IhyaMXUq*YHB<`DqL^xaqgT(DJysZN3ex2Ri6bZ4Q$^;CT`np)dtu!2-
2gU@!`CVa6++=Ux(9KW3BSTT(Hv@CxRH=k+y(BFL0&H;4yx7Yq7VI-2gnpEVC{XB?I1{M+8u6*+<YfJKvA<&l_(K|(Yx-
4@rp~MRPc`PJv2UHD_?^_VJv~*U`RM7XrhlvPA|zl?Ab%c^8Ztcwq)510e+;K=h5yusX_}OVK#L#iagRTMYF7g;8a0b_t<hEAtu?
v|<qg14oWf-
7V0Eu~<MJ`bGLf!5@)LV&OTDV*m7HZ7T7rS@wm<fJ^lvZ%CdgVsr)ti1b6L4zN&v^#te$UizEH$adU=K?lY9+$IjiV(<v+r!es~h
y106(;gDvsgzJBc1s(byaKfVeZHEQE+AAp(`fp)+07dn<(JrlQDfDhxXuAG^X9m{pkoC>x1((IU_GB8$dRgwXfvO;w@MP>A=C3i|
oYnoZ8!rH1Sw5_#OlMD4Kk~D0SE|I%U;D#gZ*|*5ez{!JF{v-
m+i*>rATYUky92By_aPanEh`0gRdyg&B(f@%2V8Br3bS2}9L&2!o^^u_jR4>W_OVPbDWv2!c8cXnqLAXnRtt^EZ?$CR`tv#4{>Ja
Ye4ZA7e9gJo4WgcO3(PrrGVUr>?$k~<*1i1KS+pR{YXCvS5uLfiLMeV#;CF!PHjYVPKD6Dm%t&Hk)%%0eogj}QBk^B~*<ZV5%-
^i|rQl~GfRLbJo*g4{11mA~0nVoBwNm}RH*<#_*VN85cI|HcSH<&Pa2;BjyC`mZkE5-
CDLu{g7$Ys)x*dUB@0B~t3L;27h`|~+gXJ%PTtpq96V?_yL2R`+&PT18WS1(RXlMJ#Cu+=j7o!sAK*5BzM3q8~tKh%7=N@pZZi0K
6|!14UeMK}JRF<PPi&zT>vf(qCw9KrnEg{(NG=keQ%cYRefo~Yk;3u<1(YkO{F6RwKMoR+5$hCegPXeFRnKy2vla4P&qdkPI8T;>
8Xj32DSj)1kF-bivb3ULT6SZp7a-
W{+qXCqW#OuV~0@=3wd;UUUn=~5zO7$&$)O71AA8m8OuukU~U+lOEOT%3K~KLapcoEDz~=oY+$Ht@+y3nX^;$(HpvLkqp+y-
AFAF>2R&UV6;%J!;cP=68VyC_ctlexR*G?HA7tE=7ue1>TWfj-d<HoOr081GIkC@0Qj(VPjx&DMCE*J>SPj-
t9fIeESwS%r)!}g>1;l;2jq3646}(e@Sf`Lls-r@-
1ITE~O{aqd`{8h5|O0DoG;HMtVFVD)3U%0irUZeckL~t%}nZ(y_=36YE+OZ1^Ov6>N3om)a+0q=5xsd&}4{=|o;c)*#IWYWuT?h1
TdJ_n;RRsv{MrQ;p^7$`;N96XI7bqQudHl|aRe{|r&_(LH*9`Mgh)^7_Z<M{L7vS6hEDEG0!#J!V{S=CY$FQj{E?nU+b6eJMz1#g
5=kvj=~%7>_K9jujT6wQO>-
>_VZ<!UuI8N}6NU{D}Wv`7LEl4O0_^y5rI0jo;X%Ezmx~csfw~?8@GBhY{A?AcU<_RN`Q(EVl5~{J5j4&>rjRFJt@aVaX31y8?6L
8K;cAbyR+DTle1e&zfdeX}p+pI%zz|FMW50B*H+o!I^*h&L1)%_{arcy}N1y7!s^B4xtkM?4Qk$z(+Y8LwS_f1LD3tA&SXzJ~&9(
*m4k=*oRuj@noiO&6;$HYj)u=T(fElTx%(7;+nJi-8Z9e-
Zl}f1r+Kqq4K$fXLJ+JHo$tB7)P4SMGpoJS92ouJ*iMOk4@;qHpY`xx;W$6`OEwN_~895k9KQs1$!EaMiF>g9|{la1nw9pD-KL(f
BpWiKdBpm$3m?n_R>2#XYt4`hJe0ha+~DB3;O%-
pmGc}%;V$H1V`Fro(H(k%)D*p?UH<e+=7SNg42>bSF`NXe&+#Opl&|{6f%0~t7zSF*g9aDY%;tZp=Jo~9po#`mI2i3Tj+AmCJ$wF
TX<*#0gP+3TJtPhY(l?p?@%&XwGc=u;%yQpOP%ixR!W?*AGT;<A<pWX;MO1wv6dUrjc#Ue6yob2kfS)H3L(%PsQ}@prK55j-
k`GM<^p~!;O{gVQd96-G-<U7*tg45bSJN`$%lJRP%X6-
l`I14*#bMY)x#)={V|z!HXML!P8r&C4)!9IK8}XL4@p#%sK5UJ2#HKoH`%TL0iaSKeocIzjL40-
ex70DbS!os5!P@`hEf6dqTe^o9L*6ZbAbgQ?r#y^gJ-
zQ1`D~WJ=$oYX>Y}b;3w3iG%>e{Ft0Q)^mifEDzhDc;y%((o{QB!C;rTy4*YD0hpkIbMfD*K`6RSv;OR-ey@fXj;r8jNtEADLj(j
aRK>KU|i21ez;X4p)WN*-
Z9z+;UTI}A$oT?t{jS!XLWY(z96CRalQG#ou*&l%t@QREZ;q!3pvxh2AF^kyk3APN@oPJY{12fEx^U<<!4^(}GiNgnu*u%!@0<vJ
x(HAk@{+o{mciaIkcWmzz@`}q^x6|!mpZt%btHAi(H#@3WLpF19Y=W&PxdRkMdoK!bgUG3t*co(W^%lcOaZrDP?f+@6z$yjmC*ka
b#6Og;KYqG038H3Q;)EK;%FJ{4zO?578lT3rz=ZLAszVkpVuJzV#P0TTcb42;ShscM{YQrL*v70=Pb^(S8x2!^KT5=!%MJZ%>S9`
U6#gg8GSX~AM_$MUifpX?Q=z|LJ?TKaSCi(;5}pdTe*2zrRUk^x>AVP3zYqhO%8`RRQhkT^?`NHkCP;_3m*Qy5?&+ENw-yX&oqIz
q1r$_bTmR@ZElm9s`o_FHKwL!|CXFV5?G*Yb4Ar$cgSZtaJG>3ms~MLFk$~97=DDfICk7U%k|2%EqxAUz5s}8hh9TXdOO&t!<E!Q
DxI`1K&I`}D{x)QqEHNc$#DpBM)O-78fiBtmO_UQHTZSQoqf~1RTr^3Hq(vX8hKPF<syj5e>AJTvsWNvqsA&osTIWXdh!-
=WtW((COch)9oP)80$7J|PURH*zKyoO`*ittl{3HWakZfakY`~QrBV=jm8I@aI8~uTv5j3l7%xO{IT)-
wgd2F@Z9Hqu7+o13Z{++)ADg=Lh`DU9`XzYQ1=pFu09@8RLfO6R8XbM^ch?TDfMgc&+@>_(=pF(&8+yZ_uQ&<K^Unm13pw~TmSiH
4AHt`m#ca;H6kH0+alD~dmo6_{U0w&QQzjrpWJk#5(-L#Dx+;Flj-tlyN7VGw<gH>TuD1MFLiV?@GcS3d1Z609=3A%(nWcemW`)>
?8Vo7>gXgu`ZQ*17}4dTTO9X*P)!cU6*GEULB9_H0GOE(}9VUXeq)R;whaf)y@I9w8P3uv33Zts!hinPH$eQRGX*L8snz?;p6nm#
r3!6Nz>8?6gn7j+s4HehN@QN$!P)2j4SQd0yT)&Y{N7q2NKPgmv##V*?FQ&gdx2t6cRH@SpeLwsREj3g<<uNP-
<TOkH3MLz{7zOZZ;hrp@CUFeKIwaaOXDQ}VFCS9dvAS@g$aY<U@{%?GW=Iitk9529s0^hccfgha-
5N7H4$}w1XJeUL=`K9abf}A>xbRM1$pPsaShuk+%$AgX~bNr{tDqVUvX<n3J6l{P^3zx!x3*+lFrgqVo{?_BSOVM-
?+_0X~pUR(eA+-
gi{I~Lr7&uf!ooIo2iQZfLWfxxuR8uX?O`hF_ZNP(4MEbFM(u7T@0ejU9RkBf0R50{`a!gVV`NP?z_xVvY)Q^W%PjqWo!&BWWTbv
eYAsy$M&{?cDM<fFQ`XNUg@O!3z6fEpOf$&f1nY4PsJL(`WsXm$;9-Kl!Xm~Vz_Wf1}_@AiO-Z2BJ)j=-
m+wDkaKh^7YzfV}y5SQGC73&@vAMGG%zXaFA6ip9>>HJ10F8G)w)jz^JJs<jhe;llVMC$jXqb@Ug**=Z(!2A>fV7T7l_k5?5CZc$
fc0cvbhXd;ODZt}jGaQ2KUO*@8O!bzI#AmApP0EoAD{;O+eSmYeh!L|W^LOKKCcZ<a&{+u34v1f?x%Z<(^`2X~2^p!(t=#7NCYnd
v4o96H4oq7r+LZd(4O>oP)Mo4MsM(J4&^3YW>90^8;25c!Z^yRdj&d1sV<&8tlL-C?M<i-;;YOR69$IT613-s-
cLXfj(EEu5vQTA{Zj(P3$fj)z5?Xj<zw6m-zz#0dNjxJSLaGK}mf-
+q(^~}3#?Qqz+U=4UCB$X2p*V|4tWR4>Hch8nYEaX56?O0e==ro6cu<5<s|Q%K2gQpK)pW?e04_`f6&Px400fEY0>xS6!c#nsnwk
Ne*uH7g++&cZXY$Om@QIB&afM!MoI(HI?Kh<pCX2${BoSGbE3*uO{Az8WGZ`J$IT<WDD$}k$Vqa`Dqq#|8Ew^1#5ViX+3%4HZdk@
XEdtEfm7!P^T$SZQvAXDhY+B8ya;R68e{x>Q~U?ULkxyo{M^P;5)yhxPcRi4>WK=xW%CA0xUzZ0Ua?BwXZN9NTuia-
X`yGxQ%04JkjK=gKC7}XQmN)I?IVQ>XoE=9Bll9g^XkDUsiXfz!wi#BL?DTDBk3e5y?wa-LsTeJx}W!qq3!-
4LcZQyWUvt|Xo@6+85Q0TM}5l=<8;A%c>d~1;`lQ1I7M!4H@<`7OXD)hQy@YfMLzz|97(-
txu(+K1v+T``oCJ&gG0L||DXm$q=&w*{c(azH~#crL>Ns~Mt6~*CglWg&6zv3XRmU+lQ9PsG;fNg#iErHKTX?oxw?)O`)iDxZF#O
gB9e8){5W|-(TULe0YB5jjMPB%exPPvaZbT#|}f!i}l2xzUpV8$9-
|HO1^JhDiN!DT=f%4Tys49<tA0z7)zi|Yaz@Tgk=Fu#aU>YmWb{_=^A9U(>=1G7A`)vLiWT6ToHVI>R|gHf;r_CSU!ZLM3|b)}r&
oPm#Z4~=L~wB?O6U&1QR2M&sU@l8WZb=YRCX~%a+0B90<eV)kpaFJ;?reh?cV1sXAC=sy^3UNSj_#k3BMM>{nKc0ziLq|A5f|bye
uDEy2SBiDwD;X2gXP8YPjqJi7k;mYF#uW7h%wCh&Md&pL9qDws))IBPjV1E^3*|-
yO$`B;9W$~PyHIRiILWZrt$tSSQ1rDh75#f{daN99S%wPNF`SM;p4=jN#@_@z0&cYayY|KnggqC2K&++d8X=mxzAKbdge}}%2y-
wxz?CaDb2&mAaDA7>i3Vzk?L*l4K<sbQTZ}=X3!4UOB)TTIEX_wNryPD15*gFMh{&_@xZ_x3#%oj#?G2CQmWj-EKy-
N=B;{`O3DBfWs_Pi)P!O3*!<9vNt_dGOSqdz6>^R)$mps#r{z&g~cGJ?W;gIylpVBQ}aE=VeaI?YD*q@VIp7B`gRnw*9!Z$u28Wq
ur&wNIIKGP$YB!P|x6nRJiNjZkZ{Kd6Wh!la&7$AJKW^6>PO2vxGIk0WK(L`dwPLOQlO+VSjTi0aWq86SwHk(4>>^<__v4Pf1)76
b(Z!I<y5AhO%pi68hA2Joc+Z?-szc9?RL=RY@!Hnazerr*Oq*z6}<Qh)Fb(X^i(r-=>a5A}9{SlR=qoo{#C?-
eC7F4`MEFvGu17@Yh?vPbEY(XF%U{=?9SWSm)xmKpIfJYyE10bc_+{@pSL%r+*+Sc1snN+(7Jq!WsiLzjrJfw)k3&{t<5%FNO$oU
-LLR8{zhJ!y_eVR^*L1V6;mR=g9vxSh9@#?eg-4)s2Tz((mxfKlPU-
;(@{d^Og_4tARKAXVknpqKqaWEJIKQao&0sS0|f!nSbIF$~@`X2%0k{wE<DXL|(Bu?sXHR{8S^DESv;jZ_^UA9|O`Kc<GI`m6P3=
e}&<N3DgZR-nBCHAKRnT8QQ4(P2EK#48JZcFS5XM@8Khn-GM|5gAQBL)L-
V8E(=H64r%ICNXP<_@;T>tpedXik_20|{8x_=sHMpc?t7G+;kyuTl?cRi(uoF{Xa1kq;^KL}ivx2#We5ji-
`=nK9R*ZMH=PiP3#Gy8|<`15*ql*jp5{jG=j>YTc+>8&%CvhkPY(wyV3e^Pir&hDWp25cO$z-s~0>9`2d@L2Z9v#bQ()k<l4!-
!jkAqx}TzVY($n6G|Tj7&V0>AOVU@#s0KnT}YEh6yaO?FWD?71Hk|Q6nG6G4!!rzZQp|{TeMn}6{mN%hi+{%%UrX7vwe#qg=I|{N
66D+r?q}}@E6}Es|d4@z_qM~i&9Cl0TOfDg3Vhv)54ER+fexpbip+knzKv=VumN?^+iRoZe-
)It`1eG4f(P8t}VaA*xt<=b#t%i9{Qn~+@sS`P}NUWy40oG+0Q@{=BbmIZ-
x4Dvc(8^*CgZvJf2x3fx}O?T^4Kijl3Fa08(lX>>ljrxSqoi((lK$(tpPkhC8mri^{n)qn0ZM0)Y)d%l=o;0Bp0kmPIuc9kL_D?p
I+2aD%!IYiO84jtinGs3wq2e87Ksem>T8f32?C{dJz3{z#1}Ms-fp#{nZ$=|}}S#+6tUMFnjyO>=erTucV!__&TXEVoC``$D)&Qo
KUiGH`3!#2)&@lX0>o^rpj^FPI)CPD=j{e9_SRMa$#R;+rC?p;bJZF4O&-4VdY+WA%?pgpD)mP-
t_PBsAQlHwkhC9+1ROLQDq)gd(%$c90w~L1v5wykgpuq{|nEg)hfzCfm-oxpY6}C}<aBJYi&<OC1-
@@X5yM6W37l0A{hZj9kaOq;00~eSrd~BnE1^%Qo3<ts>tsqfX=sC@lt=0X|sx)|T0RwI&mStqCl;Tf*fq+ZVLF0SZ&sFnO@d?2F4
Yt?br|)|E5?<~X9Q_)c-
rgi%8oLKCI2pOR!J<6iEhWr9e*WbQEhQ2E^ObO8+#({nMJj@!urenu35el&;zzpnJ_N{F$X{p^y9!<(B7A=TdQbck@N=aW{GVcfD
VQFwF;LW~$-N-Nq8Dz(@`&rYXvV#qp-CfZ7=orY+{)l@#aSgeD>Q0q-hrh5{GN8Q+1{DD>;-
5gJq!#b|uGL7Se6#+TkLdYqcR0{}yXgGqK(D2@+z9BymxDz8ONN%GI>MWQpiTJEWuLVnOksKDG;9kL<R77`)$UYL&iR#^D<j#}qf
L9RqH!uYBLibXG-pD)aD$j~S+O2l=d|cGi${(yyW;VopMlCg}X{~CkDYwFzX*9w%52b1HP%0+G0CV|R2HcJ3>E$E;3d-
l1>l<;{SP`y;OT2=a9>ByAi2$Kxca!-6{8^{|mqhbrs}-
ZG7+Q8pfoI8(#eOr!H{)3wAStmig2fZ;ffz0S8Ia}4!C3Pg7CEH&z!x}*DFqa6w{U3D&~X=JX!FA{h-
Gtl<b|a2Q@w_TwgaO}s6F|^Rqd#JDq16w1zl_6Dx6ZdIt{2rKUnH;4(rsdi<4ZB{XW-ij53-
peSr#QyzVGiKHH&QHAmyRsi0P^D`7~RNcqe*R}PNfMmsA7k<$7$8M@^l;w(#}M(`5+54QKu(Sv6izVZ2pddy<CNs(&kjDeD*488v
a`<+r>I?y(hF>>QZogg+!$F~56jAOk@Cw|{nB(_Gn+?NH-Ytt2hwFVY{9G&L3bbITMXWdVI_|?mA=SwnTK}g18sn^cx>cycCl(i~
8xXYrJ1_=PhLdy4B%k)YGEpI>f;8y4?(<NqngKPe#pZh9Dllh;B(ocjqblmmje^Lm}ad+&HWEZF~IBy9tZ6+29$yR7{$`E`JS7%n
v!@SSe_Oc&}myeg;XY4Yk8d6L9IrwbHXMh5Z&)`4G?$ZuwB%L$R>OZnLd7~p7OCEd|<tPH)5GIkx6!3}|bBU?bHfRyX8^2HQ@k|p
Y0W$D7@?vO+l9BZ&A@2a~TZCAzsqaQh2Ob6Lj;$F+hQqNs+xe~APymv$DdBz;jz{p{5v45GN28oiY-
!M@gOxxv!o#K0<hFsm3vdCP(t$|`fwA<1+A11PN51&(I0wc)wqjD8F*eN5pIfOp?LCzG!@ff)F`IL)bTqyuO;CXyG5r$lStGWiR1
R)JeDg6KjIV-R{N@+|dOsd^%5b-
+N@W9}b$pUtu(R=BrqgJjHy%}|(>)Q8o}Ny0tP@}?h_BfAKIJl1#HLGAg!ALitchv&H{uUD1~ga4;N>{U8~Hg$vcw6`Uoy}W5Ds)
lpL`AZ&I+zjSq_XGg^MBrnoq^i=Rd+R*X0^P{&95W&qQ_Yek5Ox;pMYY9dj(Xtht4S5<3iPY^n~V8~L`k^=GykL3x;4cx;Rku|m8
=WID|0IOO;(kmENlJ=RgStkaEzzs!+HDG=Q48ddss6^zi;O6^-DPb7i1Kd+iCRE-
v@cB@*>J!cBfNp|j=lP(My3_?Xxg*F$9w9?kBEmHGy6q(lts<71jybUZ#ecW}KOy3?{lX^g((B#*B{*AM2Oll>n;OteZinw134FJ
VwYul$nhJOP`6N*<l9ZglF$R*Ii7vIKI3{q>%W$fD7YTq|8)Hc=xb=U`llELbLOtPJ&;3-
?xg9%R8s_svmsajR=3Z`jQ(W?p>iltHx!KF`Ygz({jF*+KQ)Vr)CJD#0Lg^Z%h1*7U=a;}g|t7QlXV^koX51`Mh7Hx?cJYQO*2G}
MAXUEqxF2V+|ndVxJEP0eMPw=etGmi=peu6_>9H?x5*K!0LpspB2u@PI@rk5OH*`&y+rC`yq3ty{@BYL9bPDDwR)JYgfO=d=ig<;
B<h-t+7Ruun|!e1^-hG;t&Pq#k2vP-
9ExYf$_Ne}9zHZS(Vd9e1e*Hh_AC<8U>w<0uS<3_4%aXtcIDbk(Ur6k?5O>>%Nc3nGw<kOS!{2J!aZy)ug7s*Wxa7db?Hg>lb1<%
2ft?>+)XCve|$tKTgyh#fvK%ybkBED-t_X3WVm;%F2QBUAjTA&bJT%-
!rZw*~oWav(l<ZWP=vLuXDWeAHBqM&4q#m%t*MO^vNoz%84bq0Z;6!mSBhNwyQ^<@81$dJ?|6cTZGHybv$Xv$ag6&YRi!?*0ofti
Gyb+9J(Q|}xX(Onv6@}ZXBMv7fRx^oDrc@8TBmhi}V7wp=P$WKrmK0OJ^$*SKsW@*OLV?`pA|EcMcKN!p*qeq2Rc;iHDKmjXXGz&
v%K&bSvMu9C{Ceh|eT|`|_q=2BnJErybLuM{BTXDmM3>Cx#D9v(|{uB+wGQL`bT_eymY{dy&75=doz7|G8ZZ^2|$|?0-
X*f1)P}T;EUL>;DrrWh0X*S$CYy*au4MliaTOMOlcs=GuG3%6!Yl_<~OJi#*bM5#6a8)y|y<#8#VYRMv<b+#1V(?$X)>52t4cgJH
0;HcanUk&95#yq5_RzI;qrYP0C7}zY7_$vUC^|o&8Z^@SHO4Ly09v7^(3w^UaWak{?n!Y*TCQ1T_7?RV!gWDfQf6DeJ`J1rQHR7_
0*(mg(m<0cB<>PWMR1qLu?1CLt@wg3`r|gaM+*RfS<V~yygr0=_@8M9+qYLJ-gBfYsfZAA>&+%zbYEJ^0j;<{Bt=%YX|6yjTf^q&
59940=F>!cbEzAVZMEB%u+w98IMJ{fEnP${Zlr<Z!04t=S@2gcT49RAB=$XP-
$!UrwM2ue2o0&0Iw6Sspc~oy=P$w;NC!<0IH?3FuP5J;*RS{fF2eL@aZ;TA?=0(%zj^nyZ#k}ii&n;B?`&aXqXPsB*p<dly4OqhB
i8p7L+`J6Fj&gHU4#-
;W?D!d3VZms{oT$Z1zzM8K(H~Z5}^m1Ly+zW%@|y9&b_wRxmN}|0cLUdqVyp&!D!Q1h28sVG4coSXAXx?QA0t^1waj(o_c8&Y7GD
&D@U5Ag(Qi9@v^pFHXxN%SX39?F5KC>1@(a|_UJ2e5IMcCu<OlF>#(TtQ(#n!y5&<$@>sWi8dIxO?^Kx~-B&(V4q}4qzN~#45=-
<H0>Y7`o|aDj8@Kg_Q!!;<NM%%OvqL<2u-)w!wS3|C-
!u&oq;~=RofC8l6GX|j+vH^AWvp~_k7X5<bueyAT#Ypsvj3)UyDZ~aAEy*dQqPD?rKvJ$*3U<CRm+yb@$hI$RjxD|A~R&Qg)d}t{
JYVj1&gRv6_#*r!O&~?2Yys!SLK&gKaA;j?UyzDuC=4J5|KKd*!s9RYFDM$?gJt7@ls@{3^_6gkQy>&M7sc`F@*MGGHRRw<Z{gsE
KE4sJeh42C27_XKsb;k(3^p<rjkUBt)GStQC=)DRg+Dg7f79rune}BwV|zIV=;a2FtP_2ce7>&;S+VD;I$6z6X1W!BTRh7CoJ4!4
lcO@Zve}0gW<V`)wmY6ibC<E79r?^RzWBp*rI}sajmCzPRBO>>!xmKsI?6&;%nSiWt-
G#j!a`SyL5q;iP}7V#?o{l;dHFf>ZAzeoF34y*ucY@?nBxX4W3P@lNk@cC7;-
dbx#M%+IE{iPGSj_!U#}mA{`3@6?UX0HaXQdW0G^hzcLoNNFGJ0I=crZ#+V3Z*8}lK2slo<C#FkCr_*p80gjNqz+G!Oir@|wziRh
Y4OkgHs8aZSX!@~}j4JNJE#<<QdN$Gfg9oP0`^_~~90K9a&Jt*mF^|;YC&Jy-
K>EWI=HNNzYY&@WsD2xkN7k#CjE+!sEwdA=4?p`~o;>_`qVQihxKo@6r*sau6tjgYp_Qko?9lrz^EiUjD$B8WQ@ikz3Rzy_Z^=p?
F5nx$0F=CmEaKU|RCxu2Ej*?}*yZWgk?s+jV^B~$GRfRZaf*ulkmyJ`U^o;_s}8Alq18vAah{mN(AH+(;4?Hx3N+J2(WA_M^$$a;
T}br-NZhA(NLm)Nsb}?kT|2^<O*O0L>x!i@fOK>(Q92y0z)`;{%y%S9;di?Wa}!MwZWB|F22d1v!v%<T0EZD-
_C(C~BGhz=gbHI#rWRqXA5a)EegUrHzq8+%7Yc!N^i*4e3N4xXKuT0uJ*cs)eVhu+e0bh7xO2o0;|VyTmj?aZy7ixJ-
suGClMMt;GI5AOqr%fZkOj3Uc=-^M6VT59z2(T^K^<c9{5p1YiNirknOdE(%(ERO&Gu-RB?gXc6YX4c6K-OY-jdOWL5U>~@(LnsB
(y(Ddu11zgjm!%A!>}r?G7;%30bGJL;OU#fj?_3t*Nxe(%Q7Y(H4uMB{-
+W$QrYX)o?1EfHRWKxhX}A8VZ5Lacef}C*A7?PL?^tSpzc^up&TZbs^TU{UlQ1$T1xev_fBs8`woxz9pymu8z#;`%^oe#S&+(tKD
@~HX_+-
SNrd_XDaIVwfCk_YC!+3M%u>Z<5TDiRewdn45*5;JrKiaPxU40%LaT47=dQC_u0l<WT5fo(~|<Nk=F#>g2YnRm9G$U#q#sIBO3E}
JrO4jQF-Q)4QSmBzDdCu2#5^IV11&9m-@lq`Gd>vg%g==eu4x12Mvw?47kD94Dp$9%73&`inBA}r5%QEV-
&x7Htr_It`p)*PIxZ@Oe1dZ%3Nd*k?CO)5(C1;8v~*hrN}H{7=Q>+`fCw=n_(XVr9LsiU6S1lB@VGVB*e&wlNhmG3Tugs&qVP=`Q
`x=Co|7bgqTJ$lg4Rbd;f|&Bo{`Ec6RZl=sH8`IzJ`tqs(v8G9yGuc2SyNe@bfNYEUXmWUI{7%xJQrf^tywJD@RV6aPsS!Y78&K~
Fl9yKn>ZdT2!IG93DCcais7%ps9(Q4AtO^KgESXhQ)XOj}vbn8B_^Vj5gc40#k4g$x!p^nM09OdLA7nbFax^gVl+0&=Osn5h(#)d
tr{{elip40u3_Q!xv|>51aNVnR;rK8MD)#K7zDoysPOk$pL?Z|s`{fe8mTHgEzzQM@<*aUI6ez}TcHc4k~SIrX%6I%P)0y3E*#yw
h3F)qyN_pukdCW<b%km2pSpCfH<U5y}RJWnN#)_=F0{2oV58YXE}BXMf198zQW#+$Cf^<1k{_*vIvi$Y~Nb@)gXC62n^o5RfrXj~
*zQgvLS2`(r1-4tF4wwFb>BuJ#=1oy<8@nw**uIib1nta&Ayna2F7c(`T=1|0}@CW6{pZ`rs>y3FfIwhu}o(IIurqB^wa1FH!7p-
e_a6Gl8r0^}Q>kTTz+MTc5~sH*^x2g1fATb<e0n5+h*O|*o?>l#tMgOMLl@|PL?I~2R!tm3me06#VPoraB<@d+Dv-
E=v6(G~10ZW(?%3^0Z`gEyje8Q+_qnx>!%O2}U|Zl|I{NN=D|_-D?rRKKu{%`c-
eq<0^&=qjkfsQijmfHnMNRmNdsM(t)jKICRSmcEqe*$5ZLlepG*)E6Brjjdg=1pu1>R66N$Et4)=P>H{;YgRb*tCsM<0xYc#1vb|
UM34TX`4pNfq}PO13Xa1@GgAOVY8ZQ{B+Pcmqpp3>{g~WFI?XLx#0<UnV~QI7d5=ERDTV2VtroL@iR^FQ|L_AW)IK+1v8x9=T8qq
+8HZxHPbyCahGLZl`jbQ}5=aS0GoV7&F{g!A*aHR<0DHrJB(NIj&aHY_l2H&HvS>kPDRe(jrViy)Yz2)1-
poiFjQ6quW#1~K<H~P9b!;7oGoeQ!<SB{~PI}v!`<<Pjn$e6=3M2IM@rD&cH)z$t9m5tpyser>JhHs<2`v@+UWJC?;prr(Tiu}$7
PIl(v(Hnyj4!h)X)|n=!`~R$Oh!6`F9`uzEO$CaFS1E*<p=9ZYa>~i>m&&#`CMOqhIXIy;bB|jO?{~Ox~2L0<>u>GH}8n@!gg6+D
1~_>BeB=z1zT$TPx7@@*sTWGmmbFQit4aitH&3={sVrf)_mV+QRkzOxjaS{^yV4vbAc}DkI$QhPy*$xHAVTeKev5VNr2<x@~FLoO
(SnN;-aaZ)f-
NmMLC&DURKEvv&V7Gq}WAo8A&ySZO*lfe&Lc4n}i&s=?%Ss6I)LaDW^O%I3EXSz!b?b!a@#^85JiUXTy+5?yELaCizY>YPL3%8b*
aE{kU+tY%KP)5t5U*4n9uBvY`7hKjRgcbw$zBQ*;t8{l~WOfs>gfoNNosDMEMWj%P1<>lgc-uzmKJ)iqOE7?@=OyT#O!L+|_BExP
*ScuzSBGP?NYsZifz2KpHr0@68b99DL!<|f|B$cltvkIfb3)4FX*|A^lEsH)ptIWyWLWyZrE$u-
0PIrPkH*tTry(~I%+Og60w+w1_*4bh4?spGX<#pWIn>iHnwx<2CvH^o0~1~2SxN|OP5Q~U!q#Mz+@(N=lfbjr74uNM!|-44@nF-
<*e3_ulaS}CuQW=x8<VD)hv##z~o15CE;2AaTItu*fx7IF*97gIP3s@PA{&%7E*QBtjbRby2)Znv}AWFbe?7$0yyq|7O!8UC50^(
_8SvfCXeB;k<yRdCObF+9fbxZ%foG_5dkIX{f2ZrlnntM_4u30+VfFyA~q?G{pF-r~n<M)}=`-OXU<pWX)#VRucx5OMa42gB-TA3
VSXGP5E>?EM*x+l>G4(74i|OIf<uWIio}DL4TD>(mnZ>Yt1tA|S{Cjl3HTz*KdT?ay(A5cv_HJbGJBy}u`U<~?BK`8<hSsDN@bv5
gT>vJod#g*|$00hLRhILJ|{ol9O9A6-
9g`z!iDehT=QD?YV}ZxU;#kK)T}{OlVqYJ5S>*YKjr*<X71kGSKa;1TZC&%O#YT80DMcK5LDPUE<VemZo@Du+XG8*NKt^7+$yzb|
3*jsoKVC<@RVXJ4_)G7C?%5tgwO^u(+WFk9Gmy<N=!DJ6z)y72=OkH=8lr{dYm*z2zMtAmy>MCmrjPr_SJ&Usd1f?(yiZ^@2`jD4
hDTutd~|B*iETjN`&19QisPq{xsc&Zd0$mLt9s}V>Gai`G!GSap*{pJ8;dyHV=Q2E!`Y}vD7MG1PhkeNI%a>?^dKYScr;ba&E<13
`S+DLZd0Cr-
b6uZ<^EXrh02+N6n<Prk5y~T4}w=Si8zR@A7cNIDbL5bv2UM^@~=k}u~Auce$O2jaV1a|Nj8Wl?y358j3D`9=rp(OHunFkYjgHZ{
r;&ka|%|gzUK4MQYOP~fjPH{r)XrOkQk>QIyATZmKlWR#Ks$04Jw8gATlyu2TxkUMv&PD?c-
+xM3!^8vb@p6Hxiuu?Vc+j;)a}1<MQFKCU`7Vs*i2=*I{*>SIL)*T`x)cXT7;mfH&cW;(8=VI=hB5J~I>RD_`!pC|_4(Ho+G#hgO
>Te!Cs~7kz%!-mBysQ?ekx+|+kC8)((+5)7w|tZ8+|#ztAQG*-
|KdX<Xnt~(`G`Zrzg`PCJfIoA&#mgeZBm?fYC^c@=_iP*7jiP3*T?%ClLK^p`UKDg2UF~KPLM!XVY!CChj(SseokDA1}LM<GhDtS
Mr7OVVxu!ZKh`eQ@&)(Qh7gSu)QnkX)B#p;Mz);+Wk^^sjz&MJ|4Pbge57;?2hKMxUkLz<hxkoIvo#Tj`>D_6$kL#b1n$llDu`ML
`tyM@X!CO93k%cMe6hhj$L$J96KN4fE##bDR;kJM>(eN#DsHbq=BaRuo%fE79qZr03JouAAbDx`=38(DYywq2;eDmXaa;xF1*K*f
uZ)6;(%l#6Q>qXSp6auw+KFnatBH_2HhkLn<sc`jY!l@bmc`vj5wK-tx0X(?m8VV)Oihsk$K<FR&Z)%uA(G_wo%Gbr7FWC0iObmW
6jvaI-^R9@<cV=wIbQr=^ZG?ikTi`KJg!&L=sBz=A?X3<?RV28kaK`J&n<E(-
b}Ba9E)`**50bl`K8tLdy4wp7_GCBef%_a(Y=K(H17j+RLD~GWz8JZ$0b)R|q#zY4p$l1}{()HuQd?;I2ioEzqqRiV7oME0LuEVh
SmnsB;aEH{e=Vm{!7`-
$i)V>2&kkFdUDDPfzO_Q%hX4qp(l=%#?al^iQB;tJFwiB7%DD<J+VhdOzT<%Y`>@f#LHwYNwcBEBGf)FjK%{zr)!m1CduRB6~Csq
ZABC7We=&f0JkEdHx?6iwH%mRNJ6~;~S8xBC6szASg=(?Etr#40WC-MYh?KTu{#$Uwnmv_OIW6_~E<j|N8LruOI&3>%aWxcYpivY
iLc>#pLZ|{N~Mcd^R3^ZSVsmJl<m>g=-
=_urI|Tgy%zeFs%(1Ev@*ndNXz?M=<Xk4`D7ox5Jo=LLJQB>TmaiqJ*P73l8L`C*!g9NfwX4uQxE$=spd&<D|0*-vSmnMPE0I-
ceAnf6m4x3r(wLr^5pZLpucxdTj968gFXMK47JG3B}BrFDS2!v!xoI?Z3;3-5P|c9DRDS`*kAw<=mj0-it~NUQ%HsZ1ffp#bGsx-
MfZn)k>aYrOuhaS=}O1v$Ez`DRTy^-kF<m`MH^-
xdEga%`7<e$nF*FJYYg7Q^zUYg2X&R@2n1YxAmcV?Rsi+ywF6jwkBXOcIM7iU;DjvBe6XGXX~b0_0lpJ)m<7tVv>@I;)RM12bi9u
HcJi%rZNbXZ}%3JM#zSVGvh*8qeGj)9p)`z@1?ue2vPVSn`GT|b-
e>xZ~PCaIt6fzWcEvTAHVV5|M=exwGp##$6%>rmyP*S5vgRz5hGk>5FMIHGm@tKPwR|fvMV*uQ4P4(uY0X~&YEcL)1&>pD;*COH5
<aOhL!D6)tA@@wQwo@!MNcV*=WMKXhyZuL~S-
ve`ym!(F;W2fYXTl&j^2Em=bJipCVx>IYqw3J%h<BNR8hJk70gW`4lO``L^;oZpOi$(9GO)m5V4Vd@#Z~jl&G`dJsnYwrDpi!T*l
w&CMD5XPAa?h8}tHJLVyWO+(Dj&_?5rn1^r%AfpS0iO3Q25c9JmUHBa)BCYH9MTR23bJM@txI=?(vG9;2{NOicjc2~y8OoV<?EM`
kANG^Mg}v$|bXh?AC^WzJd*-KsB|gBmZVeqV^SXkI9hgjkfr7zp-Tp;AFxdv<3yV9uB`Mr3iQ#Um#RL9ctPeA_uVh{uX0UAruWlQ
r?Q5Q8Wq4bgvsLwPtA<G=*7M!@|A?g)8gSZJICJT?ks2Wm+oKykphkUd^2i2$nU{@2O#V@A0%eRgSHZo_o_9KS(MkBWS@d<Y=$%7
eF&{T2-9+IXONH)|0>hvg_ioHEF&>WIP2S)o7-Ps|GWnYb`#g2Qt;=#(Oixdvyh`tfS$=y8x3tsAo6(!$=<D;h6O<!iu0E0Ox{N-
t%xSewBDfv9;G+1q2%~<}oCIX#xrr)a5_+Se+Y?D0*g@@Wh6cZHV?kdjNj~Uydz?u{1rKzJ;x)q5=Nw~+h>#_bd^C5j%g~zp)Mag
3!+_eq2)9LQ)kD}#vm*tO<KeX1hVB<e_ouI(x92069JV3kcPVCi@`OxbfXpk(TE0tv`&5*9#H3>AeH)R#&E{OCUHjUF#Dgw`v?Zx
lNfMuX7_hzYKI~VUG)|(e%mPP|=2;rv2srs2yUQ=@b0vJ)!BA7!DRSSi00%mll&dv&p0dV0xUgU`vL)>;GtZ04qrrk)2B76ccS0I
u`QqSzIvM1ls7^>;`510Xr(n`x*@Xjy7vn~?fe`*Nt#&g27cp(IK;`RlzxtGvPGHjpO%}C2j?6f`kN7qip%V;Cs|X2Vj3tjM+I^l
UI#06v?_mw3x63tK*h=O-xw%lbDnNC<rl|8=stvaLeer&#7^wDOz-
Vut*8=L0hTw~l4R_h@T9gb1+`E}@exR?|yQtNIf#0*4;7O>aE-H_4bdB%C7>?vIz%9sA>=ChUOJl&KE-
Jq=k6`xGFiV;r(){4kYHB3Nk%;w6ro%}bu=c$uMO1t)MRyC!_2PlNF4hfsl-pP7gJE$D&~hy2qi=i8Ia3(V)pQGo4Eq_~*#r*7fI
TMe;`<<e#@m`$?&lVf1b)z^ScHK*GJyMgdQ#^{a1QJhRT}6@_|vF{k5ush_yA)i8dY1tBgA&_^l$_`_FgrBFuyjtY&VVXj({1A&1
tX!x!r4kvNxs6gNcLz4ZgB*!dt#nPIdC9av-
A@l=&MN@0*KP9+Xq#LwUNNnp%lxDNu5m!8fmC2Ac=%`|rR1?!#m>KKK5DX#;ByZPhkYjxd-M<-37U+0xOri^?^-
>O>`+M@8v^BMsY|!36(Kd~wVYYo`N0OV9+(Gx*CUFgjF;qmv3qbqcdhGc%yLP~_qoqrMqKBe<ZX=g=T}z~1YFpEXD<6C<`Vk*ZTZ
b}8GRIC>i_f#h>n@pc$RQA(_C_FHi}p1d2q<9`R|lfn4y+2njM9!v(45zO^u@@}|`Z;s-
p;8G8q!oQDUW#{1!bSgE>TJeNt!`B^(3DG*22-
TeG2j<aEB?~UPg)1I#v7&5&E`Gb&v60C$eu!>>J0g)}@QB4OC(DQT2~|AoFlC&zRI|)>U2WDYbFP`ks&&!|-%{=keQM-
MI|<$Hzi`g}(+@-|rqw_7R8&)_=vkjzJ#s#}vn7tn*5WWWQ6Q#Zh-mouyUs;HEqhwlM|{6LI`k9D)n0dk9t0^*9P_zVT=(*Md$f)
AZohOgzq;G>bhz7%bKC+fO(}Y27Z0EF$G_zX`~ptkrLN$>j`0dFcZVPxx>nm7xqpO%^Oul|*OH1aA`ZvsK-+D?m8Q{-
o{t(rL9x}{QlO&!@j>5ZyTunb|0_4N-v;)`=6vObYr4@ji~{_}_APaI!^k&H+&_lbeyih|{$(rvLkjQW3TsIr<f#4a0a->e-
=#N{nVupBN?Qq*NMPna4DLewok%??cq>K+;eu^~LoQ&5!3j&W<<pAGok%B5X{wR=M40ct{q;ZJ|3Fb{$j94(g`I$2L*K@OAJSd1T
BqgTMF(Ioz*Hu{U~Y;L!i!Lpcr%b;TSz+IGH)D|)3T*S0l94`(y$Zp8vxJ?OY)@GB<&H7RbK%1T@x&axD?8Ot|d>rRr(u05VHTSB
~-
js3LRJcf4m!CH@l&O7fK7Am%1dwtjMazI%o9GGOGJm1mzbjk+XE#mejONQmJwqOTF_4?wrZ3tGuflAGL?bb1w)@uQzEFXXvBQzN2
B0T2&z>gmH987zKk8!#J8ETP1bhP)Su4Eb4tj?^M?gc4;H7YH(i(Q!PwIAd2gHa9^w9dzMMxvrziJnww`8mYW52i(7OWhxg72ci;
3mIB~CI&a_hR(t_?;Nw*;Zh_C#MmO{e__04cF8QSazT7EYT?BmAK34o+y7>1R*h<n9p^T<H~eiOW{vvkZcXvG}+30$t=W!<KVH&`
@iW6NRaUZa0;WerIDKNXa7#M{FZdn+O5S7LsW2^QY1X=N#(an>ik$phSV#viZ%;m?Kl!$4k;(XcX3igbndf6O&oc##Xuki?^%sum
njIPeX8nWrM&$d-
;1ge%Ob8^9>emb3yD4&N#&mk~$3;G`2epoo)NNX72Q<SOIY3gek#(0;9dT48Dxc!N<;@r|2UMJ&#Wc)}I&ge#0-
gAa>v{q*G846IP)v#PF$k6cy6Lau;4A&zoY6EnHOpsI)O#zu&q(SZm(BRfD(EB<#^7?j7i_wWh>_FNslm}2}z!cJ}C4Nn2=ok8=k
+BHt9F2VPEg?Uiz8mE-(T7hZS-
2AN4ay&5w%+JTR%G>5v`Bw2X0yBx7nR?&7kvvr_7&1(Z#tG$tx2ef!sa?Lt>6Hzc9>w3mBVy?NCB-
z$QEo6{DOqdp3m4crLIdt7PO)#>EWevm*7(WH!#L9gezvTs#Jh_=C1{jpuw@Oc9j6--vF+5h;iz~@k|@SktfBWYE0Y^Cy8nnOCM0
_6ve!akFo@*70tZx?2!S6ObGu3B{_os%$`>2X*A+T~!-
2Z01L*@QLZZ?^))K?k9QXf;eTz3r(K!+Jmrd1#M=I>!UN^2T_S5lT@59gf;vRn9yPHpdV@Pd;fZ*9qBJTD7h5ZYt_A~l8hz%olqd
3WAv_?9dei(Zq4~KBL5~r03$~d@S*VXW={iMMIu<$TG@~yuJ<L*8D9X#y@_fKOhn$-
{EQ{H=c#u;0|INq=U>MRSs$w>!;Kf1DS8mRC#SVpjcrb-
T;CP`B5Hd&qw@h6GH<7~A@KcCa_+pph#GkSAs$xZ&)e)?DLrQfv+fZIHb_6PTBRE5WmAhf?bcJ{Q&YYuX>2TUymV}6v!ZQDp+&h`
%Y(n_X&u!Ub?=Jf93nBB}u7|sNZ>7n9h@*r%>=*xc82YK$-uo1`>M3rwm)J;a9g?Pva-|cI3fsNS2X{yPP?M%;k5BbV|=-
NoWO(<SbS}{Cf-
{~N&<PIi#5diA53=y?%$QxugCMfbkl3XmFB$(=~1i0%v3UnLkkKYSJu5W?M==!X6GeI$%9SPR>ryqa#Z&m?88(G0T3hNTdJf3OLp
M@>EbNZ8zl;<=2c%__TrGv9D3Z^B3t%g||q&p{<)dS48U}K4UxknLRziX`vd!tIpJEah}ZJ$=rHrrwXC6@R2*2*Ipx1~Vl#p;Xwo
tq}s26_}GB12z|-HSo*SySfsKMVa>(*@F|5BNDF>Xi30QZSbK1W2reUzLfnb|yg)>{ubcUNRFy-cxR1I(s(BEu2-
^13TVTX>t1Id=MwM$Sm@i6=1@W&7P8)h&XE|EbJ0-
lp@8YfwYL4<T^+5LF_D_oc49lgvP9@M0Py1krS9nr(?Dm<M2{=b46U<Vkjhzq}lLc{9*+CeaLAn2PO3Ken_H%_U8v6xEQ5f1R#$B
{=2jT+Bq()qqb;EI{r9D9u7rZ^T-blgcQF-8+IAI9P8r9Re|_?={Hoofk>MO%-
lni$JVwZMnwiTLja3Or=vh&$n&PCh!25~YPW?k#}}=E%;^P%GH(c$2+LO`1-^owa1$KK`k-|hWoHZJm$E4#`D;Jz>60N3`X-
O=;L!ZM2mIs*ZMGCPb{%1bn8=0$?c_qCmzs1(N?G~Zie<(NA)>U@fU?!F4r<Juf>BzGPKJ*M7V&x37>YTPg8``2`1IEwM^|cKX7T
t+_R}n#T)D?~(+!X0HPkWS^Q@J<;Kdox!cO_2>5s^mFChT`PRBMqOM9e8qez%6$@;*l&h%T(WzejH?sbj8StssyALR#b9BBOS{};
&8=#?IV?)(unIy~ag@onUV=4fu&L)g~NOic;YF!2f>H9p}}6OSA<^6#D9b~5%4opVb#B{OQ%iHMD>1ltD)s_eOzuZbmT@~D;HWO+
Ei(@|kE2G>zdiy<vIvw$__I+Z=k5UlJtEhJjfd!Mo3@8Kc3KVuN#--
p!O8MX2Ep$^#Zti8HntV8pl`0Aa4WaLR$yO=`}TuR)I6uO>*XHa%d;FitE9V_P1g@k%z2o?ksk@lgGF&j|!Aqz#)7vgp>FieEmKi
8B?^mdapq-
)=zii{a>iq3#eAy9L_iE?6eW?oXEy*@Doa0`z%C9?J~IrV7RAWM{=o)?~yuporq+32);`Tglcur4aG1mmtuNz<CNTZW)S&Zf(3<2
4(3^oHNye?5tmeGb0Urt8Ct1@*+uOD%HH@I9aYsMW)qN_O%=;Ju8XW+Z7du++kN1%<RbM&b<l^J>~2_bd<zd)g9kG^fc459NjtYG
h;OXlyEeQX@Q%lYJe&;Gxe)L*L54D6>(^*hh^np#TlZA@sQ9D?Y-
voUElD7F}2!8gYW$I>ydIFDo4<hUaDxA}XO@petNgk*~O~7iQGH^P$L&MoP3xj$`&>K&KX~9qN-
C@AaP(a7f_3IT<RKdouzukUh?riZeyO`2OdMhzE(<jKd=vjVGzsl}LgM36~JPmw6Gye(vYE6ZwV68MUB=@u3;B<eQ5oaz-
2oO8X!ik+|haYHZs3v~BAO**AuNqn~@2E!1Bmv61cvp@{D~p7nsi-e}*Wwoax|D>FNZ8YgCZ$U^ACusR7T5i<U+@ppaT-
0M>9FpMy62i3;kHQunX=Yjyx3nBqj-
3fk(y}3){7{F1;oonVp@hdl(DdQ#0#D&Z%i`Aw$GE%iOAf!lULKsT&!ms=(l|+FJ9ycUGe|qYk==tq*PAFMjq>_}0R5zkI{K40v3
;a1HOU3q-h`1ynXO^Z}pxn~@8|G^?oN5A2oeR{eUQnY`(F5Ex9F369a&rW*e3*L!-zmaN@QL(Ns}6_Cs~)ABP)(djU1ZH4zr-{-
JbT!^W%Jf}f{>n`c*?L2&lcgInEfO9B5|nwP|pH2B0%7vo3ZBFqcDqnN!Nl?=-
T)C7TXlM?&~CDb@cFs$;4iB_2xA+j!Df`UD?tmZNPrH0JdZoZR2Z6D$o@{$jZ!$oTaTpo(cyN_yDYIPr2i;l4~e}ui*cNjiW2bz^
~v6VVN~qekbEuIw#PE&&i{ndzD?@gqLDCLo%qgi?1-Bb)<zU@?#3`EeoBb^lC9_ra0sJqdlgjf5;cka1Oi-CJUs)g%MFzPCpjYA&
)<Np_4=*TPRbPqGa(=MGb~?IlbH8eYr?f&MpVP9JL0&BpQ!<t6h;|oi0RCFpMPbC})4@$yI9<j?Lew+0gm7?KfTX6>R-
Rs6gQMWIz4mj(@nGCwO7B`9ca+>h_tFaQb|UItmqApNn<yALW7&&rgD7F)uS-
Mh;=!6hzQlxmg|^uEaqylrYa4h^M&`W6K%)o+9&EiU$^C!tSx0T7E&f4ZdIW5QMSY`+X)B=%RVk?aGJFcr-bKo9?KckxVwxu1Mme
R*A1QMNDGH7}1zQ(avIcCVV-
6P45yIfIFFJXN3tRl$8hG*XYEDe{WRu&&lWx9rE+r1g~}8a+7Ww_eYumw&<Xv6Ap|Tua0)~JBrz@5}|OasK%8~^?o#Z13ZD19ALF
hR-e#}y{(inikRv$U442v+?bOp;y|{R84DM8YdBkqkLOqYf1p(<_EWDTlM>M#v2@?58B1bVzcx)_yD%L}4vN#8C-
S7>5o8?JHZmvMVxObv2sKtB6~wF!-
OYI1!h%<2E*HkXGI4Ypt0b7tYsH&|FVVl|0VOqS*CMj0OtC{MHB1J=e64tN2rRKy;san*L@-
{s|Ln`b!tmo2=@Tb@z)}UxRvE1Yjo}DZ6Cooj>pHp_2R@%l4D4ESJ$yn5n*v=76jmu8@RST?;#EB8??sXexo);Zf-mFum>dEa<>2
_KmnV3ZVno0b`bp9!kuQd90gj#(_L(Vjy<3A|rWEG~H+EnhjE&iR-O+bo>2RCCT#EH0-
}1u|G@Qj@FW{~r{Ja=VnW0xAduGshE$9WXT@>wA2|wgUAPI=vCH~^AnT?<^nlIs`#0*PzssZZ{a6odiwJfOSgnq{|x{D=q^1`i}u
7Q0*Yd;WoZU<7Qv&8$KoLGFU)no0hwZn(F#Lza9=1a32i6c99qz25bs_qdTb1r0GC}0*k$iM4NE;@qAEFI~y(GODwZF8A3t8)ir)
g-4DO}iRR+5Rg8*j)3<u{uwsd5yT=^8+SA$E%5zwzZ++w2mWF%m7@;rI;a#4ovEy0&_c};m9^q*^N~$<@c1EWgtSIli7-
;&PStIlCER)YLUPK1<oFyaid`5V+!RcjxmHUrrRo!c?ZlO8`Yw;Vpo%RLdIbw`GhctcszW-jD|NV2aK(Y5f)S0!XnZZ7V~g29F2Q
j85sEDY=qp%&b9RVvYQ#5YAw{{kKLsXT&1WnA(i1BWeDa>{M7_Rr2BpOyjC=zWxKOuC5vZp{gr<u^TXs%$v;DZr;?FEYHueT<}Vs
diatlVhD>7Ja;kELt5)yr-*>J4=daxI_bpo>rt9D-U{&K2<M4Vq#8z61)Ad4~+b#AUZgf&Des_b4Dq(w5QYX-
d!sBb@gB?kWQn787<!CxJKaz<gntJ$SEQP60nTL`=5w$OXvzWd)ANsz5++A_3wwMQ`Y<4ZWmtj11*~MtPT8mT4G;y#Y(-
$q2tKoeVe?{KTX?$w>I0N<lz7YPNsOCMJV&W=3Au0G9?i7kevRY##EzW{RHjBbr#so#S|D5Jx1$WK;2K^#{MCS>beA%Z$F%)Tk)h
Tv<cV_~FgEGxA@?F8x@{nSfUVu^y2hsws&3lo-
U<)tcR);zYJaR01q&!7?MZ`&E5Qw^$?g@DzT#NTlPf2u}<oc0+T*F5(S!Wk)h}%Tw)d6e{RtJ!|ojTNYxI*J>j9R;@`;$h5+7%jT
V_e!*MQ^J%p9jEPK~}m|)q~cA3Z+}u`1E`jmtSCt6@J3TC`cnl5Xf;6&%8hs)f`)uj=@%XMnG8ekYZL-!r(TfK4i;b>r-
M3oQP62;D}BvQ;97gw6sH0N3^IO(Zy}*aExfJOSXm5H<=y9Qud6#&7;i>x5<8!spJZT;1SBX3Xk0`eG?o;%Sob@6#0xN!lIVL-
$k1;$+slJ5q@YgJ`cA%w05zI4GVLt1H)SWr9$#TlRz`lItLi>`Kz;0h-(nl{L6oO>XPTtCW-
D*X=E5#Lk%B8xtoQxEc6?*NNPIDB$cCD(QO{H!0R=;csCu{pR)2t_0{Oq#bh)czxAi@tYha;BgQoN1k?CT?kZwEI8Y7#oYNuP@8f
UjTSMjJw-@;Lt9PW~8jL5w-
F{Q@yLT7ksjT#Nde%Ir4s@{F<<BwwfnKHyV{WDvvii70_{k!4zcGd??2duL;eYMh+1|=kAIH**tB>(jP>Y8e9!PLT_27(BkhJ8ZZ
JWL%zo2gpi7=3PSJR^=M^6GbsFoHDQx5_<F^NliEtKz_O;AOamZ5y;j{W%@+vW==x#lzl!FVvRZ5x~k9r)g-iXp$D-
|EeY8z+&je2iq*=!;v-_+v}~Bo<WApKGD#MT?Ap#uDIHL98^uOMhl}<|%|Wap4H2Z}*2^+Du5hJxuYD-7rq*+gB-
+p^M^d58{KUci_$-M5(-7y?6b%e`a!>a#lD|8}q@9IiW2nyCi#J^1{BQo?cq-Z2dHYWr##!d);{crs?NxOBc;F<-
*{eaA*^4+f3|@j4Q_zuDl)DxmMne+S05jV|ID2*P%GsEo!OZ3K&u&L@8Mus|KTb-Cq5WSv-DXG&L~br~m^J#GqUC>c!{+8lRF~MV
o0DPU1C75L%SSmx~VjlpaW7Dde!DSwr1RUzz$|G^WA@qZg#z4a84i{uoYQ2y|sROt`KMGq1{Pv%FS@LS|?$!<L4&+Vw-ajkAYiV{
Egmk+{rXyEZX=8rv?XFVDmy3W<zqSjva1S&Js(aAr_}N+g$ADxnOzR2-
(>mJ0l^y@j#}vaD_*KPWT1$hGuvO>vaYLd>8t%~=|TIZdMyGRjdVCG7?!$|R@T+LA3Z^p9BUxMn#*3@GejX^EXFdOl!fVxSu>>Cq
h}DUyYrjEGYB>_$|fdllQn9tZ92(_)1q8Vz5uPmf^dmmu3~FqwK~n)R4QJ^a6+NSWBQ@S%@e2%5CBd&M-
h(=_E){_<Q|BNqPLzmJ6VT=T#-
GTAs14&@_eM4=h_U3en)NQmZ6{}&RvR=g5KqT5u%V!ktjQZ!BKUQZAjNNGC=^18g@v~4U5l)ern;rWjqqmK27Jp{ToZZXtaR!aA_
FCK@)FCDw#ccNEJ>R~ycKo7^)@|YNg91oe#I%E*kf{ySBd{(iQ16vZ)+#a<|KZq#@EPKS<awZTGnkCz~c;%F8VR@7P>W>d>$3yc4
o6~sJZznrqBRW$ccQ5W-f54Bpk%{H>*&bTv-
F{YPktz%K;f?Yd6bsCy(<aYN%ww|NL|>j&+uD~P{KFAY`1P2X=$%QFI5&{493oUm)4bIs{N}yLNZX8+UyDxU3Pao>`z1#kbi#=<(
jIa&M4gcHxBZsyvr{kfWJD`lK&O4?F|2h(!4(rusm!4ZR}^>Ii^gZ?r{i~{7+&V+f?Sw!3;(5vt(1~MBnbv)Srt-Ul;|F%B44~U(
1)N;37#wl^tIN^#zK?RHo4q69>pzz6Motz;|Z!7$T3`nBg+%=$&xE(gl{2za1_TyBV_vNX?HT<YrF6F2<p(gO!B+5<lyEcTA<|08
HGaHqD3A>PlrLJyTXp}l5KA(bQ-
)6iF@9C6`;;zmyppPA|!Ph#lMu{d7VjW(MZLuy<aA9;wTrs?qoQyr42o*ASbLR;?i7mj$HfF?CvkF^BVfZ?5OQg6)0-
QwCH78hW>LIIftOk0u2^-=*ypA=$rS_DDoj=kF{@TOA!e$!U&kKIC2jx5Sy7O2Rcf$O`<Qu`F~Sn-lrtlaV|zDZL!S1V=_2QVCKR
bso#pN!Y2tgft~gSZiO}@FQM_{7ejMS6kZ#72TTyKeAmDHo&Yq~{SJ=yYu#?ks$&59j+7r9dL7-
s$ar;OEV1zDY}u#Zb5p2=OCD589#qRIuf~Urb%vy>#o$LVXCVhhYhwjt;&@@0uZp)$R4RaJ;oT(>N|tZ-
n;F#nyiaqsh218lcT3sP?Qkg%nMj`gki(YU)ZTqkW~7ufbc*{T0e~LbEMsW12h7BcNMak6;A99|KIe%OPyH^<&c%2-
IV7o2+0F_wb2v2NH$c@2|FMxLNbzFwKdHUI52oTP;UJ%2BHUhg+sDY;DJ|M6`DzkWL2bhj<HFZ-wvW>K5GXsH%qqH=d_}MZT?BS9
n!+ukrt(uNKNJ)|!J-
D(z)FMMN*G%cZWEMkfKJMKw;EGOAk$r}yi9I^j`{)qPEZxcM(l7?EfPNR_S^LHJ~>d?{LbKmpPiKilqHwUqAU1m+?mB%lo6&7S4E
`~XW*mPBMkW=VI{wDu&P0u7y@Tr5_3k0A}OzRTtd+YIaGe45eC>HH3jgSO9=)}HxdvOo#kBu?l&*>R4&G}2qz{P;`~^O@bT7qnaK
-;LKoPsEs;1<<hIit^HTADfC-b9JFQ;8$GNG7ufTf`+7ub<`FzviaggdEUkl7@vPqW-y675H>w+RH5XlQK_rM?lAF{2z-
?Cd^g^?0xc*5rF(N50hyF3xd;$}L7$?b>QHT0cy?45^-
AMA8&R&;?$<p<^)$&Z?EOvWFBa4~+U1!*SOMRACW7Ee!_i8Uk$BFFqZd^;M7E;g$f7JRar<<)-
Z^2btRJ}8c#u$RZq*GqM}Iy2sWF2L@*CMtjMsD^V_)R5u}_ymcre8vhygqx=~<-
vvna5Er|Rga1}GJP&0ucgcr=#;!qlZT>tdVA4*lx`wySHu>@&yosVK!|ArUYI31W^2r%4sFH)dBH@t3J1EuEMh(*V)0;L0~C~bio
+UuA5dmyUG&fj$4esoaGM!uKVt(8T5K36%>r<gg$Gwh%-
St@f}+7hc3nnGFO*u4CN2ZsYQ$x|p5%E%==MI~WPrRrtIFOzJsquIyY<@>RChYB=q^k1tsQC+Z^l}xv^Z2ppgmRc?Y?w65)tR2MA
3E**<#S8*vfEkHyqmi-rG?dCw+7bFLmFEp5}67+v|s@<|*qo-X7Yk2L<{l(Ev1qyJ#V_@8-
t&4bFsZQvVrAb#@CUI3<wBi!XX=Qt)dlI$E?U_Yv7e!G|cyFk`btlHKLtfzED6Q*Fd`6WlVXGt8|C3^TP`usPHU8s3(e4T6s+xSk
aVC1W#fxI}Mb5oS9vFJLq|CPxO?47==F@S=t0Rk$K?CDJ$;%UTTZ9KKgGp2XQ+C^lK~p%#Qo8gi)XUrGbIuE7AWd@`o%YTkFSq9~
VVqr=JE;lJtSBZJwh`hf`^cwh$I^dbCCpLk50i40LVJF$o)s35`|342VhcmR6tLMjqY4R_~7cy*S+aM9Wc5rk3eH%0`JppEl|wC9
RL7lr|wrzWu>qPa~njJ8{2_eeY=&kl`d!^YxSw-
jR)S}z|{aert)shux*iKfk4Mw9t67z-5oL_8Lf8(@@8Oe(v*#n3&x2VhsN)~0+y0K%^d+(4N54MjhA-gJFn5i_(TjPs0yMUmKg1J
<eH32M)Yl*5Q%B@Cw9trYqUk>2~KMDST8RMvLg`f%ODf<WOpSRUY=gri|;@k~b)^EX|S^InV{A+*TR%Ix*<2CnC2*9bEhhcODIgS
g#<G>(xW9Z!@6eM$8ZI_?n%DnL1n$)UJhgacuGQqIFMjY6BW2%+W?kF-
~54;@i^^aZ1u^Qe^6wtaMw98nvPa_;dEpPtHv8lhpm_nCs}l)ac)cKTJf<blHunXP_?%c1@aSn%_Q%b$M!_4?-z@4x-
`p84<+Zdg_c&laUu_iidXs4*4+i@r>yN4a)?hrXP?``!9-
E4I?Bl;zakfm_GFT#);+ytLN^EOTVhUo0klEs!sgj`AJ#YgaI_%Qoo>o!gNvADBsCP7g3}Btm(4-
lU+2dBjj0fY)?_Us&v;xMPSXY}gnNN@o12%V9n~YmWW(pt<KJa4Km7-
>@pgk?=}|mT?bukNVQ~lELE;)QW}90=mORP>1R^`rqD5C&pT%qwVjxua+mkzt;s+0uH;-&WS3y05G`ecAeyDX|Xp-
G3epco6zv0x?9%;x}TsRT7>+M-<+R*BUFki)SxAKw$``PIcaE3acH(og^GDZEuxgBT8L-Xn7zQ-?=*MjDw>NR>HOH5yC0JW77+-
aPntj&VN}Z?g*5FPsd<fA&(mAFkWf@Z1ZYAdBN*?eU9uej^BZ}^K3}cTKdnG5Ev^g7_4VC`G(rO<qG$ws#<5U#yHMkG-
;9FTGe_GJLJew|W)L_*1Mw9fX5uVy8%6}=hV`DY8#Xt<%MJ4y55#xvO;c1F(d=A}9C?JV4`R)$WCi}e0F}@IL3VkrhH<F#yc686Y
vG0(L~GZP78w-
6?zo_3=JK?4ePH+9+Pm$sYor(?RO9ToU{WCECZU>y{?Szz9cYnN;xP3px$+++I8+kDf59pr?yPDkXjw(oN7d_p$+}v3X#OUzyd3;
7{X5?yF+&qle4s;olf?TlbP(gz2QrjTw6z43Y<%DdWklvPGI)Vc?ve<-
@?$n#MoAfx(6=Mob&^8rus0?W*I(ZM#|LkPM^%n6?aTSKW0YW^B{R%6$F0uMwwLZ9DoBrsp^MN(ZI%fVxVdWeIC*N*4>ZZ*JalRF
)01g%YS-d2tVK-
Y)063Nz*QHaSw>agvQFdcwl3Km`#*;;+EeRJcBi8zbgm}(81Z~*VzG`rtvoaK#G19tscxBAtZGlIsQUVe{YP(anN?xNxm$$NVy&6
MI6>JbC0k)D0+G#(m19DhV_Ca7F$`t+`d66Vx2k+E9g_gO&t7`Fsr!w<lM~WvOZRx4F@$vBB!v-
qLyUss_G6SNhIHsk`^!=gDEg?22NN`<)=-1J2$B@}0mn<)H5?{HpOx5YKdgH9(6{}thQAfrCcb-mLdnEFxCb;F`D}YgwgTCXO$A&
l0o#yZ_i`jV4j-MT@pVCY%E<$p6Mu8$S$-
(%mQb9W`y2l$>1WAlz+rnrG5uTeLy1p^EC5h4S|+=qCBbi54JsoyVp$KYM6Y!9Tw*k|BvxTHBJOa+-
Cy$4vJsl$rW;}0Tp4VGc=+r4k5SE{^}gy1z=K*>4c|udyY)w1y$+zU-
nJiA_BYS=kkQ4NT$`J&x}U=F?9)7?NzoCymPRsM4o-lNRi7uRCnW8k*dZNLN-pcR59&ToWAlTis5!1+f(9@FaUCw}K~y&wnYik7;
+kTZh;`tUTQFsqc<brOF2%VtOP8|l(y6<YrH9Ng@jtY`!A?LgV4u{m<r{ryK(GKrIKEkggL~PNh(bP+oFTf!AQ%ODI__-
w$N@{I09|h1zPT=Y9(ut7vTpaA4TjfiG4*glEbI;j4B52=46!z)-
iVEGFPGlUBV4oCSRU=Cw&Le=KzFc5T@nm*#pfonqP}7JI2Z@x=N5g{&KV63hi`d6?eNoHcbP<YbZvlLhuWd`ASei{ZosMqtoln}5
yi_<6P^^|j)SP|IEu=S!l>+O{P)Blid9etJ2*Pr++A`KFsw+9E=n`;Yu*(NT&ONh=@*9WK*_79r-
EL<UrL%megpVxPSu6~WWO&;+Y+e=v1iCrAOI@`T-
X(#4!Q=!!Pq~Y46#Pbp#<HzJ+^L}{tf&a%h?TXk1Y&Vu65<sGw_yh_Lh+Lma+Dh&yN#Jca4E;ZqYdIwmKFYkX@0Jn=%wc{Z<ln_%
6s>O40mjlRbojn9DYTH4H))c5g9_Bfn@J#Yq?21O^cpzwF`Z2|qp-Nf7m(dtuhe6bEx%WO04U5sBDN3bWiUQ%(TQ$z{uE>*SCX(0
&El!WJHI`?^iZY70SRly8qTs3N!R9VYMPw4Yd97EO49{@=pkg5>`e&Wv^~3=uR#eHNoy?aVAdMcNrhmqsF}f)L;X1t9<mUiZ)Z-
q-
!Og1@N0>It|QRzOXmC~WiXmuui`+S>}S^MyrtPqBW0xm8zAncRkX3sw(DBG|96t|_9sotwEtE(2_PPAe!4fd;tqP@)>AZ1fO_bf{
(a2^NLAOM!wK;4Vd!ut(S<TB{DIs8#aqxQI#C@-
*5E5r2RSmQk)o!$2G6{w1qA=%_Ev^EtxlMAu$27dm@B8Vnj!&)9|u%}Sj42$2)5rHEORZv65HtyM=f=g#bU?dah8+^N=);~oSOKC
BirWjIBhj%v883dInC|M&SKsk!D!uWQf-zjZvQ^`&O!T_hi^4awqHHwFK;H?=|MX_$!xt-RZm0AB^_ZL=X-
pguRZJvJ(&TGZB@2;TA5<ohG-
8Dz7O{MZpRweeZxA#U(l!sN;_N<GAOx!jc^K!OPy&1`mVr{l~a)7If?2ckr9)4tGS|9|CudvD`bmgoQdDO3%NTeK)yvdh(JOH?5}
Nlynmn;1!EfQ4ZglttUDk|kM`Y%#X?v+sG`Cod_vQr){VYZOZ4C3)YE^Z1=}eEQ0+k$B~no%R_KY&{=nP1`a$z_b<ylUzpcU$t&L
;!3c-zu!Yu|DYGij&8<c9aVtc6hYc9Oo#-nL>2NRD9A;dhD{-AV)dc=A|{cXSZsDBHCE(Hn0&%Gn=l&qGeUP?c%d843X=rWKEkv-
gEbf4gJugNL$0%Y)^77xU9Kt<A8?v`bPB)Jv0tiYZMd@80@<>=-0^70?%C`RI_=M}BD$P+bi|pH-8_s<HG7#?PLtClMf#hwj)rP-
w9_tiu9b5BLsut;!C4#*PUnlX{2uvQxz+}nbJSqp%8B@=f@uz=?n0upyId_M=844HC5NgK)yq*I&X=1tfskBv862UI1)lFmi|gyz
hFry?gbFW;UhA*f`YFpCZQ7bnVs>7}tAg>laFVUqlRT_jocoLQawYE1HgjH7Q`g|Jnbo1?)JB`O5F6qTdp*~nKl~jWLS+_k<#w<E
s_HKLVAuTB-
Q36$MYb9RWRzRMw>bUhpityJ*A)#XUgVcub=4hQ3i!!sQ+U(v@UR`4d^%P>*v#5(y)nb!S9La@1by+P0s#!{w=MR({D>+NQJP2qV
uRKuyD*hGa0#UAt$<bwk<c#S4W&aNSmS*zs8qs>Q=BziRm;U}aa%%ZGp@6<ign1nB3q_pfPrxNmPfv#neY^KK5Syd>Tf0h9L+cqd
^qKfR6Jt#e~jkF%Fr0mm^b;F5cIGuFJsy`fc{44fnzRLEYoZ1t!e4H++=_}+b`c@rW`YwibCu-hNpmeFe@Fb!8=D``SPOY07~c!<
A*}0bA-Q90&5}=jhG_D!2n)qBt6WdrItN;xir`}5xY0bDwGM!m*i;`NhJ?-
Y<I7~GgVhh=Id2gdF={Bb*KL1?R`q~O`Jux4cL0hj4X{s)S^J^I(DQRjh)ZKUhCU=iz<s=Sl7&o!d!UFoGn^761H@nFKCig6nJVI
HQR1i_$0MudRyM%;I!vV`=2&za>7lnFSAwm{+=CN5^rvdY{^R-
rfI}1hMjcb8zxeTEJJ#?Xgw`fpNFk{EsURNP?+V%)rBr;XO*T2rNzVr(ltzT`BW7U^Y&asA!6Snb+qX<`$64C4r$c6j<<v07bww|
FFNiQZh>V(fhCzm`305@1@ag51>Ur`0}^x6|JyzbI{WoX2Ivo+eLvpzqhaZ!l#l{FpesFxQ+^9P*<e_DsU@WnIgkL`R#}N=;i5~(
?L|2_EIQl4WjFkwA9&E`FFN4|{mXH`UM@Y$ZfhcnilIE&84+8xFWVH@+bmyY735E2RL;1`y^xADT4i)RsEKJq<T8rdm6&~Yd%+7m
T@%GuWTt^@euOTqJ?h1{G`F=f7G~K|rFfh7&XAr;Ps^2ceJgs$gNt+A0~@P6uzC1R6`IQ*>#&6nGMFqh4IGT#foUU*N5@eW<ZMk9
@y+AZv4L!SqhquhKjsbE-pUe5^6}v1<(=JQ6&2V!-4hh)eHuAyF*V-
noweuBO#CBO?LYJE=@&WnK4;~#*qg>;T^h>ll_OT==%QbWmV=Y4)Vl#Xs12EgB|h+7-G@xcS9K1-
sZt?bn%ZBbWGZ<RhoJ}?#bRhehj|QrTd{j@{?N}S%$C=7D9m<5uwtKz8xxMR$!&^y>}R>kkTa3Y_U3Kd>?>b)#|65hH<s#Fl2p>o
PdT>Q;i)aew$+-
SMZ=2GnsjIVC;J&Sh|&!m$P{10o;Y+!?M2du{S0^@_6BUZR1N(>IzH;+%27pm8uWTYo@{Gc%~BjJc%s5D_O6jn3wNS_fp?8JS{%~
QkW%|g%-z)2dz-vL3-@=UfiLUDHq|qjE-Ujt{IdO>%qvWhBg&PKohv~7eO{34im<K9sPtyF_$`|Q=TuOb2V$w2<sx3m&L~6ck)uh
rhRH#wZ{n!QiH1l62(^S^ih!X)enmqlk5GJO=atU#+c|D*Lv&F?o@<O>8bO?+H--F$JWw&BeY${i`nF;u&Q3uLXu2~Pi=z;OMy_l
LVxqFn@A162S+^r+O@HM$m!qV265ar$9*nf(psy+%WR;_^2yara!9penTGyMkYb5RY`%pF&S6rI7`*1h0YKK$^ZPYP7En0}MO;;^
+4~LOMkga4lUhKG5D;ZPqLvWuiMaX0g9ab)JVS2v%J>F)R#IPX8g7IaNiy(x+pkHt9H#3p-
tQIuQX%jOOtwL90{eiT`9}@oKh3o0BKg>JF0qU<piS5yMv;}a+pA2JX=Zz89JEz)|k^QgQ5KrkJ52r}sX}K>dd+hMjI0sg-
+x;ZFvKwTnF2<7Qw&l~M7X{t#$x6_ropc-28|9kHP*wIf=dP2ZrV!t<c2=mz#pg>&7IKEJnVNAk_$2qTPvU#tj9XI>-
e7I5xKYj^@Fx;6nBcY}dT?-HJA0<LP<dIQJ8-z-
`p1EZuHJ@e<}Txxmpn=O7*vF#FI(1=17kheH@1@lV>{VL%gGT6+FxE=Ydu}TDqS#6`aqQP#5AFi4IRpT<121>shhVZZl^m~|8LR%
NEdxW(vE_-5*<uX9SbBOBmZIwEpZJS%z~T+Iv9N;R8DU)iiAe?=NQX~q}FWC-
)sGZuAIa@p{xT~Rgb_GIVTnd!h?cvAIo5m+Y}^twA`+o-
rsXv;6e`9Ib)aoy^pCWbu`vcRJenaw9{zk*sSS;Z(;64TSWjTe`a>9&k=u#BJ7?nx6Q9zd`1RVUBgPpSG_f1ypy9faSfx4o8`$@+
$=2?8|tSn5is?jZ;mb2yK{2sXRr|jv<5>z#LFmt5{UFeIE3s5mdmE+v#bR0z_&)HJ>>^f1)CkC)xRL`5ljtVzf1b?+9M;Y`Q{03p
7<uX;aDzc6!e-vFn{(QJjPG@0l)Z!3$re+lr#|q$%z^J=QLi%5Aoy`*@uEH-$G{%1DzXA7=mcS*sn*)3H;wlPDwbj%(l-2-
FpC@@aVYL!G2jEiHS7f^d6w+_(`ds1duiUv6Vi?lSewZ(rZ65AQl^^kI*Ln5k8o}hYg2Vic>hvN9i>J0dlSWoM8GSA5AcQl1q?N+
AX}(1UfZ+yhza@0hRZ7F@fueAC6c3S@98w+^ei(Dnr|u(3`ZH`5UvE+w>wYWVyVIalqNUqgM6LVG|kvR=WS+a%$He&fFaJ72jD$^
UmWu@4WxV9q4s^WKMa#P6CEKazgJ%0We+u1tdTXT}5*#bP$`BO^0<=D)c)J>R)E;6-gg{?@!Uf8c*&C;~~%;Evi8CYXn*-
ug@eAaTp5JRpH6aIw?9~mvD+AniS3JpiV&jqe2~zTQodnAH;IV{kJ=d*i*cF6j6v!NrPh~ehM8=^dX&2_%>ZQhmm#uRE`>TqU<`D
^aa`pb*;P7JCew3sK<ABV2%DSA5f=wKxwY)nOyo%hf&wU4$UW5NJ`Be+g*>@s88-
jC(3#b=x@wJzrZ)<dBgsCj_T%B`HX;5J|MYHOR>h1`_~v2+4>od(#{hfqZn%z&QG{4<+cWmYaJ3)6y;={4Z9+=e-XP@-
N>I!;jeq|_9tJu+rM$A|F5yjzkP?_e?KQ?Cm@8e#7*`q`v{uY-J2_A+Do3}a!F4q1%S7+ZSuduHL1$8xlxT?Sg1IHTU?RHum-k_`
ku+a0uH(+0ehH_jsr8%_EawlE7)%#d%fJ~hL;L=i+&=vN?YUMBgJbSji3NZ=%ihskawMFmnjTt5*P2n$J#mJY5al15|b@Dypv0e0
Be;Y`h;xTrQAQ+o(vtxiZ-
uT{&&gWaV{46cZiG896ln_(iSk@>5`)Y+`lm_#8)`^yY_K!ANG*Gkt1E7?sTMw&beLZyJ9Ncj}a?mf-vX^8b#|ie-ttdwne7FXrO
zozvVdU47n}*%n%EUlN-i`>`qWpV&SGxCeQv5l~jaeFjeP*L9(NkkF8^ln<;~@asF$2)bR0tbQ&E3i7-
Ywp&KLw)KUH59n}xDNA;t7R6p)Gs&`Ns4d*mZ{bF--lW&u}GYDqiNTP&K9GE6QXP*}0B9()<kW$A)hG}syQ<yxfltf+YyL4Wc83q
_;qch`+d%c-xegQ)XXk#Y{eI6E-Aa<a0JWlLozJrZbo)L-
up`ld7w@tspNg=D3Uuondr`bB0GEv=~!sr5RHna^j;CUnH!^@74v`Kp<Ij%Uz=m@f@YtkJ|HL55~H~O4B4suGVQ$n5xr5EWHrqp@
&fgE9iBjf|G$}YJTKhe@H<*m@_Gk7m*Ch$#tQ3=R;2$JD)X$X5dJe8;{CcTvO`pR>SF;A#6Wfjv+NjDs0scGThA5Mn^zk*YOS5Wo
6D&PF|WFlWc7;`oM|D=r_B?!=hFE19w#s&Vf>1W3a$+_AbuXL1WI)L<@PYq|=<qCxZ^L%~r&1LfZJm|&!-
fR3!Ry*+9m*4Kdy6CHmu6l<rB;{3UjwxGR_GLu}UK1qXytNFVT-g2X{bGu8UG{};$r9v2X&y9q-
Pk7A9Xyf6#Xwx@dvYw`;%!IA=v4sz+rpNQXj}ZL$(&lH(V~82Mnw*kWm0yJhh;f}ykOy%r55&Xx5*YBZMUN>J=$)oq$2@`FG>P=Y
%i1KxWUnbuFFf3`v&DI2iI9gkanlTQoMvKV{O-
Zh`}5P`y@wwj);xyH;$BT!x4Rqv_9J_0Cm2&$8#CZ^CONrvJy!ARGVYiMhtp#Nz)Dvr~vKrC4>%cU>x5LLwJJ~g$oL8vZ`>IrWu6
>L^wgwD(=WF7#g#%MVCPGieBq`all@*zWwn#)04pPIeN&aIa|jQQ)T?lqYLo+`wbhRV5}wBUohwr26WQaBy^a&6CB&LVnATxVL;t
ZvR;gyYY>Gj9EyfsMy!&sF?_2HH2r7Td)KY!bLeK6mj|uo(9~%7zv*e4oU(A)(etwBXUwa_N)OlP_4T!dJN^3k5PK@N+G3Fe%%D5
&cwGmnJjG(s@HvjF<WZe3)EZerUD;uur^2Nu67L#kl<yz4f=*ViadX7Uup~-
DM3E+mRulNb6Km|m0XEA`oJnOnKT8t52?)$e){<CZiLX$LtJe-gS#RK~0@&W?8n!0#+o}LY>pQ5%?Op3ZTuJ0N%Dor)g(a_~6p#`
Yw4EJ7#gK!+xM}(Q5)DkTcX5v_$Tfv>63789H*D-?;sUvq`*x0fdAC?W%clz6PT;3mx>NvDc1Wl!M>*0^mzZlb5bIg1$!vsly-ai
15-Wxg<FWRr;T~h>t%oyr&*N2Bzk9+{-liYWXPuk{?T7dw=#effuG1xuSh~e8FP117zKZ|)(|7-
?LTpu#pQ_g@+KL*|;~}y(DzdD2K+h59Kg230Yp0Uu@X&c4Cf5mhsb11_x=s*6+T2%wn7h8M<mLN};&HUVOwSJ%hFjO)Ru@}P8Z=Z
@S-6Pe%KpJ<kC5xz{oUU3%15`iVdtkrov?6-;hr4s3IU-Lb}b{|7j^)~0UvmfdCj_BgGtsvkVamajNKDswV^C8B7J`P`G<e|;m2?
P-HCA&2yBEvMP=bC8;RhDCK+rWXadT(obCn~#2F$sl`W{ZPB-(#io|4aQmscW4HN&2<^Me`?)d6^gvgFHIFo8;VV~KnUv-fJ<K%p
ym_Qzd9;5N2XGRRLl{mjyN8%!mmOUvsG7=zd>B$&K&Tf}3qh~EU<%>2a<A#yDB9`u;F39>`xJE~bs-&l%JVj|exKT^`1lG<-
e_KXahnQpyBCiU@Fx83~<;-
<XVE$YVUgsF*C;IgDPGf$m4tw|PuS$0rtCkl6qSby3*vdFv)6tS?dXsFKX(sgitj<v^f5aGW7Tx7rTm)pxNBOb{LEJOdN$x1tSeG
nQS8RQ1*~b!oj@KTF=9yNt1N`=yMpd1VuF#~D`}j??0l*oTZP$tVrh1Y@@`_qC>ei9PNFZ+i<4>NlK<8z4X_=>;%I7j~c)V(z;!C
xgzYV$lS0BdmQiz1Dyrk!1{;?X~T-
p|$d6cfCH_^H>A?#>zFrTiosW6T$*_|KLRZjb;Ouu5GXth8%9|~p*(_Rs84D66ynkP5SddiCe9Fx;!s5~kv{M;x8H1z_jpytpd<5
YtyStujBsSX<*ioV2wwt8qe0qf*QmJ3#neTUfjQn(>HGlMaP6i<i-
eIMCm%FIVAlF2dLSTog%eF9Im(HV?Q{YJ?Kleo&69iz*u6`CdZ2ZXfnmo*Z;8X%jlyU5>m3nH+29rH#LOUU;nTGn$+D?cx*FK9!6
qk0CcF`k4vt67OcJmhzmP$U_WAa}Q!`5xfD^u%Q^8CCU<(K#PbWpKiuDMD%&%?wm_qCOJSx<*;BOY$<=nXRvYfJ8s*wV~H^5uPEQ
pI#e2R@06F;+I8Y#E7wFsqed=-
Hce|b3pes)n0`IftF$u*esb&KPnx;gASr}<|CG6;{0Houmkl~B2&hzI4#)=k3J3mWRAX|=ezGX8tZh0+E7bQh7q;s#N(JD83$;^r
LUNXfd9dpcH6XQgc443J0A3aSbJynXS0jH^VMdap@uwViJEdZ4|DMeb@E7{YD+9Srd90uP;NJ?#k~{v55vPvdApmXWR|reez))<Z
ig~s!oq~NgZ|1_1{EN&4R3PsT@)s?)t}-
#^7M*YCd%vwv4MUN8|aQno=EbfqU)AHa44;=a|m{HIpZ%1C~yBM&6i(Q2@4voFvkGh4?E><Ut^(<ikgr1L^?kNVa^Y`FwPsG`!|e
m-e{TkBP2dDy!kPb+Syzxp_(_Bh{k)oc&q_5!+_a~VlN9Il5MurN4?>h%T9V7kyfldF^|n<XL4Pmn`L>aP=b6Tnzm;d-
Pah^^Jv=sT8GTWf^{Bvt+IGvTX@JqW<zZjETq|-u^yT49?vkda2Ic&dmqDHTh%{m-=VOv+@-
%EQU>_}6d@yVp5oRCX;T#5!*FCQ3j6ggxT$DE8)bQ83~o?8oGF3~W+q`x*!%|Ag>@)5QCKMCu8QQ?DxQFdLn9sDiCNO7_vqlJ&Sp
8vI#i(!s2a#-h)f+x!J!D|QOHKVnS%Qrlx0Yjs`NlSIgzm56+{Nlh%cJ$Ioe#;&G-5Vog0aOx_$ekR?A%|d|32yHTnitzH-ZfAtd
uvQdMKB7h)LzTmfBp+}onVBXk3RH)Yr{tkrMtSPCh*L@zHkMpaT(87yRkS4cvp5+}Q`9#m4;vhhJ@9o#4Fz7oXGne@1~T`FV8njM
qX`fA72W!8<&E@(df(V|z8g>@`(mYGrPuL=2t<Y3wumj+L64pbb<OED%#umo-
w6n}$9SV(>jWseM!+8?HQO7OE#Zno4M1%q&p5n0hIjk8j@hJ}jA7TiHtNJ!+#kRpKSu;vP>c%&@ZWx+3(wtiAi0e6w^H~-
aJyic4xcaLNAyBHt)o)^{MIrL=Ha)Is*JXX)e*I(zG=wH66nHx6pET8joLplVmX|)-|85E@-
(@n!_w1G;B_K6NL&u&|hKzO@lTIb-vA?x#bH0~>l(+cHu(m&)U_<f9dMiI3kN%yD>?ki|Dc1SJgl7|tr6{Gt~br5HXeQec7GKkfO
raqKIO!-2M5c)Hzt&l2!S2D3OBW2OxrH`jmD;)u5f!B31yuD-pqTqhOlohEKs?kJ`Srtu)Tk?E}1bR?k<>hvctOp0Dw)I|<O+I4*
&I0Bj9`{D;T?@RE{hR3;*qY@Q2^+Z6iY3_qowNs<s=Ib?a%eB8wj^O4T5o%BAh0%_R6>%kr9$4UYVtt;v%YF~(e~$r6~zD4IV2h<
mgC^Wj%kTV0!^%#r)f+S$<-
q==v!24?J64foUB6T%OnF*f9pM?YSmEt_~%mmS%~7_?xgtDDaM7>6aN8!L!Rh`3ptarYPQlhz_Qg}tt}>()L{F_0wYCMQ5IU!vKL
+x_<2+%PlK8Pk6(3!wp><i&Afc~=5XIw>yW0S>&T-!jy$@l)|I@2I*+a^Deria@_oBSJ{zlSes9OHH9oTiB<-
8k)*+9Si)N*2klUTA1}l@Q%0V*OL*-D{;dLsguBR60w1av9u0T~U!0A=Jz#gNdG8tRJqW2z+d-
5J6gAwX7e&GvxOepar7g>Dj1rrAj%8fc6ki1*Xtwst~eYwX;!&q4!p*FWja|-C#W1l=MNq>WNFuj$ykRO*j-
2q?Tr5f_8RT;1Ps^K47QnCHQ{*^@apo4;rZS@LH1s2xDRsu=!Q2369Y4uIeeWpF;(T&L8siMXzi*E(<VY#}aPqet-WDpJIrt0dh_
7nH}u)lp>X;J)m0KQ6xr_*U=Qg~q#WiH2xSB&!!Iht`MLPY_Bc530i^)(`O?Tx8!ModC?wb~CXuWn0Xb@)9J(&FZPgGi86LqkMCr
G1|h^IMk{k5d=>3OH*L0pK>&ItQYqV1}EOT1cUj=6y4(vdfa0<BhGgUM$Yg1KL8FbD_U8(ub08p46cCX?<?l7WQ2f_1z*+X1d526
3p?ytw`W%JFcbg*~(A=M*me@L-<?48(l+GtvC%Xi)L5IU{n41XEEbUJUYvTVO37a-
FPgj0K&0od_tk9z4{RNg#0NQ^b5wbHV7IO_gdG6-
gev3*{4KOG182!C>Kp>WWL(y358WvNgzbEaY^On7wcBKt*GA(zSS#V6kH@ald>2{nys(zMD5LZgLzeMF1fMGLf8XHL@lAYTEC?Cn
Rc$t?P5gSR9Udr<%fLde6y5%4feC5fonabg~I5k6ZmOf&k{qnGWNhO)40K}bo(AOhrD@k(01p_*qUf;E*2O{J&M-
&i3ZvpKr^sAIp<n;dCiVpR48=P?pm!FnO1bg`x53hG6*xanGD!EHIO+jXwUTk>Hk?+Hu^v8Wz9mud7IUaoW1=i0Enx6BD|uvrwwg
foX52eU1VHk!y7u-
IG?xM?W)GP3)^Y=ePcSz>me6zZ#8)6V(zWx4t?Og)7+tl^S9gT7A=0+^srR$X?vmV^+yg`_?<K7B!si~suNBI6>(Q~obaZw*`9rv
vVwpfm?bvc<M;SduLiTmD#pGiO0sUAXA~R2ivH+_wPu|?)v)8cdNUoTe6-
?g6?3K)nL~eFV<y+i8XNd0LU%6a<dXqgraYMfc|T{X2n4Q>MZ#*5!(3gJc6N)Kn=YEPwkX%Uz%iKQKts>ZTG@PapHX}rBC^n9Aqn
{e?S%&+db4#S*67+$|5uyvHoqRq=N@{xwIW<BRzTk71uY<UgBw?oyoc>U2lwr41=-Jm%Z@pm5ReU(+3VY`><f*ykkQRU`1^6w-
ppT`nLpLuqJlmO(ad{_6W6#UWVnhMs%71|*%0cvF3@v>%UPS+#NJQTfW(cE>?%-
C8)gpBFf$2TW)9FYGcl%_=dA~Ej@jO#<|~ru6YUaJUs2f8g3($nMQgQ0Yw-
`Lu6ZXy_Vz=}bG@GSuy(dj<aXiHC<uR#bBrw8>3qWKTCjJH;Gjy|snpi)2r+(Qe~3T8j2|By;ML*jDADoQFu{ru!q3IQ;?Q<Eh0Y
7u*%Z1jSicV6;38P52EvYk{YvlWAXxo<DPL*Cf#SWP$$79ze)wm&6IOrP*+YIi5A9moy|BD7(kpA1ACN#PZ=#9S9|oB98PBnuGGR
-6%5ZE}d53+ARO;RNu-
k`;S3XnR37rCxZNRaB>Y}kJDon6{T}RA2CNKWg6YR7_O1*~_Lcd<H7k=`E9v$_WsjC#G3)zI5*rcyc^Zdx?{cn~ZD&N(R08#Q{fn
mp{E!L&t`JMX_|9-
v8$TO9~o#coDLvSpz#|&f9F}BGyKiQ+j7#wXj?N|L#su5?40UgRu2S^~)4UsD17Me{Kpw1i1nm|Bjtcpx+cMFP(hf!Zikj2Gnn$J
;AiD~(CvGR=ycr3qy$9(T?*LXT;zx)p|pO$i=yv0gB7RO?>q+8Hqzhe~_GIw~pOaC`aW%aiog~W}e|C`-
f?xHr18>o#_rZyaR!bqQd9(`&!e9v2o{QP`5tf3J<)X<0@g7J&(kweN(=(<6bt`n+m5TxopI8@z-
AXVp#!d?(+O=;I5sTG~J8z@NGqwi>O9c?^EBb_FGqrX1M)0khKyCL0dbW|;n?#bo8u7|z~1r$`9(sxLH^J&A|BMS?~2vaOBwQ*%t
Dg7qWNP~N?LK~0?^5DKUHmi6aWwu{tV=RerfCYX_A^Xy8RF>X+I?5xQM}m&*6#DWD=s3UoMyC5jwu)6Sex+(9tN-
s1a6DVQcKLp!)M3=1g2lftBkJ%kOxJPUOf}u{adWW*giWHsI!6@}2npjtrmG^mLOw=JT8L`APd~#XC!iioJUXE1MaTb@`BHm*u_;
M;VR4PL8qQn1MNNe#j&g?+H{RQ=arj8)3iPHDa@>}|ry}a86HMHe-
4i;&Zp+R|<p{go!c#oOZnyOG%Y+k(6if4&WkAJsCqV^Ql(r0fX@tQ*P9mo#9rqIW(yJ}re6_k?tUOHT3;4t&P+tNuns5mpPx=__Y
H5uU%G0dQW}6heZ7bkrciWFB`WFT*E1O;|yQO~B;T8UsO1F`&Es_Bebv;K%PLl<Nfj(g#U|wU@j>ayZ^W~Ck*)fh4%2p|GDw-
P>=_5IXKrOohi7ONP)(>d;x+omlUhymBsM6vAuv=rQ+Z1=DnF+DZYCYgh9w8acq<c<JO3-EL?7`V_l>&wiEG3@t5awcDN~mI-kY+
w6`BuFIQ!?M0mwb0-
=dFpJqy5hIka}Ow(|)JqlvkPxFcFrEqD^Bqk&(sji(*OQ5u_xRgHA{NX5X%rn7wM+5PDlpj3^|TD(Xa2{8c<b*_5yAsTD|_W0nbi
f%Gr!STIE}p|^dPK+RrW@Vdn7vUcTikYSO?CUBY!`X7N%IfJY|ns}L%5k<_T?4vXDNVPb&`yxP4S*zDt@cC3C`nGM#h{&qhjLs*G
?0_TRt|j(W47GGO3$DPGDJ-$dwGjc9yx<%?()|KNTK2>IX3@s9!|2)Z2{*sVZ-Eu3fmmh0-
fukDh{V7GBmX(s6==^nFic+Pl<yhmAqJ%pmhCnC>bvCqX@oAOp7^j^A*knG4)*pRH^9;_F?1Aq&|t4;x&=h2{n4reO`CL@E=m2zF
0_dVh>Im{(URmbsm?){ppxeb_MQ71GNYZq`n%#cw6t(~pRPV9Y)x_{G7ly{T+Q-
*wz|!()V7{bG~ww2_n@m)wt)<33%f!u2XiA@Kwp+wGG9RYVwug8NjgJJEgIwFE|v&@;_4>PW>c@k6>gW3xsv2O=Ic@?aS^;RLe52
A$RqS-D3_#*lI!_ms`4ZKim3*+SDtvSCJ-
E}e`o1$0_hRL8@uy1qlt{W(gOBdHDF#gT+9`jTqM2I;avu2OaWbebq{q){ye1lbxOX@isdY4F%u~czxfv>;2}>3vyu(6=)4gs|2L
XER=+)6DM1)ddW?F#fQgzu;jkGtQ0|#MySQv`ps&6G9{(|VR$;o(p`;DdXI@=ruY(QIo{kcc(fn|JaNxd1ViE64ni;@7%DZs&E;)
9of$e=hPk=k)pY46RO)z#n|Ax-9KAdB7@A&G&u{EdDiY$Y$Q1X?j3+B6bxXP(OEegTA7&c5QSch82J4`K|ckwPRG1Yz-
?_x(Kj7~@)5stF@ZDPJ)FPq%g_MWiqCseEMbp%x3u!P&v+|IWNW!}YCIs;6j^RqJ;&dY=<(=98H?5d?x0usXMbA|3aqGz*kd7G)s
Tc>5t24jCrv!vnMB|-~<RcGrS=2>}rvv*|?K=COpdeZqeI-Hlx_CoUbxKDo#y-
V8cr^V_sgSY<Zl7|SlI{t7xc>lf&<VkMJSJOfqAi=^@jul$8Z?V$^<YqD_E^Ps48=|&~>lOMW&&VxUUfgAz(=3G@waa@b+F*%?4k
R9(5bc-
~5|VIQctl`UC~s!81>NEn)FQW*2r12qqUVmTbC^W#IU@Z>ZMWAJ?4&dsd&`t$qL@HDwIra8ll#VI##WMYj{%OI7bwBbg7A#?8<b$
bq2oKqy;JpIZGnQzZ|6KK;(j!=>g_+`PObUQYF;Hv-BACD)#Tu#a6NQ0tp4!dv3kaTWctW?(jC}xa%!r20d{I?2OMc|l+X2tvbK&
R9Xz{_k)dyKS=#T(_b3s$3g+w*VMKBlyU8#zCA_*XE{-
qVm%Y%*;_xt^&GK=ZGw&jyrvoJqL$e>4eshpVMW*rUeswL(*Iu<fc|&a}?=IIlTwOy`xD_VC<qC;N*pTgSbUog`p&mP9PjjKw8d_
tX@Q*zWu6U2OZ-
(T5mk3LCylcq)*Wn*t{~%7!D>5SH)8Huo@cwEYzaNiV1&~3gM1+@!L2)fRj9^K9BfsM@MHPZlvn?7ewf^|8e<UX2kQ_<n*ZKO2`j
F^`c}xsp%xW#6iw^lj_7jPyp!Zk&+3H%j-
qz~Ye_3cQ4u(S|O=4Sjb_mDPHY+Hf`7&R74<Uu05o63;4ni<!#x51*@j1I<^Ma8$R~|!eFS}zPqwwcCxyGN>JXz8G>N;6*%5Ac;4
jxFM2CG%gt*8Gxu)--zYP{Y!s3+)u{emJg<rUR_`R%OchbtRqy60H7p6o~#Z=e8enlDLCBFM+)ddu#0yzO4UdJ048-
ql&n)FZJeWTcFNBJ3CRQPQKawDT8f$m*hhX-n}|7X$D9*t<WWd%Iq`5THnasAvatoI;RKSS`s;FB7U$=lx-hqjL{KwaU?6!msHBs
u)i$5JY)_;Y^?g+&qV^e*<@Im!_)>iDV>A#V3^t`CBr@JKCoty&Lx5vMY0-
obVCi^(ibwr&n=ds!la6+SQ>*7(4x`zIHlDwPo;CcXn*7X(w(AT9`tciwgX(?1>k7y<-
ENdVSaK^?I63oC7JeT(6)ZulDvz{iQZnN3N!1TwAqMrlq=?=(LGSJ48Q?%gtIgytfGOo}q}8oGfkQ3H(}eyjuAk7j|)c`1w*v@p{
{#N$m_wdO3WRKHv-*Iso?LRmnoE)?GTE!iXtsm2PsOy8(5Q3QVmZ-
}*hm$5gsZp#bv?Pg%8B^7*99<k^CLnH=(Y3rBIWR)@slxXsmq+0bKgUGfZ7E6P_$KzUK}3{|<MTc}>5ty$4nw}+GFU2>`dSaeVHv
rcE(qsCv|tbo5lb5!!>&+;3j=#84ddsmjmvM2Bf=EYc{>G=O=sTVM<ej2qtdB!>~7i)Amo6cY{A+VgP?J=#8;PR=^Fl#Fpk>|%hp
PgbSu~>)WKO|VBte~l+>FQH_H0Yr@v;jo~P`aB46w&o(0;Ke8AzW=b>*pvN=`N(=36BI7N$2UT{7sRz^sP{k(scC|c|q0PqY)~UL
fvDD)glhg0{cswr^6LLWKKnUGOs>j`I6!XsNF`GM-
6m_(t3!4w<%OilVwjRPO6q7His%%s7~jh5TAC<9d^&u+DUe6Oda8f{BWKq>EE)K!3<uhM?>5%Z^)2Kw~2+K6qrI<)E#j5+Co#4>6
O+A7amWx?4iU1{x7k1PR=YyXng<GOh>{dcwsVh=i7=rm`uJZ4_drkrYN_?&PE0qmD<!3Sb23ryei+|^rg9py!&2ac!EszBp8hBMh
NOKX{4~F6wA#DC(9%hKP}I+1t4GaVR%TAZRma3rTVf$Q8u_ulH2XUK~chQsbg}^3zS+?lyr>Zsg5hu^6*30B0GQk`KKR$#k|{T1p
B4j7cjNb=+zvgnz-
T0^7pa){PO*ezx?#G6LOQ|p{#O@cS))UyE<*yek0k#Z0w5mO_dCWccX8BeYA2w?^IE3j(3OWKODXHl@KIufbrD6chq>qdSDI?>@q
B3UOJw#_}D-
DFyy4u%fwx^GBE1Bt}B<M<bwls&W)3SKBSSp(`PdhNTurTfj*i0<`fY@qdp1LbYt8nny~0cEs@M2{u84NVz8RUP3x~e{rY#&vaAS
>c~6H0$Kz3X@u$TZ+SzPSX0Od=23Q7#V1gC3kaUXmfe)Y<isuw-
$Af4Xnm!Fs84c4uTNQWtk|mcZdXi)CH>(8D$1P?eGRbe1Vvm9rtnXBIDqIk~hEUeIis8y_nrFA<wux|GS{Kc}VEIcrI(nwx5ZJ1s
M$tBTRHQfAy5v4#ziaF~u7w+fcv=&?RB<#ZW<oTbtID2(1K8?rlVs5E;iWmqNpF&zda2~Jz|nK!b7!0IW-xCR_*UaAcOF|iO;jmN
MVn>UvSte#_j^qI1dc!wlZQ{h_hz4h4Ov84m=S94<g9CJEG6t^1V8~=>N>VkOPz|Fc=*%X;HS6tr&Tx)5w&BQW<&TOU5hnhf5@^5
YQv8|UH$UYzyHUtKm7FLP!prA)n-l`tEI9<frj2Qi^UAM>jTU>KA>D}eu-HTmXZ7lgbU2R1LzibS)W#t(`n6D-
{pl~CpDj9H&~Td<;zBmwRJa4t>f~;)dlFy5RgF&>NR2M5Sp$A-
z9!!3VEs*pHdz4X!tEy6}}b@D`2}|zTI{UWB|H_?`N55#hnW*;@q@?jBexl)e^1}qd1ufNZNxQu?(;gt`&a}9a26u9c5Vyk_97B`
jk`%A3esP*+{`5rSIk;Dk%1Oml!%|C{r9Jv;T>Y5lt}Gq5$OKveJ!`Qv$Q?KfnJjfG_mmy$s(|Rnc5F&f_-
z7xu6GPQbT<;0U7_O`W0deALHBT8n3jFm6Vi<DWA8?m4c&<O@rojLD<K9%ICne((coR;OQR-
%O5QUTU5o*3H&phbN1$k6%yjq6uLwdr?9@Go89T!SoZZaun+UiFx6u2B4)fH>&?}&^zn*OqS+QWJbWcFJu2Cm3jHvw8I39B9!$ZR
wXc%M6GJYC%nxT_ZeCWqrr;~882btO(Y54v_km-
8Hf4=AvjfRryj<M<GQwP;8a5wHblM67hPNmt?OB82YWXSbhk%u{~hir6KIntR^x3xesd-XGuTc({`u@fL_!0zHwUu;dcAFZhtt>2
6%X!OD%Km6-!FHu|Gp0Q@ASKU)_Mn$xYerPS1o8+2CqKv@%?L^__BMfIRslQ@+RT&7c6{F3WMy6b)-
~?EZFRUR@diR(^<jq?RP3vr5i}U+JXt5k`Cd5uh=^zaCGCoe~xhc801xC*FY`Ua=pS66x{<=WE6+rbWWP>twuCwY-
1~SxrnP$#F#Kl8QxPnFY-
$<ikN1j<ha4yKtQSZ;uyhX*+)kllCCL_F1eBEDQw6YfmP|jNpuwQy3EK`Y$CO==A;b>axU`eC2;U)){omX7L-
pJMtyCWQ?*kCMZYec)DQ$+{W-pi)|-
jti9cf{3zHQw;Y!#m5pBzLxv{r0Y{67{f55F>!U`A<6asJ##}AOa*l()DXVTo~G85ZNcD7H(mU2LZ-
4z*~;57wk9>FjB0kBkq*A$=`9*G-(26GM@-
RuDH8Go;R0Zhb$^+Y^aqc4CxeauskLmF)h7`^NvG@8Zh7C3s@D@`&@ar$};dwggz(L<b1!3ykSCgM$G*b`uV7!MI<ibg>>rc7tv4
cL~YLc-C*z!2E7RxMj~nPps}KoQ`+pqK{>Y8;kg+NV#YOy*)`PTN7X)gL=Ps7iJ{j#Y99Ia;jYT?2WZ?)cj0VFOv%%y(U8bR-
6))yIKv_}r+xD~Hj6YWawH_VB`DDS^B>+~d@$V*+eDBkxpw3ySCq4)Jb<X7G>o6Z=^AUmskmgC0-O{-
w`U3RPgF?2>wX>4@7Um5MTQ_n{WR^5E1;UtNOYos8kj3y+9l6RR|bF5$FEwq7AH-UT>HAG8ly-
)1Bm(yn%5h5^jQosLpjVKj;P0_pQ2dcK)!$&EK%7=^Y6fPk`a6=D}u{@0vJsil3lIIf++%&Sq*UG}xA$fJ&I_haZvSl%i+EXCru-
mHr@k8*@KZ+>+kuLi706PMwaPY+oNW4lut@~w9FU=?EP2cWcLDb4YSRjJbnz2%H2k}<OxX}dWvwsLvM;UqH+*agZ%c}NA?FfS@TO
Y3_j4oJp!#}(A1s0lFWCP}2hs(V|B>e~y&^(FU<VplEwqr>@Hv%>8_p^wScTVnGMZu=)0id*bYrRC&Gl08i-@-
3F2M#}ex6^WhU$l)MyKf?1*n>qXAXx*nJ(1++#12q^p&*Ro)vsSu^Ifr`8iy;56qAzbS7~UN)>Dj7_(?I-
MViZ~Qhzt*@YT}49#QE<%#p~vC=v)kp_48|61|gO&eN$QdvktT<xFyJnH_GzHt)l)={Hh?)eGWxG<@iz`I!HIlK_mXt#!Azt!6X9
%*>TuG=R4y9kduM=Q<}m25Dv`Lpm^XOQ9LQ)cPM8*y!MrI$`E#8p^9nv6;8C`qctQfJR%ppJ;xugyrY4~vfKz$6MM?sSg&(x;TiA
}wpPOJwkGfq>6l#bkw#4U7*h_`YH&zz!wxB1jL{&{gn+>lapnkxldHjH2u{RM4%GARt%g@+^gMOEPXF(tme)1kp{(3T<7wC8)xpK
_3Zs$e4)oemBY1q6>oJ6DSWU0-
#Cq5{b{?NxS{+!+ERV>5)UeBrDXg(<RVJ^gW6w&#elXa<M_iRT2}UOotXz)xhHmj*UgQ<*9cOrmB&Co%DY*loBBRJmVVt3qD7&gg
8Mup%%u@BL!>nb|Eu-gYM!u=S%V&wqP2K8cygw7`CO>N<sLQiE7E=MeN7NTm;pHkN)t6HFcG%D>3}_|tR25;v8hOnhv{2MOtJ`gs
k!N;zN;7nUd;4WVpY}9xmd^p?dqil%&r$3sS7lRu2|V|^tGi&d3K!@C^tznr1NrEaf%?RT@HLF0qss8=+j?TgiB9yqARTAceu8E3
G#d2a|M79p%=r4zcGV?M4R2Nuk?^*`=hSz(t8mfHz?NbHW4gjcdb%3?uc5mFs=+r*`@q+3St@e7S;z*0`i64s&zic;Cb634YMRuS
W#{3ou@CkdOL18>HkOo>S0rb?N-J|wgPO}dI=}-6Ja6OjwP`Oiz$}OIS*}-2m}?tot-7j~-
hsS!1EtMLZB<BnC2TkE`DnR*GfQuaYVGaL+ozszaIdM;RE^pSNSUsqkAt&*^wPG-iF6%f`mv<jQC0B?(+(fs%VQ%3#*e;{RAsBmC
44wT{OKQ17tC=7Im#{ukh%rtjV)_h6w8HTs!tY(Xj=h+i-CJ@kW0g$g#QFHu@)onT>lHi>oePBiK&DTBr!k$?Bjk-aS<Tr8qkH5-
@4D|+k{?((?Zz$_&(ik6MPcGf2Da$x*B|1;#1F;h#xm^c}Cww&s2mqU8ii9N{}$5qBKAU@9c+Iy8R_$2-uQjNVTv(w4S+gxQ%M-
Q$AhaCF+(6>tc1#<n)e@3;SV-4@-
OqBbz4l_2FFr6GeW0IG_#>4`T{+^c%uolEepNKxbOMfgXNPX5?o^f@nBY7V|E~V`B613>GtT4l$!IDbNlt-S_`9|0^1Oqfi}<GkS
i0zmWN|Sdp(SIK&LN#G>^%BR4{KnTb}*2G1=ly;swhP$$z{Jkx~cJJiX{%4hkEuV#ze<F>|_@W_przaF=bV<!{*tH3^R97YKieQ?
0n?+*w4WBzXkc1cVk$F0Tn^=6sFMj`rO%qR>PsGHF$hYPJyW>YSlF3};)la7AGbD8E@6x4`uv+FfOPzl}ZY;h=aQ0W%+Q?@;%@|F
Feu4*au+FnLnpk*pX(_D>N8O5|F)crOJh0RpEP!-A)a@d7LpnMTMn>b@!5ryTD-
HQD)%1tHCSp6sN_onOust7y(&zt$P>L(O%#uTqeJHkh4yUlakGY38$qm1`4Uu=r_db3((%EqJlOz><9Rq-mTD;!M*Es&uh^`%rOU
E4}PmeQu{k+Vd?8@k%A)3xl^C>)@RmzVNgVhV?iP2K3(#}(nDMH(8OnZX(z9C$@13D666uUmDKqlShd!Dr+{HC<Qd+TLZ^X`FqMQ
2x<#hvQRm2?2$qwAES^W|G8xe+9T1$dD~z#FX*w0pg6$B-g;kBx3iG9st!Sm2k1V>|qT?kPlr%ozAk_H7NJlok^0YT-;~e_w-
P@DX2BY=Nx&${+O2xP4Nr;#wDr*%T8dIW7L$%ia}^1NUa#Js`|!dtZNrTzOVHvS^P6S@suvxs{4o5g0pCrO5hY(t_{4d(0H49(e|
a8+Wc`_8dJ!Hwjp&;+i#)_Wh+GdA<)V!sjRgVR#v5Xb(7zgA~%cq>e8OnY*kD~(M?9@eEfN(I|6cNSji2O@-
hsh1v(mrpV50<hHycw-Fv7M-dysO050!>ZY$zPlsnnk*P91-
2V@xxl+SnH|Mfrq&CvC06uX|apoKuJ{g!b=@qFImz~KbzibbbHjutJF=noQ|M!9K+J#vN}%b3y?s`)a7BPH}{fYa#J&MYMo4$rNy
q@2!kb+gnQ+Ol*x@apsnh-5G;6wBc6DCtKAsQ6Evv_bQPZPK=6z}@9GI^w36_5+yM-DN3{f{^%J_zdH{b4zYLy~Kd2f!AAGGy`;#
Fr2)O-
n8xJ+h{z<x__|}%WhseiO99+r|>~+ZptVcCxgEF3ylW;RhGCS*&u^hU%T|%>z6HgUoHJR*L_8={y%^PY5lmlKQI4OkblzxA;%c&>
3%a?=gV1nluoCu#WG{PVSy@P;@th5=8O4+gAve7D0;w}%^$NBY2)*vg?yK8C`W0)B#D3M<o1Zc8{nKaAxY91ki5dcxVx2*{q6v{+
Z%u0X}8;We=vwXF3U5R&0YHP=r}k^U%L0zOL<VwUls7+x7v#gowjm=$Z_o;A=P*}99~Ifh2=Blz@O4L`0{I1T|JD?dU@f9g?%Ic-
ACysLNFbXlw5bAU9t{7$^_pKC19DngI7ZbEt24WL*I;tQw@g5a~_GH2Uil!+u-5}(J1(BtY09{i;-
8TdW~?Dc1CEIV_EKd5^UcbYuO9<Hrh))1f6@gSmCnGU@|VVs(v<`qrt?qD)#$g_3X08WtqaKiInn=?6l>vl53LCP^W4=ZKLeo=+$
alR42igYN8n5<YHH_N%O&DP`HJ<M>f6SJ7r=J$Q#w%+TX~sH%CaU>r}))NvrFQYJ&K5_FRvbqe0JGel{RMyLICh!2caN$KkC_v0m
JB^MzfkJ=19|DW{ug2PJkW#_2q1%#c55xjDb+r|-
~)@KwQQSWm5;zfoGuKc<SFd9LLgVc*5$jJ8b3<$@hn2K}n+tzyu#&d)WZXmDq{GGBr%1-
Jga5Eg0b`}~^has@+2_iVP%PGLx`5*xo_6f699dwEgo+trTiv8rwzRHx9!p)wEb9Zl<mAO>m5@B;o7Xu)K{iTsO+LvA$AVoNqtU1
<mts8EOqH>e~|LT^rbnwmr!@mbS_m~U*|mDb(Fd4^fMMq?VYfH6Rw19KZ1!GfuhK=8IcCZ4c1#_fO-
LAIP9(YOZ(kXMovTf8XYqQvNGFmw^;Xz&tFK7KYhJd0<B(Wn|m5)Lrd$y{McejRcN9?0eyzU(ZZeef#$l7}WqnWz*Ey|Jh-HQ=QZ
<h?c>MMRgpVPOhG=Pgb$#`KpkhEWYJ+JD4K-
MwV$Q91vBRYD!YN0<LTb_+kLGF8>}QokhXW_wyY+o{j1^IRslzp<wdoXVP2hzt@OkZxzJo3+QBQ%cm|dm#LSC(OQkY@nB_^F~4?J
JZHbn*`pQ(0jJxIi5tsFBxGG@dP)ya*q6zL$98(Oy_o$F7G&M9)W*1z$(YE_;6%WL0V<xhB+e*V6T4EltcIm+++v6ir@s#nY=@+>
UJ<f{RaKXce&n9!_H6}NXF4HMc2#K0^K*G%!?$`N(0Gaul1t!UH<8Fv0kO*C<db$<~J<s+RT|N$)+t0gn_mTV*56mQ^e*eIbvz(4
ySJ6{@|HdG$5Xe9#`zja&%_*4lG^MyN<=F5FCZWuCd29`65XMFqwRB8eW3V#=``C7{0zXqG*^4CEJ!J7)?{-1@5J^s{v<kk-
~Sm4`Ius2WYqCahjNXLhqYyp~UxAK>_!1k%GS!0Vi_w>;pd2mgyh+y)$Bg>-)$y{{2#<k)4OfuKXo{G{UL>y;g|l)2vG-
2ZWA`$+b0N<6AeY#eHjr;$st8lRN|5r^^<4AXqm_ewi6z40ni;)Z6l-pP*&GJX*9`Nb-Ug2oI+Vriwh*y?gST=rUj)Un5Mt=_Kd!
QG84nqyEba)?+k)6sX1sZ(d$htZ$FwWUdq6yLwa{-
DCXN5m(^IHPFL5_3$pfvmbi+_e2XxCRuuaMSc%N{zD0vSAzek1R;VY#I%(kM!c|n&~4@pjNT@0NR+CdYZJ094S?YVw_^kXm-PR3G
>1$4kE|JVlnB~q6uYcLML1H$gM+r}FnEJH0^UN$4SpgVs5<#ha<dZJr~;-
$2W}42{)gN7f}B}!_}Y{H_N0#vwB{+AS|eVH;>jp8?Oy9!cDH3#fSns28L$|ZuvicSHY2B8(C8BTjN`b$f>E)s3RpBS%}IjRLT(V
jDA$%~R@*?J1r5u<T_=R|$a4q(27k?Z6x$d{!}YU;pPaX*XV2%)-9g-
cjnALuryq9*{5yDkC5<=521N`7#=DPVj3Fv_zJ5X0lvOa4%w!72j<_laSPN+}KR_|0-
vMg!42Epv^XiHD(`yP6^VW%3Qw&iWL35!c9;aJ>IB=F}zy6JbLOr)yJc%aVAD@7`2eaz9jWlfkkQL&T4R!1!y6lR=NI!`x`$i`TR
IL-NS;wnbCs413qJivY3o@1<u%m`{d)vM4^iDA(OXovBIvO1JqeJ*Fc3vvOF9qWMk#x_Fi3;T76_?xQpD3#AszzreuXtMh%9N58o
4!e^uQ*)?17xp)-8(>{w}8O-
=y#C}IK$OHAk~J<IZPe(KW@8&v+%CLge>eFxIr{^hjRIVqeQ)rS=(WRX=EU)$al}~wlHbj<h*&cqjlR@+E+sRzFO;aBkYzrO?EAJ
6=tlGy>SKPJ<wsMfc=cmTNAzAsjzq-
;bH{8XTUy=Ff?9`5u{{S!J>Vgsw7wy5g%|RYQB$l39qr*0gYhaR|xu?s9w7kGaAyMyhR|OJh^ThWs1?QIcALS5^>t#rE_w)*-
c7L6%aW@Rqt(uNN-ZuIT1}eupZwRx<z1z9Ol((N7yf(wM6O#Yjpu*8Wjw#>8A;7Z#N7^hC^PWJ0E;+r(LXC`3?lY{#3D3OXjHP5p
q?QI@>9!UY&jHkF;;~!GkUK>ufIjl?MFnOw&JM=uMP_iXX(UV;)>TM>vAIV;fvIZ2Nk<+`WorV6X2M`E_7q7qjVAXml~1k$A0n_7
r>t)3+6+*H@hYPz&tE;;wp5^K8)7=LIvH+Vg?i!1KXed-
K7I6QbJ67#u9x&XJqUIhyOb8RmAqZcBLo_`=i28Dz_^$}VVv4Q|_8{a7u=KAha1d~ZvHeBUgWLlGJOP5es$@0%0KDC@E*H3N#Eb9
$}sNfe6026At<$|oE2N1dW+AxuZYAg&jfTLxzv`z?ixrpmP>vR*^BT1e;D<q<O3lN=?6+PjGMZVmtbo~;)3;wBom6)b*Y=$zP7$`
lQlPK^Z8il$XryOCm0F+T}g;>HTmhv(<4EX>{GVx1{G7#u<KQPO{TnUCN1|MKDG1*<{{p1_IXI#*Ix0e#Y{B)9{*2k6Vic;JHS<V
dRp=t5P;tr<)yN4(GlZkDnuJgGyxUTjb<_{CsH>`w9@5tsTJQM)3jqOPSGXn(yoNYP6Y>7bU*C3P{c&ym7^UTr(uLs7r3%1(Kxbg
u5Z${mTC%5C@%-hYiDkC^6H?-bqZ06Dx2V9s^09v_;y>}@&5dgnS$E5OZVuT)QiY--A4j+EsbDG54KU<O(00MnBE8KxyN)^rsLUM
+dye7Jy5F5e|EkoqpkwL6M}5;`69f+|zO+KJ&=<a?yE_TGS1O%NR@M4QULmTdE3yu?YVuN2p!?uFT?C-
mIncrN3r#+_20;RL0njcL*!=F#Oa2ypb8QBM<|u*$A(k3*hG=$#^n(;;JHb)(~0nqA0SB=mZ9?U)Z=Ug(ukzXyAaN`<?(VH>S;3U
kx&KfFP~w8~gL&<VRiO{5jfi{&gur&TDP(+W;fU#uyDD~WI?xJJpu5Y9eScH}pu!ooAfTHEjVDT1vEt-
=dJ;+VAk0{yauJ8=BD#1EQu*2yQfdyw}!`BZ6`JqPmGjqQhM^r7GD4zli_Bi(x_z@B8Y#S?=3Z?YBI65m4`5us`3j?{0H6q`x=Ax
@+CkuE3kL!`Wlu$2;{Z8p7ayg>jDZXJ^L2x*U6JgkhiD9HU^V1rK8wKuCc7#0EQZdNJVc|Sn0V>ADbsTHK~r17P|wMz_kA4?bwWg
7aFp~mReWGspqUXqFBLN_|Z|2qQ`REvhD;Vo;MS1=WJYR_I)pf8|5wh<#ABoR6TI%Mlo)pQ(ln5mGghM*J=0;PzbR_T&4BHG0{M;
LY0S<H$mJ89{(6NSA{)08Nq<l@r7J-
Zwa;!G&{2qzLHk)vCa7?IDJZ<PF8KE|Be4+L0{%aY99hfUjDFhrTN@b*O^26I9y{b57#7*z$iNC#2w?z04u!R4VhULdz;T3GUywN
I1Lau{Ak0;#fKH`dok^y!=NapfabAK1Y8s)9$CO+qzRXO`$Ibitsyf-
_0v_f23^U#y|>MEFFwb9cltd|M}Lrq653p%uJqalc&5GbDMvD2EZb6|LvwRi5HC?w`;cLL6}&CsIjg!Ko--1+Zh+>nji2==rj8(p
~5fH2VJlP)h>@6aWAK2mk;8AzZDhYDa_|007Hm000*N0044tVsCU`aBN|2Ze?;WZeet3c4aPbd9^)jbK6Fe-
}Ni7+PV@TffOm*`ydU~$+qmMPLWcwa=9%P3k4EG5@!&=0-z-3?*9ArV+NQ3z=xBKH;IXP^mO<1^lKh_e0+Qtl+m-
W79w4VGT&H15?Y0b#jF&emBm4lh}?>jQsnbsCdO8hT1B*4$3dCqRwy!&gd&-
3`p3t|N6{)v^U|7?n@ki|P{1!$whGFn`kfX>^E_Qy8I;A*)RM(NX-$&#D%&80Bs)6#;rij)!&=+-=TR(t-
|6R~NaJT=JN+!kMN%@v{&XG1p#li7^B~{&IL^`S&4-
7phktl_q3s;Or#CD6A3xpR+C`b$vYF!?eY&~&a(Dmr@#f=K?{g3r;^>2yWPK(My7iMVONlXdh-
Fy0hVQ?*^Eg-(-pNP*<Ne*^&6l6=fBt#`@b>ne$7xXh<?WXK*a--Wqghn?vvidOMd3e3LD3KL1%dURfRdRXrt4`e+(UkUn(P1zEG
EpT1E6|zyO#p^JPiMNE`*;48-
FI^xJ3bgam^ox(1eXY!T+j{TlaRdbJ!MzMq`kD;lGGzu`F8{KBrOWXWQ_Z>;SZT%qc7<KNmrfCR>JVPMO|gH|#>-
1V4*BScomaKfwPw`P%w1*#Ubu8c|YYpk1Og@mFcM4U;Y7<mtK;`>4>Qp_7=~1EKGxgBxcld~N(<mgc_^olYDKum&F7dq<zyGL2@3
;_}<jl0$q4f5IqOBxzAbvz<$c&}j-ds`Q;2qLSLJdgH?`!rR31kOp~-
rP=JCYmslhPcs~*m30)GDvSr+&HC9=%)Wh>G)~4H0Co?8y0m+Y2F6Pfgi*5C(P695iFuTW6_|IwD1&mnMRpp<w|gC;7mKY*{*(Km
F+$yAk2Xg+^2=x?w!&_iieES1)u9kUK3hubvR@Wkgy_9@6Z6k?5J&T97NF|>zImLbL0$-&jYH|8(aOzt^@+e0eHGzvFe^*3&Lh-L
zoG9e1s56Y*|S3TKr!v=w|fTC3?zfm+`GaLS2qo`T7&FaJTOr?_*Cnp(~CnFY`nEY3v@KMmO&Ae;D|#f_qgf>;e*JdAf|rD&gjU3
KL%W*MHea^X0FpC9`69Vn-
^^B{*YObpm&eI09;?dx&1A8G{$RyI&9n={{BO#HwM`24XM_JSfCN8G1`xG_pl1kzS)N&pJJci#d;miK8-9ekcAa<u(`<65-
34=ULB^Tj1GHQAEz%O_owL^Vv^thbF_mE4NzS}H?+lxhIC#6#xv+#o^&Y-
y}^NO&^qi>TQwrVx?HAt^sDeiF$*%$iA=N6KF95l^|OOF%S{1BJocwSIa_vf$7r(Ok33AXc01;i_VDF8PYxe?&)|m)m`)3kKZE0N
@St5#c=A9}iXwuMewXk!VYMXgC=_!m37-
9zG>=1@bVlLMV5#Q9Ez)&96W(2#2sypS$HzAbM75<Qk{Nh7;k(t!!fy!U3QN2OvohX{tqjINPFU~_{+9(VN*N{VbX{103MiQ)f_5
#PGwUTPmsYtH);!H$f;?pC;OkpaLFFs4nu@$wMj60P(|O-|TtXH6lcc2;5HzTG1&^cL^roQ5LaRuEY-
9yih)zf{E3I4*UP&1ME39-
r@5Lb3fVo9vTi`ow3ad!_id6a=n1dN9SOa+;Y!rv;EE}Z|m%wGaH*_x?>H`t0XG6yxGtxPIGhm={Km?cr92***zw0}v@3@-
D^l(VemTk|W@0o<7fIo4=i6`5rZ>onC%M+*AgY(DJhkxXxY9w)qPEyqv$HJCzY!g!4R71%PUZcVrIult%J#&Fe&!Qn{>K8cmJQzT
9j=J0(2y(&&#6vO8p=br_TeN9c&$f`y;-
Dz3>tqorPN3NPPzgYMyAbi*1=`cxMgLj2dGLZkCjRX&@+fq3aIk}95sSjjBAEW76q#EuE?y%phH})NKa(_Hp{|c1IN}n<*R%V7d*
|Z6Z~Jb4@HfX*4gxX@mJFwfoqEm)H?NbQjJ8S{J7XfCyD8IFHLajaYP&lrR<yfK(^r_C?oKtkwk(vl&KMcl-JMvxwPS?<%qeOj;y
^zMqm}1f(6$4YP!|wcq?$jPl~03-
=jq?W2{nmD!hCfu2hGtWKaBBISq;=I(ZI@z4!|f<R+L!wh5PoMLmhChqaPJ>6jNa<VRY1(^rZm;<!KQE2@FjSNvkuGLttWEL8FEa
t!xG;mpPR`jogdT<Wl_}T}*1O<6`MAp+_uCN0^cncFjAt{&{|WVXQBm6UK*Z7?d@&)ndeUM3mz)ZCF6`@tQ<^icEA`5~878RsGqz
US&~^?ujLrpH*UjR-pO{fP-jv#48z(#SqL~jN4m)Oi!*-4b%cc(GYDCTB@K}=OXN>fX~8A0E{8Z!U{_GD$0x1%C*uY-dIynz6g<M
pwbNW30SW+?UNN_YnkFYvYsO_;Srfj65|}O=!N1L?N<pJuORyc{{foHz6KGo3c@hLbPJ%9N5wY_H5HhL3yXDBugrsIKynSD0VZ1H
)_ffUdL^cFtktS-&VEx}P17{C8-
?{Gpgrp7rG#=vxM*t==U(*TpVrip3{Ydo^WG{!uB2R6wZ+`9=+x%Y8?>Rfjg5iR=_<%<l2@tPHQA%(fcouMk*9_I7iZjFEu5h<Kv
H>QwK1p==GFc+%53|jN+wL7mk>{#xF^#z+|*yVaD1Y!-~tpSKY-~6hg=rPNQ81-Tq2zbG-xC&#fjtA1TtcdF|U=u>vlEp52ZJ|)q
K@k*Y#tT$nGAxMeVRi+RSlQRhj_qroJ?V-
;x*V)*&ube(4SSor87fbd71H66p~bEYzHc?M&2!U26{#|E5KOUa!${Xjo|syED2p$Q$ZQK}Vq>$&`il{^$Rir6Tk2KV$l=y$6pJe
Sel_8{6SZ@*40zveblBckOfH<0S{Cpug``y<0WBDR02x#nH(0=&`#_y<;7iK(GW1WDP0@R8JkT6B@Wv&FzzZ^wxCKHeD1r`q$KPw
&`GrP``zd%v9|_igf<2EZ4!v?ZK{|9Yl~)Rg9JlTNY^$yPM#}J$2=du>S&bU+7`0Tx&-
C_SFV0u>$OE0`i*7gl%+mk#uL22N7WR2!Y0Ro~OBeVnSc7!NN`j@oCqxYV2YgTtkn>fV#!C!KL#j@4~fC)ajcZc^B+?%z^~T;Vqh
zCIDS`Je^9Oo+48xy8f_%!^kn@mIZ#M>6${NhT7ImyB2X8P{$e5!u1AyoB(||5(D-MaUeZ7%^=VQ3dAZaH@5v|0Dm9`H|z;&-
r5WgIYKHl!|bZ_CN*RkSCVzETfS|W@NFB@Ei$zeK#r@!u16#d;kC1}<|>lYLyY1{a+wL_3-
eEhYz;ZuMN3?081Qsl`y}D>S^|al#H9+I0hs0R7~<22Y9~#N3)95z7q)dqvJd}-
S%chvdd$}pItIxGU9K>i&s7G65(fk4Pk@ZdAxyfAh358%*9;gTLgnv}+Y288!vE@nvVQ7=x5MB^h(3KRLO!q~9UIKTF`rDgMw_qE
_LY0k^bfbpw@KvFi2)3}v58JMy8b2LUVs5EXX}!kwM6)sg8@e{SwI0S^E9DsYcBp9SPVs|GyEb7g^y81UZQ&+3Dd1J&~rKv{615-
l|Tsw2-
Bf%=SOqz9XjsboIezej*Jn&Zkw~tmc}3rtseZksddFgLs!)M@rHO{@it`p>nubgpNJQqLO~C83HEpzOdRS;<sdE|Ye%XZPP%4MlH
ajc++=J=lkKeKJ)`YWj=r;G5Y9zA=85XfWsv333Qt^e_#!_vouG*XHPODcR@Iu}Mh3a7#@b*c8~o%$2sO=rcbfb0^&y2u41g88CP
#H#2$OQcRHLC%YpCi`8EAW4s}&Rkvzf?BYCDev1ez*GUlkNtFe4^la}NJ0Y{3GZ2NLN94`kxLX2M@v6R#b+Ft{LVwzEMtFgXCz1d
(frCxrPp0aCI8!?9Yg{O2f*19qX18sjjEG2>G}K?`(=-
WD{YmdqoA%|)7tr0_wh0=q+lONyu+OHbbbZLT@J?l3*Zo&e4wbz=blG#h};&Cf1pLUTbNs!m*=^>aZW<{P$h%4Qqxmo)#>&3UhT0
?0W`gGx<bpFrfsCK%~9P)#$)ki)WelTe*17HHr|K|zhEj1T+_)xayb7quxrK0baBs}#H+kSLN2_Lr-TEZmO*nAQw<wZLsC8bP{+=
>8VLT$CHRjfH=%a3c&z%%c#quo#r5s|fRL0hB!l@ws$_tul>8j!{J+n*iXKH2*fPU}55MozU0<;IN-
2PgI{5FG6JJS~3t|M<E@euOf7aZL|#F6(=37@+C!rq68y?5c}ZSdcYJb;uI61a^DCRKrmaP;;apHkaL<*gnjVJWi;W^CR>JN!T4a
PNQj6!i<Sxg>g#-OPJ4I$|ByX4O$4Rfy?-+@<;IYJsuPM1=Lqb~G5tHr*RhkT{L`_vnU!O5;ld7|M(%Lp;$H(TF-
{^0niRE;zY2<PE%1R?jXUP)CHkj@T{*{rb4H<67CHZBU!1DnJ@vzYSEHs}ZH9VDt#gpKASvwr@SWpg?WE$Db2;Rwil{=CQ}+fjd0
LWI-9<+;aF%gf6<7xzV}}bem~>&!i~D=|&@a|2+cEKFqi3ga=z=JAO)U<gMt~-2j+^$KR+?w?1iv4O-
rt@7m9wtCBrtB$<2r{cn=C<&BR-cN1!-~U&|*o;I=M&#R>h<?$t)3)s4bOXo2e=}Ks8S^O7(>!odHIF-
PyUVqt%#~?J8s~R_zDNbV!Rs2jL-pAuqN9;`F1USWmaKQdsODT~_F5@_}+C0!)Ti?~wi3y0VHD@EkYdK?}!t3IGs5kqs<ZyR&bo%
TSMVPR8YV5`YotrU-
0ApaE0X0~1SSzNQ|1>th<nK~~g<8A$0C*a$)~E6^>)Sc6s_nh^3rOyL5Fz>@+urBG4hhpzCa0#t0RUcZ55uso~s$u!l-
Ijb?YczPPRz2Rg6Epg4)Avfs!Z&3JT4_vKh590Ew&C3#EXl`k)u6dLfC4?kkW<0Kk8OiMST7VWkYHYo?E=KZPh&in?e+YnNDuLRM
u<O~-
R3R|kz+X>wc>=n6>i58G3M4JcCA6*dO^oJw(Z`&;O@N%PrHh^3Tez@IiWS36?3zri5bYu~7ctxRihym%2XP8g=2boZo$x(p^cqnp
%QcAsYADr(6xAv9gAlQ&6%SQ07_%;rQ<e*|#zC2+2})P3s1Y^ieXm>fF*bd0t!>A8?^*Bq(l_GG3<T|Lkpgh-PzH#-
XPBy?)`;ZUP^new&z1}ukTqN2U~=G=KQ4h{69znV!3DEPEQc;tH@eUqQ$^?GvaE|&@ZUq*W}p&|Yt!GfDs)6D?@Lb77Wd}~N1hnI
j%G8kk~6c?>Zy}k3V6f(8Lp7zz|fUUqco1MW|k-
(8674~M)GI?ry5UFWS?2@f$jufFy2&7ky3={Yma%OI|439$5`%P<2AIx)M;MBt765pi!qHYNQL7rZ?nm63HdPYwm9s42WQ7B0_tG
pRof*;azG(Oi-
h=V35Zohjz$BFnT7lJtY2fX&`$w82)@#a!38=Tu>jV?vfF^mz6L{<v#AR#nGgZ<Ql<nBro}inf#<$9ECGG%KhWkN%mmheh3g=vpe
I3<Lh^|qF!#!*18bB3VQ<*5EKv5|usv#fQ`gmUzM$=-Cx3%dc<&ASBPcu5fa3NlK)k3<-
$=NZZ+5`FsGtrdRAZ_U?Keir??ZjK8})-
C8VLFpIEu`@kMZh7niSg3$VFpUWY@lfes|YCRj`4_ZvBB2xN?{p7#=`^UU>XS)~T%nlZUwCy*l*Jhk)*`F-l`eQdj)XjtvH^-
o3p*l<(FGQ20#B-
DY??QfI?aOC#c1onw>=3NIH6kqF$O4T&Es&}*yUo1ldPzNuWHyhs{QX5c+Ww6N(=EUo7hLuvFgaZN*9g@*_r%qMV|>W7t@_FGn~p
*CyXWomNPS9|RN$~H0rB)diX9#&S>T{`y9)}45vX+0-
WXy25!UqQ{J;Tm;cV%58(l}p^WZqdj$fB`MQbX+n8r7RGV%A|mFf?JPAL8EyD-_s3n5IlcMJ*_j7gH?=MV_)+CJiV5ssD<#m0@+L
X<NShxsx977ZHbf|YPN_sXek$0mFc>uWvcKGCb8P-
In|#cr}ym!S{|^+=(d12t=ZT#j1ASbPrYGW&!KQ5y{C;V_cr(LQ2*Wy>itI`YD}vQ9xVntFxTbo-
2r#%9Ox`g3r5>K>Y(8;YTdRqQp{uhKqrn?!PZ^`(7CS-
^nFRZ=7ezzbu7z%12(GT03Qv>ClQ(xVAEzV?q86algCGJ2jC@GWoiSI)u%cYhq5Jdk<7%2^R(>&1aqs3QJB<uXYGvwW=s-1x8-
r`(+vi68IA_e{hzKLfAIe@@V`9VeDd!<fA)X6zPq}8{D;$Q^UKxkt^fJ{_V)f4|L*$gp=#JT45e`NX8~^ZlpCq#FHYM^9)T0Lo}I
Jpl}o360%hy-
4F;LrHntkHKFqhL<x*?8gDsymuCP^uRuA_d?jQXR*H=IK_fYA7z4>(Qf4siE{o2`v;O%L1dwur>hUNxa6wJO8J_}LO$uR)!o)9HB
cVB<H{`g23d_AGut+D2fL|H>GHjh31o;>?F)sNRGJZZp_mT;Z8L#N{?O^1VNzK*d~T}tfsYQvVI#xpVMKulX+wIeVJ1$j_vPA5cK
XJMqWN$Z0!)l3++@eiDcCN6s(`uNwg)M=6|6RkwEmTheko$?Xrj%z@1xI^v$+7_=IhWQt8NyA=e6K2eb8kMpqvVIS@d>;f$-
Z0YG0s-`hRWzCJ8gucAPM&p5e}TprVI;<v^wh-yzdqw?C-
t?wYmBb(0Dp?!G9C%D0=zSYVM748B!%dq0qk9}5cDqVGTh!w!ZMFCxuf|*RFo;-
jQ|?}CUb^o9bu&dUqyj862P0D&DI&Z(RqOB1B>tD7giRp3wcI@VA0)(^C|}<oAs?<@Lod|y2>6E6oEVH7~n)X%p)c92wJh8h7s<5
0+}yM!M8N<m<NPQA;e4a3ZjLLLQ%+c2Z5#red{NthM<+8*)Ubu!$hLM3pZK8X>=Z0?x>b>3pN1%j&9}f#SZkyiz3a<`+@)?!#jL^
pB_D<GZs`twHXcMRXX22@p*%{o>m|CREuR=p134U)DI+(yXw1Z<SlVA-
8zzE`infsmVOZ}05nWk>+G)Vf_s>F!eWjAW$B$ntJRwCMpn33H7?qzY*Hx-hLm>I&>&6YfV{A<g*H{(eJEG;c4-
Pd)wrqIv7>6*Jml?KC%DNj@_PEz30({?E6UTCL|z>wEVUIYM2obVEUeZ^BHfn<5Lx1+)Kkl4Ha4eY&-
lZbQBp0H7cRNOy|y1%rt=+zJ6lF^2(GQi?Ys8MxaE^-
$s3#lb+U7tI5asY@qw`U8BY{dT#zOX#Y2_p7h>qXpgLWnYcbcH;Eq5GuX`PcTrFV-
;3Yg>oBjH(x<JK|$eXN~hSP&e(O1flB24N^{`!wUo(uKsPn7-LR_kn)YuTg1L{-~=5Dz=Go1#yFJBW?S7I>l`r;9WUi%a-
<|M8>$NcDO8Qh1FGB-MwqCNf2br?uk)TB<F3xebKiHMVXB?q*1HfE9FDviiX6)!A_TI&fdP2@Y5_p$~3ivfTAOyiyDXw@>*|I!at
GYkbqi?)Gye?oXS+Xft$Q2cuWxfF@5UUvmix9;5bePAjP~JHWcN@u2(U%(GtyJ<*?OP0v6`@!+DrTJ@^J^NTiwq5Fm)Y`zBqDyUI
)^;f2s2kzT;dbx2<(vPEWLahoZCD;eEC_eYV{PD(p#r$z=S<1ta<V_fv4in-
N^q*YHvd<6K*Z$*(yHbr;ZQ#*m*;hB%xYJj6y3xmkIh;rN3J+YX3@Y8e(C*NeA}c)dnrzB5EjL+O+SAuwfWUge#yL0Fe?8-
`)#ZQQ9KeH^!p45DbgjHN8@4(t(STl<PNce7E$`hBfwIkSgxJY3@e15HWFhG1T&a^s|Lu}*6RR`Z>M2|rRlSBfmEL}R0!k(8dAZV
nPQ#!zJyzNkV&76y+++9C!niOFHelP7HK2QnupZ<|>sp(;=zRJO=SW^tmNMI@l(?~1e~2n?`1pzpg)I5}E>Zx!XFy(1(zbzodqcf
(!R{~n_g5FIm2|bOL<ruHp9+{zJhigUhgEd}#lSx>g1qr48wIFE*zdj9-qE1I!?-}5<x$%7qnZ5?Xa&=AmK(W-
eHqm=yi3<e575l{GWTej>F4F3m0qmI(GrZL`fy3D(y(fPdsCWWCA}j(O@6didv{me-Paw_nO|E5Ej%`^FByb)QOQfQl}|CA7HdGM
M2Y2=Nigm$Bej$=Y257PEXim~_CsSjW90QN>PtnHGX|2L6mz^!-p;KQpw_`<sF3Y3D(8gb__fw*+|beVhn8dFofk}O%AFT(-
Fw|QxUp)S+o`X-Y3-
@uaG?I|m>Xy*XNAWfcg%(MDqYp5(=osbWcs*~mTGtg8s8QDG;hu+v1zMYK)eK@?NPJyi*eKHbAw^kpwnMu=kA+}8;Ib$xSPRl?N1
Ba?v^>3t!;(Hpz1pcm3poClYTp@=sd&-A<E9U!@Xkm9d$A7b<kqdW^mME3Mp>zlkvfD(AM-
8%q&jvD3h*?fL6{FKU{sf`S~kQ<0jye+}}O=U$5`J-
alZH|LgT*M}^+w?&?2%#SPpg`D@>$f8n2jypB#cMyhMo(ge!m{VrENoy2KTfPpCdDv_ivi=Iq8Lf?*q)iex7-
I*pW1qjx~$3r}}p5ENJ8M!vqra_-yt$_ge^X)wl^~3$`bt^2Py@q(bN_=U*?d-
Lepf?KCxpubEKVHv<MxFL4(UXt!FvS^N6H_EqR95Iq52f*nPo-7*^gIyiRc_MCyy#+!-
Tm1*$0y`%BgEU44I*iwTd&nFaL+iqEmy-ou6|L%^6C1^nO6Gs=F1(dA33tW310kAEVP88MxCuSr02fCpW(Ztk`wekQpG-
*mtvdl1Ht@OL{nP$RV6juAh3AD(&u+xDl|G)xOCXxcko6!?(TNI;MT9(wBBN>NA1Y65psNMiuVds&dhjPqJjWq(T>dH&kuM0$NR5
{6BRt%eEH##i?9Fd@!`s-q}`>REx!J8^Z4-
x|Hm6>cYW)Bxw^S)zXHYkK7@;3%?e%TtTtbGa+*`Z6WKR5<J1C{>&}>Kc9Xk(SbA@verZR(9;WUNu`V=U!v}WHTzb}nle;cHL*nK
ln6+G4Jm(^cvWNXFx`c)&JH{5Su;&zS+@PDze(Y2PD_VBgxUYOhguU(Is*SOuGA0bRy;lWQ+nh=SRBcjIx`9aP+O5W>jn_l!I_lM
aT7=50$7kxT+9<J;(?lJ~!RrpFD@7D5$AtzvrS4#`P?{&_^kS0ZoV`7*5oN&N#72wQ)i9bfgZ$pvLI(9Mm#1U)+1B&(O`5OC@*rP
)VkXwI;Z`FesxF~Zxp*|}`+<CP;Q>CPm3nvKGmPKM>G>upK+@YBStn=sf5pK*ry>40gz~F+sLH$je^5&U1QY-
O00;m803lpJ6q^jx8~^~xUjP6b0001TZ(?tBUvO+;Zf<3AFK%IWX=h<{X>V>WaCy}{Yj4{|w%_wBm;|>b9g4Bj2fEg>K-
Nv#plK4+E{cUA5F>IVv8G6t583M8{qOfYX2=<m@}s+5+!b0!8qS<KbLPC~44*xFb|EtvH!^X`Oys$&oHTFbx)M!V<a1{$$}-
K@zVq`tN92i9$=_O8H%^wWWW0|v>C}zb!x-
m#Srx&vXV0Fb+p?${CoampziRWiDT=K2x1!njMeR4~R{G7pl(p`dx7%{>h}y}^ZtU*WlT}r0ojOk8neLdRTd|h>B<NZJ)1Ace=TD
v_a^=JuS=Y-#REfJ4^|im1O_j#sQQv;#A0J=eh-
zJXXHOjXh%he$QH!b)dm7|XbGmYP@KbBFQ&bKQ$8RD)yq<=y&MMioRj$ztk`zdj$1-Xvk=LuD+D38GSUB-NJvKD!GR;M`XN=+^Z_
;&Jv<f%zG(7R)Nei3wHFtmRT*T?8{SnZwnAJsF#jqz5hUGGI(%LBn5UE%>X;VA$R>bh5ZORrmC<n~qCeUle8MDmg`MqvAWw2_YSp
@Y)l+vG_A_6e~=dTz6)erWpTk@q|u>$7f!*AbRTt?^q=N@lUnnT|#i}V;b&h&NwE^FC{rm3*eKZ=S{=J77d#V%bFV|JtJBmb?)YN
^qs@h~&6#a1F)x<}ECIZYJHEQ3);(JCv7YNA~xYxtE(QBgZc05!)&1=3+ZI8q4Xr^u4f#2OLs=J>ZRMJz*j9z@YPE0%z76a^?BuF
>J@h?qT6>~pjTNlJnX;PXs=(B0f=whDXx^g5JQxq|T)#SqAL3BO!MBrXyeMSeHE&&~K7n)wH8lW4Q1?i%8(sM`uUm~%k;^-
bSZE!1q9cWNML%S1FXx~V{~$jV*i2-
%z01H$>MoTq?>;I8ctFRNB6xzCFD8X&4Yfeuv@qPhuSrWGu>1;tiHc@0#L%T_(A3V>2a_=6vTbcCziwRjua{3MMVmw-aQkiPgbPw
>a|BtG%owhrCt=QD!gy*!m)`GAknbBNOn9#cOM9ZPm<;Kg`jNKR3uYmm4sDvK-
&iFo)eLfszyS}ds(Lcj*Mmf6bZm8=Vq8O)4Nf(UpjzUHwpQaj6J;^JoELu!V7fPeQe2`Gd*{7~eQAJfxH+~_A)3v(E&DSl=QvL9A
*mEMMK&uWzvb&-cgSD-
(^h*l!afH7TTG2JlGCOq<tM*^?$XdDC!;)mYcP$2k0xi{v@!3vTta4pFFViPcnY0m7)x&pN?F0LmlegH#I6LpvZAGlZee9=Rcpvv
s-%HYZ*H0Y|^^hBWV?m}d8enkwil4^&kYwdQxec-U-1}4T;U!5#`+!0w-7An?cSsGs(YsrBXC2wzWlfy8S)j4eu{FoMsZgf-aZ9;
58VTE6OIUtTm_MO%xc0P*OApodZmez<DBXZ;;CZ08(StHrwPUS^IyQSP5Xbpf0Wf<bb)W{SYfxnjf&}2&RoytvAr)%g5JZZ1OAPm
i+C_^?3NwWr(_K$zg>-2XSlE$gPITQIhgZ513E{_fRR?jcNvYXHFx93-
~t${(xD&3*N#YJSa0X7AQ;dr3W)B7~{IcOmXu7hN%;&7i7tUdfLJz?)2>H7(JDhKRIt0Jc9#4B!r=|IT$piUCG)?!7tJLp%!l3%`
>(YHz@DVRl<AqRhd{Y&)m?c0ktm(jOxzI$P`UcCG6!|@b&8*lKg_IbRiiX40}X9cPvU8Y&u?420Iqjq553P;4TEE`8Sxb#_(XM0C
EV!$>7aBY5_!=8f*Pn%R^&IY&uND(-
f8(7h8isG~e_j)I1ps$^>fHi{ALq9vsvH`G4{tApxJKMCbaTw|WFbptn+_V53N7v3$f|np2)YYPiH-
Wu_glK3{;-;vwB-(CAJJ3Hc<b}4OX~9Wx-QA1e%NY;xjz2#kPi%*77~48~r%}Bqj!@KyWF3Q#ITFVK+u*Vyh0}n~&Vwvn2Iw-
SaoR+pZ8l&Nf0qifmXQrW&7=>e!zDC4|CkBkp9HvS;zFCzsA;f*rjgr{t(&rQ;2LAo-gQs>R~|STMcst6Q=8^Ul}gq{wu7(>h@({
USW`S$yzm2P-
|1<{1bv9Vd=q>=o~WN}XdRCX4eyBL*S2o9Kz2YoF3>S;<k84<>JP6gypK1?T(d6LG^r<8YjUg}GA0Lah))}!b{z~`wRhD<Gjcv}>
-@2lK8JAT^>-gWUR=IAzc|y35TJg^6PGkM8`Mx^i0ZXzYmE#3s=?={#-
oKdISXoaUWZ3=pvDrK>VaCo^p*~2s%2`rbH@V*yohckj~lHdT!l}-K@e<!Dna799c}(aSHt`)!LDA9o;-
J6J188<tiaG0;uvt7V2_g&;}IlIn`><5PP{1!Qm6vlj!I5I;{c>lEbiPCZI(DIwBn%r<4mNWBcWN;P6K*92R|LNCP4dOYM-
ZhA`HJg8?R=>e3n~c5~)*9`pYBWG!>|0_CUz-A_oA%juPw*1=Pxc{Pg<r`{><=i%)OPFQZ>xzkh$B+=$ThCh!mOf6XAx^?LrDID-
H~z<FO3#k%dQk#~n2Zwe762B7INa&$l~s<e~NhZ*W{xS;_sh^sf%gH2_%c9a+BX2u(_%$P%#d&g9^@E^Mh=%YK7t(aD|5N3sH6)!
(Od7;!~&=laHHacD#24wUM#LXr3>cbPyI~bx(K>#tmBWgPU$)9i^jp!o+2W5lSl#($@b5x?n&E{^{?jo`W;1B@ng?oL)w!`vaa<T
Ikb8}`e5cmgZ7Z~kIT^|+KM-wjZRP;MVC6(rxfB?Qs(V%PIh2s4RjPwdd^VuL2>KKfS0!0vKnx#>*sre|*hcJbJ8=MV(r5Ug2w19
$)UgyyNf;QOhAc5cRKt#YLyawBVJx&)B%T)R#gH6Plx?qjR_2C*RoC7hM$xVakDhwHr;T@)V**1NTNh4u9l@NyjhoS%3RMPT3|Il
C)Bm3z?z!gj2yf~B7;H;+;+UmLUx1!lNRRN))14f6;PffYd?Y3=5*Az>N8s-
kgh>liVW4r!b%?;WO)HVRDfH&c11MV}XkL#vtV~SUxIfZTs=A9uhqU8VB(iyAl0$6agKn;)CVyf4X?|NoBh;wsh%8OaO&m8tT0l&
@139%4=SxGg@K_+t9u0xesCU6`&g5x3TniFdjv_z4W)FoB_F^=03+{gWoH0G#3OfX<r{+RgyU8QY_MkZRp9)m8~M@k(6iPCs0A*4
y%^T2+}lUNo#Toy&<(o#AU?7X@b&UC$LBDH>*Or_K(GLBZemBsJ053U531S`sO*vdlXXlxIBRiN_+Tol6Y_8k4}*|XOTxL;sgo0~
#|!k7zK0YeaAV6Zme$L*NQ!R~;YTY-NEzrjk=VN}O@=9DTT;Wp3bU;L<NuU#t4aPAd^b9nFpc33oDebM(LXCxfNoqFL2#MPnYv05
-T8soJSieY50_c6dp1%^1-{pyUSS=a)vbE|rBv<~R*sdX+UdzS<}^-
BaU^L8sitYzmS0h6R*c!K7$d>SfSpUzGfi=kRZ_SGgD?OnFdzy$FLi3WE@Cs=uFtG<Ran%w6Fxgs4GO!{z2bATX6$da|1gy9^R<i
9oLRHMfm9!NIo93S|0@jjxipbEgJ;oOX(h|jb5lgS&;pGal&T&QDMmw@|fA4h6;o91pm$j2mTCbr82SQZ@jGyDTCg9lCl61DHiB<
yS}Y0{CJ2uCfQN~uZ4R(qw|2|Ly*%8$1kQA3}otm_|37WW>P&gm;MOigMYA>7Y_|BtwjI3zULNiXj%cIq`5$Yk!T2tY?bdb>z3yE
KUTZ?I7&sWD<BU=S_gcN*nAh~&?mpUA0!z5?dQlnI`6CO~Eg>Z{t>$!ZCEzCDB3p5VTO)uPn^FARLp(txyW!S+xDSrpe!mR?It@q
+KO26&dWDaZgNRl1|3K_W_U`g;FIyC~s5SVfoK8GrfBLseY*zo9DrJuMY|l+68eD05dD5cwGzmeTvB5&JP%1WIIF_y(0o;-
;i1oNDakj#e5ey#u5Ke~@KJQ`+xj{7)CU6YJ1jBQP11&2xDZc9>0eG3UeG2ADce0E6)>&Jx1+@%;a760R0tTM@OJrrg7d@3A3fZy
?RXEe`SD1UH2fZK}ZK9FJ92;ASpGeC>dB5?PaiDZvvir_Fc3L|DMYVLu2XXAhlql_pNP5j7Y!1?hEJW_uN|Y_;5O1quqEq_TfYZm
krcHFias&?IpInnT4%&#^ZE2YnJaA2<=D{ZmYCLcj-
8@EJqR7|fk(DNAsCstRYrm`^BJOVmAxzOF1$_)Gi>;g<9rv4VlBdZ*m%Yl^XUB15JI`3Ve%8^UZ{H;Y<cR9lfzA%Y>4l<@#p78s6
AXF$m%Ro|n1TwUIyM_lzi)p6(B_a8t06kYuC`o~WTBO6E!(K*B*hAw?WxNoJv`Vn)jK#8wTwp*qWMXb91jL$|`R~V46JO^BV(8Pt
PYB|GQS3H4-a+p=2>?~|So7Ni#gc3FZHZ)D7I-(1P@__3zf<|;RJ4%M#H!ds5MOkl(=ETRy@x-
Tu%Zbm&OD8_W=NOPACt(+Vl1k9S{Ba0MO#~)VC3vb6AHWXI#`u|@2I8^xmeT1+P(m=TLxqePnRXna^AW?Okk(A57E_rEjk^`HD$3
nArq`6yN^&m4Ei@{gj^qw=2(h$mz;qqObLUf4tT0Xne_<_R-
24&~J~iy>vQ4wb*??dHx7AqwRyQDc5a!NM7zThcOl%8eMl7<cG4uMjkC)$TWq`t}Ooqdo!cym)qs$L7OwV<Z9VP)%KyJB5uNPs^8
N5U3soKYkan|C|FKRGZp&dkbB}tpw&YS>d*AOg$T;jJSJa@N2R;-
I8slNg0eLO#pE^Tjql|#G6AX>`;3k~;kXx5f^m4h9Qur4c#j=E|q{XuUYCi(8Cci%=o{^)Om9R81eHJ9H_uT)0=JHdLh0BG&6s^W
K<*WjmY(ZSDGX|+XuRolR42;OLB%JcMvblcur0P?3sI!z^nCi8_1WR%6^W{66{5Ee*HSn`N_9?9QYkO|e}6dWcZdgpVh2e0fHm^Q
RFYezY&e&>Qww|W7vDU!~eo%&x4!#|PXNf^O?*=@pHsl^PZtMK5OJC4I;dr92O*Q32|*;(egJjvCWk2L9iyjpnw+dn!rAAP>?CX<
e=-
f0prRGhJ=Gz03*85pWHGPk&eEm0b)#)^j&(_j<e&?;+Rx0BA<H@Iv(&}_5Dca~VofI#D_z`PJx0V44@?fBgJxLVB+lb3ms&j<@%k
qQ&+`KaC!Q8a9;wd<n3#!YDRP9B+Cd`<;{v>e0#{T{RUKm(8k3otP)H$J=MLp8lmx-Xa7e)WYn2Yee5QZL-
2v&Bq*KO3+maZw{n;>@kMMId5Xy95DzF<5_i>ghm>`I0@)F5PwFcN335(<(6=2{XJnoo8aHgaA~d6b;Q*VUXnZ;60{IJ%*MQG4r$
<gPhHMgSy?ip6AUh&~dxPc5I8TP88UMcXc)csXa8w?OqX%kwuEVpoxKcRDpL4@bJWdX{5R>#(;3|uWt7w7B5gG()SbkzTjHa;gG$
5i^~Jf`(0KASC<>rA`k?%TRLruuU*%@oo%tiq1V0ce$e81^P4njHeuIdO-Th^Dn!=(I`i5Ye|u%>sA`pobsgMP;7sE=0jIyMj^J(
zebmU%z4<n{OGBfkRm#-
I(dzt?AX!Ep>&@b{?i`D|v3~BnXPXaXRRfh_RmMN*3~+=VIeG=lJ?d6+K2YFImojhCO8#Y=LOcozf;>vnq38)nFwT1_2n2(VOBz?
ww^%D+z~l8Y26zs@K7h~BC02(CM_GJF9c7<^gMo(~Jgw#J;%uOziq#6UtC)9SCx@~VX|AFG#0uh>qE}ZFZiDRI203&aU^%ddVQ;o
MO-xAy(?esgJ6gA*O6W{O!@MdN-
)w`oKfQTlf+bG^!V&5Da^N*c5Fgp`IWPc501z!IS3^Hw#2)BRm}&Jxjs$JpJ&weG$ccFFd@C_M#ay9Ogk~RN8m<fctxX~D6*2q^p
)T@G9amE3CeKyHjuKLsu}HxB60#7iC0I{@c~?XZL*Aia1e`K6U|Hr+6KVz#?o!t#@F5h6M_Lyd#cHsrn8C-
Rzf>Al7=!eXf=bb7C8!892W)-
dOjAEj8P+R=v9O$`uRV*$>JYnJ0ldZW*DqkGd4Mh_^em3~9~tB`iORKO!xsh{QlFPE@9bn?0UH2jh7LaI%zx|6Yk{tW(I$0UFx&)
{JU>HpZIBbuQOH30ex|zTK@-Yz{vb7fNY+CXJ(k6O&3D^_DM1_BgJC4zwQ;Ok>fFAHg*me2I_BOFJrrA-*=t=rkYYkAw4-24TMa-
BstG3I<WY}dZS0_b*V8?SpQVTZsizOFFCX<Ist0^rW*7KSaC`Xqu+Rd~u$8NYlL|2z+(?a1Yf-I4?G@<DTRvE}R_eq!&-
Lm3>krX)Z$AF`=JKb%M{nP~dH?N&&#)k9IjhYya}UFIcJO`=4>9Uh#`i7`6?mAdwu&*rQN0M5q-
&BeRe&V%{2WVu)OMb~dH+88`NIz%KK}B-`^?D1JvH-d-+z4n#v8)T6dDs(+uS}A_ucEuH^038djxQP`1w<W^Ih1WHbtgaG<6-
$J&^N5oczbkQ{RBb`U4YDF7a_Q_1lAsqj^jTBTT3@#OFT!J_NzM^JmwwTGs$wgk+=9pi>fR4)~11pl=}R`i%PGw?lL8BFtFw1SaA
eqg~-LaZ_;u6>~jZUr?`DsGzx-GM>YSoJgkRu?3K`${{WBGxtvL;mJW<I@UE8$)LxJr@7SHaiGp@Q~OA~T}^kZ8>hkNSSuiJbq&a
Rs4_h3_UK`?<BgP7r>_8o7iyMqt^~^~YO0#SY8e`Xv38!23+bIv%L78OgUXjIEM8btD45Fl&^cV);4LQQ1V0_?WV;vv^%$KYn9O4
xJlwOJ-PTVr$8eTuTFeLa5xfG+NZqks<LZ{VXguqC)KTShW|~p6)AnQQpiip#1d1(;BQm~^R?*?p5?p%p&Sz%as<Pb!SP8~Pj&=~
~*hm$nCoR?_VT9O~0&0fgka=()NbGdg{X^3FiqJG_ni?b?6uhZ=pJ12D8}K!_@d2!Ev-
>ncq08Xzbf)k#x>tyw`!B$~>Ks2&@AJ6Bq&q2w7%<<vAa_{x!hYWuUrt?_8*zFkzQ)-?sBarjR?_o8U1CA$j)6K-
Rm1mEjrQ#hAsn#1nsO+7=1oXb0!Q%rdd?05)K;TQjiwCSuD@Q=<k^^YS6E-
%?xG1B(D3~0exbEX(WdXBoej$K71{12>TrTVDIueR^VBKG$KVE=ecs<4xDWBL1Dm05KOZDZY~802Kp*$cGzmHPMb&3WCJc#+c%nl
A7)vNP(|N7mhY<hp?IljV(BTqTlNHIrS6}#a>JC9>UL+tWq&obi$k4VX?3HsV>u7=i1I+`bMNz}rG=m5sCI)<}zcz9vstjPTTb)9
ew|Lrz2Yi%HEYrqO=MIYH3{b10Xi7ZmP)8(`sn&;_peh97POG4IcnI(&dJBS?q`3hM&qd0rNLqRUL=XEV$;X;fLraN?8#G4hR*Q!
AqO?vUA0HESxv*X$_-xwPcfO`jk0tWob2HUF3H+0w->SX13Hg%eAFjli%r*=Qa}IZUK%LIN{cfz%hq@FQCNwX<PU9LMPq&-
L4!C(-1Vl>X*54aDn2X&w^=6!w-g-dNoy=UStk$jju60~>YE>PH-
2(~}iMv!ni*XdUzS#TATYtIt)!Bhn=}+|U```+eRUwiXudr~1fNMPW=9H8S#VzNxdl;cX7#iTsmu4FeH<pM8n>ZaNLTfrb6W-
u$Klm9Kh%Q}6C!+4%g~Q5hDA>YZ`p3QO`HX_8)-
e0bJj3_eL66CjG)by2zdL2S1WcUTxF`>FO~~mC3|Oz}&b+KZtoEvolCQPVJ+};_S$$Vs9pY%0mriR<twtGYaBh2axL6altFzowC0
4V+yYsoc0krk4drCG0rv184#s;oE>m-9)@2QE!50%?fEt8!EXKFj2>En12hDtou5$(iLyRK2xWk!m-
uF^$c{^>8RpXdzWM9>(X3<ZiGStrw7nw|dkQbdLKotXQ)Q$CfZ8Hf%aO^VNGypo}z2$kVLw{%Z<^z=)z_?f9%^rek?htkBWweZo@
pxz+e`t_bvC#g-
J3irZZeCAUUTs}a8k+P4KUVvL#H1rug1GSMj<BJXO@i^i75cVv?@QUVGJeogCn?K7xg+KM2N+kTHsn0ECd@Mi{TdMeYc~}ph_8T}
KY+P4{_lV6q^#$Oz9BqzTFx}Zw1UXsstJ49~&`VPHFT1kDfkXXVoms2OHma}G7XiE4Rb8nXLoe_!)iuF7u5eY>J4{IPR;;7ZYefB
A@L_m{r{(wx6B)Opk?{_quT(PL;A%GL3LDy2Uu--SY|@*94tBzj#){7r0eL`2w_RGdpxB}JkaFg&-
Kn~8jAaZDiVoWON*xX9FY9(*m~n47plj*%P1<Z+yh1$MXP4ml{kZW97j$7iU%@&(TX;T#0uiIJ#^I7N@nf(4&b#ZM;k`G1huU{Y+
jP}mCLP@KQM#dfJO~T9Rzb&~e3`h9K*wo=1BHL;<lHph_vIgW$WZvHws#~-
KhbRukreE5;2Cb!y<X>o*r~VM9o0@!i5m*80`z+L%7_7oO5I;71)Fkrj0Q#HtSYgUb1XPl7fq;d&E@0ZfTI`{O_Sj}gC;3U+cw;p
ry*m><e<ygIA_RlO`5N!^=`*Ye64)IOkDb`Y~S5^BjX)!hqq$f$&9)?R?-*ePki-S4OMGxU8v_WY1M&K)M=i`+X$#&z7-
7@r+O(<h3_fl4(3wtN|30DNpSfkl8?!fXl})cyLWD?g^f|_F`_HMU3@(+O{Y|Ko;O7sZ$e$|Nkje`mfmM>`B#2P!i&?c9<X<F@ZK
NQrl~?ZtQ{<8Jp4uQrPmFabJr`H28_4CkVtDRX5?zXHQpBF&YpgS1^1!FUD4M?w;_5b`{kQzyOO&UytY5M85VyHJBKdO+Mp!;=9*
pm-E?XZ9WxrZcKz%$?4ddJf}XHU4LLc~zvlO|bf}&_VM)gK8Y<yXN#-
<;miW?2e%2AZf65P3e9GRXGDbs3|IfMFrI0?1z)(@?BzV6n>mt+D1dPSqX#?Y$(j?WwA+`GcqfC33=bY?Q*7U-
o`5o6iLOW}1fYDLEz5zmI$-0$6s&~tAd_m7ChMV7Ug(TF?G!wt<1~0k5BgILBH{qz;EATuTx-
lJlsdwhko~pNP_I!l5w=nlQ<yq;7O5tHaFnLMA_~X_Mt6v7z?Jm3yy>HX>JT&f1yMZt9ui-qk4~j<f!`l(|cZ4d5ciw<C)Sh;~)x
|tg)g5`dr=tul-4HKUE5Ie}VGh=MpPy|V_OR!zmBJk_*0JAtYP>#(-
uvU;f}!&FXssx|z4iCqaelXri$6h^S<W+KI(UugfAEC_qr0Myd-LVbwQ**wP6pJF8G53u1ReE`GkUomXQU~KH2~J#<exK4sy;iSi
+T7?F{){WVH<pL1+Gr%u9>(2?Ke20rWYjK_a=mgTwPc9Qt{1CLvb^CyBE4iLFrk4Ld*9@t?J96m(a{d*8oHM)x(^ATMOSh_&-
of0Rj{Q6aWAK2mk;8AzUy?EKyhh004FX000>P0000000000004ji00000a&Ka9bYF06VQy|^axY(BX>MtBUtcb8c~DCM0u%!j000
080000XT!#BvQ*{CW0Gk8=02TlM00000000000Du9H0001TZ(?tBUvO+;Zf<3AFJg6RY-BEQc~DCM0u%!j000080000XT#8H~YWN
fY0MJSR02lxO00000000000Du8F0{{SWZ(?tBUvO+;Zf<3AFJx(RaBN|DE^v8JO928D0~7!N00;m803lo)Uvg^<t^fdYcLD$y000
00000000001h0cIEg0CI0)Z**U9Y+-
J0WpXcNZf9w3WiDfIa8OGD0u%!j000080000XT&=2VM}!;z0Lx?m02crN00000000000Du9g#sC0vZ(?tBUvO+;Zf<3AFK%IUX?A
5UaCuNm0Rj{Q6aWAK2mk;8AzVNdn+(((007Bf000{R0000000000004jiVdDS*a&Ka9bYF06VQy|^axZRSc4=o}bZKvHE^v8JO9c
i10000600sc10RRAQ0096100"""
    payload=base64.b85decode(b"".join(encoded.split()))
    key="68bc6c13d7fc18fc2d4ec8d364b59aa61879a9c2946748e766fda8d9f566058e"
    if hashlib.sha256(payload).hexdigest()!=key:raise RuntimeError("Planner payload checksum mismatch")
    root=Path(__file__).resolve().parent/".hero_planner"/key
    with zipfile.ZipFile(io.BytesIO(payload))as archive:
        for name in archive.namelist():
            parts=Path(name).parts
            if len(parts)!=2 or parts[0]!="robot_planner" or parts[1]in(".",".."):raise RuntimeError("Invalid planner payload path")
            path=root/name;data=archive.read(name)
            if not path.is_file()or path.read_bytes()!=data:
                path.parent.mkdir(parents=True,exist_ok=True)
                temporary=path.with_name(path.name+"."+str(os.getpid())+".tmp")
                temporary.write_bytes(data);os.replace(temporary,path)
    name="_hero_planner_"+key[:16]
    spec=importlib.util.spec_from_file_location(name,root/"robot_planner/__init__.py")
    module=importlib.util.module_from_spec(spec);sys.modules[name]=module;spec.loader.exec_module(module)
    module.install(NavigationMapper,debug=bool(globals().get("DEBUG",True)))
    module.install_display(KavoshRobot,source=__file__,enabled=bool(globals().get("ASYNC_DRAW",True)))

_install_native_planner()
# END GENERATED NATIVE PLANNER BUNDLE

if __name__ == "__main__":
    KavoshRobot.CAPTURE_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "exploration_captures")
    kavosh = KavoshRobot()
    kavosh.driving = HeroDriving(kavosh, autonomous=True)
    exploration = HeroExploration(kavosh, os.path.join(KavoshRobot.CAPTURE_DIR, "exploration.jsonl") if kavosh.DEBUG_ENABLED else None)
    encounter_yield = EncounterYield()
    encounter_observation = EncounterObservation(enabled=OBSERVE_ENCOUNTERS)
    if kavosh.DEBUG_ENABLED:
        kavosh.driving.set_debug(trace_path=os.path.join(KavoshRobot.CAPTURE_DIR, "driving.jsonl"))
        print("Autonomous exploration: arrows/Space take control; G resumes driving; E pauses/resumes target selection.")
    driving_result = None
    step = 0
    prev_keys = set()
    save_key = ord("S")
    auto_key = ord("A")
    closure_key = ord("F")
    MODMASK = Keyboard.SHIFT | Keyboard.CONTROL | Keyboard.ALT

    try:
        while kavosh.robot.step(kavosh.timestep) != -1:
            frame_started_ns = time.perf_counter_ns()
            exploration.clock.now = kavosh.robot.getTime()
            with exploration.log.stage("sensors"):
                data = kavosh.read_all_sensors()
            with exploration.log.stage("mapping", time=data["time"]):
                kavosh.mapper.update(data, show=kavosh.SHOW_MAPS)
    
            step += 1
            model_update = step % 20 == 0
            if model_update:
                with exploration.log.stage("report_model", time=data["time"]):
                    kavosh.mapper.build_report_matrix(show=kavosh.SHOW_MAPS)
    
            # Step 1 navigation layer: 30 mm debug map from the high-res occupancy map.
            with exploration.log.stage("navigation_and_route_safety", time=data["time"]):
                kavosh.navigator.update(data, kavosh.mapper, show=kavosh.SHOW_MAPS)
    
            # Drain ALL keys held this step (Webots queues them) so several drive keys
            # can be pressed at once; strip modifier bits to a set of base key codes.
            raw_keys = []
            k = kavosh.keyboard.getKey()
            while k != -1:
                raw_keys.append(k)
                k = kavosh.keyboard.getKey()
            keys = {kc & ~MODMASK for kc in raw_keys}
            shift = any(kc & Keyboard.SHIFT for kc in raw_keys)
            encounter_yield.update(data, kavosh, exploration, keys, prev_keys)
    
            with exploration.log.stage("vision_observe", time=data["time"]):
                inspection_active = kavosh.vision.prepare_inspection(data,kavosh,exploration,keys,prev_keys)
            exploration.update(data, keys, prev_keys, arrival_ready=(driving_result is None
                or driving_result.state in ('complete','manual','idle','recovered')),
                local_continuation=getattr(kavosh.driving,'committed_crossing',False),
                inspection_active=inspection_active)
            with exploration.log.stage("driving", time=data["time"]):
                driving_result = kavosh.vision.drive_inspection(data,kavosh,exploration,keys,prev_keys,turbo=shift)
            if not inspection_active and kavosh.vision._inspection is None:
                exploration.drive_feedback(data, driving_result)
            with exploration.log.stage("vision_report", time=data["time"]):
                kavosh.vision.process_mission(data,kavosh,exploration)
            if kavosh.vision._inspection is None:
                exploration.finish_tick(data, manual=(not exploration.enabled or
                    driving_result.state in ('manual','idle') or bool(keys & {Keyboard.UP, Keyboard.DOWN,
                    Keyboard.LEFT, Keyboard.RIGHT, ord(' ')})))
    
            encounter_observation.record(data, driving_result, kavosh, exploration,
                loop_started_ns=frame_started_ns)

            # Edge-triggered actions (fire once per fresh press, not while held).
            if kavosh.DEBUG_ENABLED and save_key in keys and save_key not in prev_keys:
                saved = kavosh.save_capture(data)
                model_update = True
                if saved:
                    sys.stdout.write(f"\nSaved capture point {kavosh.capture_index}:\n")
                    for path in saved:
                        sys.stdout.write(f"  {path}\n")
                    sys.stdout.flush()
            if kavosh.DEBUG_ENABLED and auto_key in keys and auto_key not in prev_keys:
                kavosh.auto_capture = not kavosh.auto_capture
                kavosh._last_auto_capture_t = -1e9
                state = "ON" if kavosh.auto_capture else "OFF"
                sys.stdout.write(f"\nAuto-capture {state}\n")
                sys.stdout.flush()
            if closure_key in keys and closure_key not in prev_keys:
                kavosh.mapper.closure_enabled = not kavosh.mapper.closure_enabled
                state = "ON" if kavosh.mapper.closure_enabled else "OFF"
                kavosh.mapper.build_report_matrix(show=kavosh.SHOW_MAPS)
                model_update = True
                if kavosh.DEBUG_ENABLED:
                    sys.stdout.write(
                        f"\nBorder closure {state} "
                        f"(+{kavosh.mapper.last_closure_added} cells)\n"
                    )
                    sys.stdout.flush()
            prev_keys = keys
    
            # while enabled, drop a numbered capture on a fixed interval so a
            # mid-motion disruption is recorded even without a keypress
            if kavosh.DEBUG_ENABLED and kavosh.auto_capture:
                now = data["time"]
                if now - kavosh._last_auto_capture_t >= kavosh.AUTO_CAPTURE_PERIOD_S:
                    kavosh._last_auto_capture_t = now
                    saved = kavosh.save_capture(data)
                    model_update = True
                    if saved:
                        sys.stdout.write(
                            f"\n[auto] saved point {kavosh.capture_index} "
                            f"(speed {kavosh.mapper.last_speed:.0f} mm/s)\n"
                        )
                        sys.stdout.flush()
    
            kavosh.record_debug_step(data, keys, model_update)
            kavosh.print_sensor_data(data)
            exploration.log.event("complete_frame", time=data["time"],
                wall_ms=(time.perf_counter_ns()-frame_started_ns)/1e6,
                target=exploration.target["corner"] if exploration.target else None,
                status=exploration.status)
    
    finally:
        kavosh.vision.close()
        kavosh.driving.close()
        exploration.close()
        kavosh.navigator.close_planner()
        encounter_observation.close()
