nima_adaptive_mesh.py

#!/usr/bin/env python3
"""
nima_adaptive_mesh.py — Frequency-Agile RF Sensing + 3D Mesh Generation

THE GREEN LINES — the navigable 3D wireframe that makes Nima's AR work.

This module takes the RF sensing from nima_vision_core and adds two
critical capabilities:

  1. ADAPTIVE FREQUENCY SWEEP
     The three RF bands don't sit on fixed frequencies. They SWEEP —
     chirping across their range to find the cleanest signal path.
     This is cognitive radio applied to spatial sensing:

       Sub-GHz  (800-950 MHz):  Sweeps to find wall reflections
                                 that aren't in a null zone
       2.4 GHz  (2412-2484 MHz): Hops Wi-Fi channels to avoid
                                 congestion, finds clearest CSI path
       5 GHz    (5180-5825 MHz): Sweeps for best mmWave reflection
                                 off furniture surfaces

     Why? A fixed frequency can hit a destructive interference null
     at a specific wall angle and just NOT see that wall. Sweeping
     past the null catches it on the next chirp. Better signal =
     better mesh = better VFX placement.

  2. 3D ROOM MESH GENERATION
     Converts the spatial map (point cloud from vision_core) into a
     proper 3D mesh — vertices, edges, and faces. This is the
     "green lines" wireframe that:
       - Shows Nima where the room's surfaces are
       - Provides the geometry for correct AR compositing
       - Enables pathfinding through the affordance graph
       - Can be exported for visualization/debugging

     The mesh is built incrementally — each vision frame adds or
     refines mesh points. Over time, the mesh converges on the
     real room geometry, even from noisy RF data.

NEUROBIOLOGICAL MAPPING:
  The adaptive sweep is like the cochlea's frequency decomposition —
  it doesn't listen to one frequency, it sweeps across the spectrum
  to build a complete picture. The mesh is like the hippocampal
  cognitive map — a graph representation of space, not pixels.

  In the brain, place cells (O'Keefe 2014) fire at specific locations,
  and grid cells (Moser & Moser 2014) tile space in hexagonal patterns.
  This mesh does the same: vertices = place cells, edges = spatial
  relationships, faces = continuous surfaces.

INTEGRATION:
  This module wraps SyntheticVisionComposite and enhances it with:
    - Frequency sweep control
    - Mesh generation from spatial maps
    - Mesh-to-VFX pipeline data

  Usage:
    from nima_vision_core import SyntheticVisionComposite
    vision = SyntheticVisionComposite()
    mesh = AdaptiveFrequencyMesh(vision)
    vision.initialize()
    mesh.update(vision.process_frame())
    print(mesh.to_dict())
"""
from __future__ import annotations

import logging
import math
import time
from dataclasses import dataclass, field
from enum import Enum
from typing import Any, Dict, List, Optional, Set, Tuple

import numpy as np

logger = logging.getLogger("NimaMesh")

# Try to import vision types for type hints
try:
    from nima_vision_core import (
        FrequencyBand, HardwareTier, RFDisturbance,
        EntityPose, SurfacePoint, SpatialMap,
        SyntheticVisionComposite,
    )
    _HAS_VISION = True
except ImportError:
    _HAS_VISION = False
    # Provide stubs for standalone type checking
    class FrequencyBand:
        SUB_GHZ = "sub_ghz"
        WIFI_2_4 = "2.4ghz"
        WIFI_5 = "5ghz"
    class HardwareTier:
        TIER_0_SOFTWARE = type("T", (), {"value": 0})()
    class SpatialMap:
        pass


# ═══════════════════════════════════════════════════════════════════════════
# FREQUENCY SWEEP CONFIGURATION
# ═══════════════════════════════════════════════════════════════════════════

@dataclass
class FrequencyRange:
    """
    The tunable frequency range for one RF band.
    Frequencies are in MHz for readability.
    """
    band: str
    center_mhz: float
    bandwidth_mhz: float
    step_mhz: float = 5.0  # how fine the sweep steps are
    min_snr_db: float = 3.0  # minimum SNR to consider a frequency usable

    @property
    def min_mhz(self) -> float:
        return self.center_mhz - self.bandwidth_mhz / 2

    @property
    def max_mhz(self) -> float:
        return self.center_mhz + self.bandwidth_mhz / 2

    @property
    def n_steps(self) -> int:
        return max(1, int(self.bandwidth_mhz / self.step_mhz))


# Default frequency ranges for each band
DEFAULT_FREQUENCY_RANGES = {
    FrequencyBand.SUB_GHZ: FrequencyRange(
        band="sub_ghz",
        center_mhz=900.0,
        bandwidth_mhz=150.0,   # 825-975 MHz
        step_mhz=10.0,
        min_snr_db=2.0,
    ),
    FrequencyBand.WIFI_2_4: FrequencyRange(
        band="2.4ghz",
        center_mhz=2447.0,
        bandwidth_mhz=72.0,   # 2412-2484 MHz (all Wi-Fi channels)
        step_mhz=5.0,
        min_snr_db=5.0,
    ),
    FrequencyBand.WIFI_5: FrequencyRange(
        band="5ghz",
        center_mhz=5500.0,
        bandwidth_mhz=645.0,  # 5180-5825 MHz (all 5GHz channels)
        step_mhz=20.0,
        min_snr_db=3.0,
    ),
}


# ═══════════════════════════════════════════════════════════════════════════
# FREQUENCY QUALITY TRACKER
# ═══════════════════════════════════════════════════════════════════════════

class FrequencyQualityTracker:
    """
    Tracks signal quality at each frequency step across frames.
    Learns which frequencies give the best signal for each band.

    This is the "adaptation" — over time, the system learns that
    900 MHz gives great wall reflections but 935 MHz is in a null,
    so it avoids 935 MHz and spends more time at 900 MHz.

    NEUROBIOLOGICAL ANALOGUE:
      This is sensory adaptation — your brain down-weights noisy
      receptors and up-weights reliable ones. If one eye has
      cataracts, the brain shifts dominance to the good eye.
      Same principle, applied to RF frequencies.
    """

    def __init__(self) -> None:
        # band → list of (freq_mhz, quality_score) per frame
        self._history: Dict[str, List[Tuple[float, float]]] = {}
        self._optimal_freq: Dict[str, float] = {}
        self._max_history = 50  # frames to remember

    def record(self, band: Any, freq_mhz: float, quality: float) -> None:
        """Record the signal quality at a specific frequency."""
        band_key = band.value if hasattr(band, 'value') else str(band)
        if band_key not in self._history:
            self._history[band] = []
        self._history[band].append((freq_mhz, quality))
        if len(self._history[band]) > self._max_history:
            self._history[band] = self._history[band][-self._max_history:]
        self._update_optimal(band)

    def _update_optimal(self, band: str) -> None:
        """Find the frequency with the highest average quality."""
        if band not in self._history or len(self._history[band]) < 3:
            return
        # Aggregate quality by frequency
        freq_scores: Dict[float, List[float]] = {}
        for freq, q in self._history[band]:
            # Bucket to nearest step to avoid floating point drift
            key = round(freq / 5.0) * 5.0
            if key not in freq_scores:
                freq_scores[key] = []
            freq_scores[key].append(q)

        # Find the frequency with highest average quality
        best_freq = 0.0
        best_score = -999.0
        for freq, scores in freq_scores.items():
            if len(scores) >= 2:  # need at least 2 samples
                avg = sum(scores) / len(scores)
                if avg > best_score:
                    best_score = avg
                    best_freq = freq
        if best_freq > 0:
            self._optimal_freq[band] = best_freq

    def get_optimal(self, band: str) -> Optional[float]:
        """Get the best frequency for a band, or None if unknown."""
        return self._optimal_freq.get(band)

    def get_band_quality_map(self, band: str) -> List[Dict[str, Any]]:
        """Get quality scores across all frequencies for a band."""
        if band not in self._history:
            return []
        freq_scores: Dict[float, List[float]] = {}
        for freq, q in self._history[band]:
            key = round(freq / 5.0) * 5.0
            if key not in freq_scores:
                freq_scores[key] = []
            freq_scores[key].append(q)
        return [
            {"freq_mhz": freq, "avg_quality": round(sum(s)/len(s), 3), "samples": len(s)}
            for freq, s in sorted(freq_scores.items())
        ]

    def get_stats(self) -> Dict[str, Any]:
        return {
            "optimal_frequencies": {k: round(v, 1) for k, v in self._optimal_freq.items()},
            "bands_tracked": list(self._history.keys()),
        }


# ═══════════════════════════════════════════════════════════════════════════
# MESH VERTEX / EDGE / FACE
# ═══════════════════════════════════════════════════════════════════════════

@dataclass
class MeshVertex:
    """A vertex in the 3D mesh — a point in room space."""
    vertex_id: int
    position: Tuple[float, float, float]  # (x, y, z) meters
    vertex_type: str = "surface"  # surface / entity / navigation
    confidence: float = 0.5
    last_seen: float = field(default_factory=time.time)

    def to_dict(self) -> Dict[str, Any]:
        return {
            "id": self.vertex_id,
            "pos": [round(float(p), 3) for p in self.position],
            "type": self.vertex_type,
            "conf": round(float(self.confidence), 3),
        }


@dataclass
class MeshEdge:
    """An edge connecting two vertices — a spatial relationship."""
    from_vertex: int
    to_vertex: int
    distance: float  # meters
    edge_type: str = "surface"  # surface / navigation / vertical
    confidence: float = 0.5

    def to_dict(self) -> Dict[str, Any]:
        return {
            "from": self.from_vertex,
            "to": self.to_vertex,
            "dist": round(self.distance, 3),
            "type": self.edge_type,
            "conf": round(self.confidence, 3),
        }


@dataclass
class MeshFace:
    """A triangular face connecting three vertices."""
    v0: int
    v1: int
    v2: int
    normal: Tuple[float, float, float] = (0, 0, 1)  # face normal
    surface_type: str = "unknown"
    material: str = "unknown"


# ═══════════════════════════════════════════════════════════════════════════
# THE 3D ROOM MESH
# ═══════════════════════════════════════════════════════════════════════════

class RoomMesh:
    """
    The 3D mesh of the room — vertices, edges, and faces built
    from RF sensing data.

    This mesh is what makes AR compositing work. Without it, you'd
    have to guess where to place the avatar. With it, you know
    EXACTLY where every surface is, and the compositor can project
    Nima onto the correct position in the camera frame.

    BUILD STRATEGY:
      The mesh is built incrementally from surface points:
        1. Surface points from vision_core arrive each frame
        2. Points are clustered into mesh vertices (Delaunay-like)
        3. Nearby vertices are connected with edges
        4. Triangles (faces) are formed from edge loops
        5. Existing vertices are updated (not recreated) when
           new data confirms their position

    This means the mesh STARTS coarse (first frame) and CONVERGES
    to the real room geometry over multiple frames. Like how your
    visual system builds a scene model over the first 200ms of
    looking at a room.

    NEUROBIOLOGICAL ANALOGUE:
      This is the dorsal stream's "where" pathway — it builds a
      spatial model of the environment without caring about what
      things ARE, only WHERE they are. The ventral stream handles
      "what" (that's the affordance graph + entity recognition).
    """

    # How close two vertices must be to merge
    MERGE_DISTANCE = 0.3  # meters

    def __init__(self) -> None:
        self.vertices: Dict[int, MeshVertex] = {}
        self.edges: List[MeshEdge] = []
        self.faces: List[MeshFace] = []
        self._next_vertex_id: int = 1
        self._recycled_ids: List[int] = []
        self._edge_set: Set[Tuple[int, int]] = set()  # for dedup
        self._frame_count: int = 0

    def build_from_spatial_map(self, spatial_map: Any) -> None:
        """
        Build/update the mesh from a spatial map.

        This is called every frame. It:
          1. Adds new surface points as vertices
          2. Connects nearby vertices with edges
          3. Forms triangular faces from edge loops
        """
        if spatial_map is None:
            return
        self._frame_count += 1
        now = time.time()

        surfaces = getattr(spatial_map, "surfaces", [])
        if not surfaces:
            return

        # Convert surface points to vertex positions
        new_positions: List[Tuple[float, float, float, str, str]] = []
        for sp in surfaces:
            pos = sp.position
            stype = getattr(sp, "surface_type", "unknown")
            material = getattr(sp, "material", "unknown")
            new_positions.append((pos[0], pos[1], pos[2], stype, material))

        # Update or create vertices
        for x, y, z, stype, material in new_positions:
            existing = self._find_nearby_vertex(x, y, z)
            if existing is not None:
                # Update existing vertex (moving average)
                v = self.vertices[existing]
                alpha = 0.1  # slow adaptation
                old_pos = v.position
                v.position = (
                    old_pos[0] * (1 - alpha) + x * alpha,
                    old_pos[1] * (1 - alpha) + y * alpha,
                    old_pos[2] * (1 - alpha) + z * alpha,
                )
                v.confidence = min(1.0, v.confidence + 0.01)
                v.last_seen = now
                v.vertex_type = stype
            else:
                # New vertex
                vid = self._allocate_vertex_id()
                self.vertices[vid] = MeshVertex(
                    vertex_id=vid,
                    position=(round(x, 3), round(y, 3), round(z, 3)),
                    vertex_type=stype,
                    confidence=0.3,
                    last_seen=now,
                )

        # Connect nearby vertices with edges (every 5 frames to save CPU, but always on frame 1)
        if self._frame_count == 1 or self._frame_count % 5 == 0:
            self._rebuild_edges(now)

        # Form faces from edge loops (every 10 frames, but always on frame 1)
        if self._frame_count == 1 or self._frame_count % 10 == 0:
            self._rebuild_faces()

        # Prge stale vertices (not seen for 30 seconds)
        self._prune_stale(now, timeout=30.0)

    def _allocate_vertex_id(self) -> int:
        if self._recycled_ids:
            return self._recycled_ids.pop(0)
        vid = self._next_vertex_id
        self._next_vertex_id += 1
        return vid

    def _find_nearby_vertex(self, x: float, y: float, z: float,
                            max_dist: float = 0.3) -> Optional[int]:
        """Find an existing vertex within max_dist of the given position."""
        best_id = None
        best_dist = max_dist
        for vid, v in self.vertices.items():
            dx = v.position[0] - x
            dy = v.position[1] - y
            dz = v.position[2] - z
            d = math.sqrt(dx*dx + dy*dy + dz*dz)
            if d < best_dist:
                best_dist = d
                best_id = vid
        return best_id

    def _rebuild_edges(self, now: float) -> None:
        """
        Connect nearby vertices with edges.
        Only connects vertices of the same type (floor-floor, wall-wall).
        """
        self.edges.clear()
        self._edge_set.clear()

        # Group vertices by type for efficient comparison
        by_type: Dict[str, List[MeshVertex]] = {}
        for v in self.vertices.values():
            by_type.setdefault(v.vertex_type, []).append(v)

        max_edge_dist = 0.6  # meters — max distance for an edge

        for vtype, verts in by_type.items():
            for i, v1 in enumerate(verts):
                for v2 in verts[i+1:]:
                    dx = v1.position[0] - v2.position[0]
                    dy = v1.position[1] - v2.position[1]
                    dz = v1.position[2] - v2.position[2]
                    dist = math.sqrt(dx*dx + dy*dy + dz*dz)
                    if dist < max_edge_dist:
                        edge_key = (min(v1.vertex_id, v2.vertex_id),
                                    max(v1.vertex_id, v2.vertex_id))
                        if edge_key not in self._edge_set:
                            self._edge_set.add(edge_key)
                            avg_conf = (v1.confidence + v2.confidence) / 2
                            self.edges.append(MeshEdge(
                                from_vertex=edge_key[0],
                                to_vertex=edge_key[1],
                                distance=round(dist, 3),
                                edge_type=vtype,
                                confidence=avg_conf,
                            ))

        # Also connect floor to furniture edges (navigation transitions)
        floor_verts = by_type.get("floor", [])
        furn_verts = by_type.get("furniture", [])
        for fv in floor_verts:
            for fuv in furn_verts:
                dx = fv.position[0] - fuv.position[0]
                dy = fv.position[1] - fuv.position[1]
                dz = fv.position[2] - fuv.position[2]
                dist = math.sqrt(dx*dx + dy*dy + dz*dz)
                if dist < 0.8:  # slightly longer range for floor→furniture
                    edge_key = (min(fv.vertex_id, fuv.vertex_id),
                                max(fv.vertex_id, fuv.vertex_id))
                    if edge_key not in self._edge_set:
                        self._edge_set.add(edge_key)
                        self.edges.append(MeshEdge(
                            from_vertex=edge_key[0],
                            to_vertex=edge_key[1],
                            distance=round(dist, 3),
                            edge_type="navigation",
                            confidence=0.4,
                        ))

    def _rebuild_faces(self) -> None:
        """
        Form triangular faces from connected edges.
        Uses a simple heuristic: for each vertex, find pairs of
        connected neighbors and form a triangle.
        """
        self.faces.clear()

        # Build adjacency list
        adj: Dict[int, List[int]] = {vid: [] for vid in self.vertices}
        for e in self.edges:
            adj.setdefault(e.from_vertex, []).append(e.to_vertex)
            adj.setdefault(e.to_vertex, []).append(e.from_vertex)

        # For each vertex, try to form triangles with pairs of neighbors
        visited_faces: Set[Tuple[int, int, int]] = set()
        for v0, neighbors in adj.items():
            if len(neighbors) < 2:
                continue
            for i in range(len(neighbors)):
                for j in range(i + 1, len(neighbors)):
                    v1, v2 = neighbors[i], neighbors[j]
                    # Check if v1-v2 are also connected (forms a closed triangle)
                    if v2 in adj.get(v1, []):
                        # Canonical face ordering (sorted)
                        face_key = tuple(sorted([v0, v1, v2]))
                        if face_key not in visited_faces:
                            visited_faces.add(face_key)
                            # Compute face normal
                            p0 = self.vertices[v0].position
                            p1 = self.vertices[v1].position
                            p2 = self.vertices[v2].position
                            # Cross product of two edges
                            ax = p1[0] - p0[0]
                            ay = p1[1] - p0[1]
                            az = p1[2] - p0[2]
                            bx = p2[0] - p0[0]
                            by = p2[1] - p0[1]
                            bz = p2[2] - p0[2]
                            nx = ay * bz - az * by
                            ny = az * bx - ax * bz
                            nz = ax * by - ay * bx
                            length = math.sqrt(nx*nx + ny*ny + nz*nz) or 1.0
                            normal = (nx/length, ny/length, nz/length)

                            self.faces.append(MeshFace(
                                v0=face_key[0], v1=face_key[1], v2=face_key[2],
                                normal=normal,
                                surface_type=self.vertices[v0].vertex_type,
                            ))

        # Limit face count for performance (keep highest-confidence)
        if len(self.faces) > 2000:
            # Keep faces from the most confident edges
            # (simple heuristic: just truncate — a smarter version would rank)
            self.faces = self.faces[:2000]

    def _prune_stale(self, now: float, timeout: float = 30.0) -> None:
        """Remove vertices not seen recently."""
        stale = [vid for vid, v in self.vertices.items()
                 if now - v.last_seen > timeout]
        for vid in stale:
            self._recycled_ids.append(vid)
            del self.vertices[vid]
        if stale:
            logger.debug("[Mesh] pruned %d stale vertices", len(stale))

    def get_wireframe_data(self) -> Dict[str, Any]:
        """
        Get the mesh as wireframe data for rendering.

        Returns vertices and edges — the "green lines" that show
        the room structure in a debug visualization.
        """
        return {
            "vertices": [v.to_dict() for v in self.vertices.values()],
            "edges": [e.to_dict() for e in self.edges],
            "vertex_count": len(self.vertices),
            "edge_count": len(self.edges),
            "face_count": len(self.faces),
        }

    def get_stats(self) -> Dict[str, Any]:
        type_counts: Dict[str, int] = {}
        for v in self.vertices.values():
            type_counts[v.vertex_type] = type_counts.get(v.vertex_type, 0) + 1
        return {
            "total_vertices": len(self.vertices),
            "total_edges": len(self.edges),
            "total_faces": len(self.faces),
            "by_type": type_counts,
            "frame_count": self._frame_count,
        }


# ═══════════════════════════════════════════════════════════════════════════
# ADAPTIVE FREQUENCY MESH — the unified system
# ═══════════════════════════════════════════════════════════════════════════

class AdaptiveFrequencyMesh:
    """
    Frequency-agile RF sensing + 3D mesh generation, unified.

    Wraps the vision system and adds:
      1. Frequency sweep control (auto-adapts to find best frequencies)
      2. 3D mesh generation from spatial maps (the "green lines")
      3. Mesh quality metrics and export

    This is what the AR compositor reads to place Nima correctly
    in the camera frame. The mesh tells the compositor:
      - Where the floor is (for correct height)
      - Where walls are (for occlusion)
      - Where furniture is (for affordance-aware placement)

    NEUROBIOLOGICAL ANALOGUE:
      This is the dorsal visual stream (V1 → V2 → PPC) building
      the spatial model of the environment. It processes the "where"
      channel: where are surfaces, how far away, what's the geometry.
      The ventral stream (affordance graph) handles the "what" channel.

    Usage:
        vision = SyntheticVisionComposite()
        mesh = AdaptiveFrequencyMesh(vision)
        vision.initialize()

        # Main loop
        spatial_map = vision.process_frame()
        mesh.update(spatial_map)
        wireframe = mesh.get_wireframe_data()
    """

    # Sweep patterns — how the frequencies cycle
    SWEEP_SEQUENTIAL = "sequential"   # sweep low→high
    SWEEP_ADAPTIVE = "adaptive"       # spend more time on good frequencies
    SWEEP_RANDOM = "random"           # random hop (anti-interference)

    def __init__(self,
                 vision: Any,
                 sweep_mode: str = "adaptive",
                 ) -> None:
        """
        Args:
            vision: A SyntheticVisionComposite instance.
            sweep_mode: How to sweep frequencies (sequential/adaptive/random).
        """
        self.vision = vision
        self.sweep_mode = sweep_mode

        # Frequency ranges for each band
        self.freq_ranges = dict(DEFAULT_FREQUENCY_RANGES)

        # Quality tracking (learns best frequencies over time)
        self.quality_tracker = FrequencyQualityTracker()

        # Current sweep state
        self._sweep_step: Dict[str, int] = {}
        self._current_frequencies: Dict[str, float] = {}
        for band_key in self.freq_ranges:
            self._sweep_step[band_key] = 0
            self._current_frequencies[band_key] = self.freq_ranges[band_key].center_mhz

        # The 3D room mesh
        self.mesh = RoomMesh()

        # Stats
        self._update_count = 0
        self._last_spatial_map: Optional[Any] = None

    def update(self, spatial_map: Any) -> Dict[str, Any]:
        """
        Process a new spatial map:
          1. Advance frequency sweep
          2. Track signal quality at current frequencies
          3. Build/update the 3D mesh
          4. Return mesh + frequency state
        """
        self._update_count += 1
        self._last_spatial_map = spatial_map
        now = time.time()

        # ── 1. ADVANCE FREQUENCY SWEEP ──
        for band_key, freq_range in self.freq_ranges.items():
            if self.sweep_mode == self.SWEEP_SEQUENTIAL:
                # Linear sweep through all steps
                step = self._sweep_step[band_key]
                freq = freq_range.min_mhz + step * freq_range.step_mhz
                if freq > freq_range.max_mhz:
                    freq = freq_range.min_mhz  # wrap around
                    self._sweep_step[band_key] = 0
                else:
                    self._sweep_step[band_key] += 1
                self._current_frequencies[band_key] = freq

            elif self.sweep_mode == self.SWEEP_ADAPTIVE:
                # Spend more time near the optimal frequency
                optimal = self.quality_tracker.get_optimal(band_key)
                if optimal and self._update_count > 10:
                    # 70% of the time, stay near optimal
                    if np.random.random() < 0.7:
                        jitter = np.random.uniform(-freq_range.step_mhz,
                                                     freq_range.step_mhz)
                        freq = optimal + jitter
                    else:
                        # 30% exploration
                        freq = np.random.uniform(freq_range.min_mhz, freq_range.max_mhz)
                    self._current_frequencies[band_key] = freq
                else:
                    # Not enough data yet — sequential
                    step = self._sweep_step[band_key]
                    freq = freq_range.min_mhz + step * freq_range.step_mhz
                    if freq > freq_range.max_mhz:
                        freq = freq_range.min_mhz
                        self._sweep_step[band_key] = 0
                    else:
                        self._sweep_step[band_key] += 1
                    self._current_frequencies[band_key] = freq

            elif self.sweep_mode == self.SWEEP_RANDOM:
                freq = np.random.uniform(freq_range.min_mhz, freq_range.max_mhz)
                self._current_frequencies[band_key] = freq

        # ── 2. TRACK SIGNAL QUALITY ──
        if spatial_map is not None:
            entities = getattr(spatial_map, "entities", [])
            surfaces = getattr(spatial_map, "surfaces", [])
            # Quality = how much structure we detected (more = better frequency)
            quality = min(1.0, (len(entities) * 0.3 + len(surfaces) * 0.01))

            for band_key in self.freq_ranges:
                self.quality_tracker.record(
                    band_key,
                    self._current_frequencies[band_key],
                    quality,
                )

        # ── 3. BUILD/UPDATE THE 3D MESH ──
        self.mesh.build_from_spatial_map(spatial_map)

        # ── 4. Return state ──
        return {
            "frequencies": {k.value if hasattr(k, 'value') else str(k): round(v, 1) for k, v in self._current_frequencies.items()},
            "mesh_stats": self.mesh.get_stats(),
            "quality": self.quality_tracker.get_stats(),
        }

    def get_optimal_frequencies(self) -> Dict[str, Optional[float]]:
        """Get the best frequency found for each band."""
        return {k.value if hasattr(k, 'value') else str(k): self.quality_tracker.get_optimal(k)
                for k in self.freq_ranges}

    def to_dict(self) -> Dict[str, Any]:
        """Full serialization for monitoring / visualization."""
        return {
            "sweep_mode": self.sweep_mode,
            "current_frequencies": {k.value if hasattr(k, 'value') else str(k): round(v, 1) for k, v in self._current_frequencies.items()},
            "optimal_frequencies": self.get_optimal_frequencies(),
            "quality": self.quality_tracker.get_stats(),
            "mesh": self.mesh.get_wireframe_data(),
            "update_count": self._update_count,
        }


# ═══════════════════════════════════════════════════════════════════════════
# SELF-TEST
# ═══════════════════════════════════════════════════════════════════════════

if __name__ == "__main__":
    import json
    import sys
    import os

    logging.basicConfig(level=logging.INFO,
                        format="%(asctime)s [%(levelname)s] %(message)s")

    print("=== Adaptive Frequency Mesh — Self Test ===\n")

    sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))

    # Test 1: RoomMesh build from spatial map
    print("--- Test 1: Mesh from SpatialMap ---")
    from nima_vision_core import (
        SpatialMap, SurfacePoint, EntityPose, HardwareTier,
        SyntheticVisionComposite,
    )

    # Create a fake spatial map with some surfaces
    surfaces = [
        SurfacePoint(position=(0.0, 0.0, 0.0), surface_type="floor", height=0.0, material="hard"),
        SurfacePoint(position=(1.0, 0.0, 0.0), surface_type="floor", height=0.0, material="hard"),
        SurfacePoint(position=(2.0, 0.0, 0.0), surface_type="floor", height=0.0, material="hard"),
        SurfacePoint(position=(0.0, 1.0, 0.0), surface_type="floor", height=0.0, material="hard"),
        SurfacePoint(position=(1.0, 1.0, 0.0), surface_type="floor", height=0.0, material="hard"),
        SurfacePoint(position=(2.0, 1.0, 0.0), surface_type="floor", height=0.0, material="hard"),
        SurfacePoint(position=(0.0, 2.0, 0.0), surface_type="floor", height=0.0, material="hard"),
        SurfacePoint(position=(1.0, 2.0, 0.0), surface_type="floor", height=0.0, material="hard"),
        SurfacePoint(position=(2.0, 2.0, 0.0), surface_type="floor", height=0.0, material="hard"),
        SurfacePoint(position=(3.0, 1.0, 0.4), surface_type="furniture", height=0.4, material="soft"),
    ]
    spatial_map = SpatialMap(
        surfaces=surfaces,
        entities=[EntityPose(entity_id=1, position=(1.5, 1.5, 0.0),
                              velocity=(0.1, 0.0, 0.0), height_estimate=1.7,
                              confidence=0.8)],
        room_bounds={"x_min": 0, "x_max": 4, "y_min": 0, "y_max": 4},
        tier_used=HardwareTier.TIER_0_SOFTWARE,
    )

    mesh = RoomMesh()
    mesh.build_from_spatial_map(spatial_map)
    stats = mesh.get_stats()
    print(f"  Vertices: {stats['total_vertices']}")
    print(f"  Edges:    {stats['total_edges']}")
    print(f"  Faces:    {stats['total_faces']}")
    print(f"  By type:  {stats['by_type']}")

    # Test 2: Incremental updates (mesh should converge, not duplicate)
    print("\n--- Test 2: Incremental convergence ---")
    for i in range(10):
        # Slightly perturb positions (simulating RF noise)
        import random
        noisy_surfaces = []
        for sp in surfaces:
            noisy_surfaces.append(SurfacePoint(
                position=(
                    sp.position[0] + random.uniform(-0.05, 0.05),
                    sp.position[1] + random.uniform(-0.05, 0.05),
                    sp.position[2],
                ),
                surface_type=sp.surface_type,
                height=sp.height,
                material=sp.material,
            ))
        noisy_map = SpatialMap(surfaces=noisy_surfaces, tier_used=HardwareTier.TIER_0_SOFTWARE)
        mesh.build_from_spatial_map(noisy_map)

    stats = mesh.get_stats()
    print(f"  After 10 noisy updates:")
    print(f"  Vertices: {stats['total_vertices']} (should be ~10, not 100)")
    print(f"  Edges:    {stats['total_edges']}")
    print(f"  Faces:    {stats['total_faces']}")

    # Test 3: AdaptiveFrequencyMesh with real vision system
    print("\n--- Test 3: Full AdaptiveFrequencyMesh ---")
    vision = SyntheticVisionComposite()
    adaptive_mesh = AdaptiveFrequencyMesh(vision, sweep_mode="adaptive")
    vision.initialize()

    for i in range(5):
        spatial_map = vision.process_frame()
        result = adaptive_mesh.update(spatial_map)
        print(f"  Frame {i+1}: freqs={result['frequencies']}, "
              f"mesh verts={result['mesh_stats']['total_vertices']}, "
              f"edges={result['mesh_stats']['total_edges']}")

    print(f"\n  Optimal frequencies: {adaptive_mesh.get_optimal_frequencies()}")
    print(f"  Quality: {json.dumps(adaptive_mesh.quality_tracker.get_stats(), indent=4, default=str)}")

    vision.shutdown()

    # Test 4: Wireframe export
    print("\n--- Test 4: Wireframe export ---")
    wireframe = adaptive_mesh.mesh.get_wireframe_data()
    print(f"  Export: {wireframe['vertex_count']} vertices, {wireframe['edge_count']} edges")
    if wireframe['vertices']:
        print(f"  Sample vertex: {wireframe['vertices'][0]}")
    if wireframe['edges']:
        print(f"  Sample edge: {wireframe['edges'][0]}")

    print("\n=== Adaptive mesh self-test PASSED ===")