From b2a40da12724750ef6f16d2d6a6a482cde155431 Mon Sep 17 00:00:00 2001
From: Yash Nitin Virulkar <virulkaryashed@gmail.com>
Date: Thu, 30 Apr 2026 22:48:38 +0530
Subject: [PATCH] fix(sensing): UDP relay for multi-node node_id firmware
 clobber

  Closes #374
  May fix #386 (same root cause, unverified on Windows)
---
 pyproject.toml                                |   2 +-
 scripts/csi_node_id_relay.py                  |  84 +++++
 scripts/test_csi_node_id_relay.py             | 103 ++++++
 ui/heatmap.html                               | 307 ++++++++++++++++++
 ui/viz.html                                   |   6 +-
 v2/Cargo.lock                                 | 113 ++++++-
 .../wifi-densepose-sensing-server/Cargo.toml  |   8 +-
 .../src/field_bridge.rs                       | 122 ++++++-
 8 files changed, 729 insertions(+), 16 deletions(-)
 create mode 100755 scripts/csi_node_id_relay.py
 create mode 100644 scripts/test_csi_node_id_relay.py
 create mode 100644 ui/heatmap.html

diff --git a/pyproject.toml b/pyproject.toml
index aa03506b8..0ad014dba 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -282,7 +282,7 @@ addopts = [
     "--cov-branch",
     "-v",
 ]
-testpaths = ["tests"]
+testpaths = ["tests", "scripts"]
 python_files = ["test_*.py", "*_test.py"]
 python_classes = ["Test*", "Describe*", "When*"]
 python_functions = ["test_*", "it_*", "should_*"]
diff --git a/scripts/csi_node_id_relay.py b/scripts/csi_node_id_relay.py
new file mode 100755
index 000000000..21066af7c
--- /dev/null
+++ b/scripts/csi_node_id_relay.py
@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+"""
+UDP relay that rewrites byte 4 (node_id) of CSI/vitals packets based on source IP.
+
+Workaround for firmware bug (issues #374/#386): the WiFi stack clobbers
+g_nvs_config.node_id between main and csi_collector_init, so all CSI packets ship
+with node_id=1 regardless of NVS. We patch byte 4 here using a source-IP map.
+
+Listens on 0.0.0.0:5005 (where ESP32s send) and forwards rewritten packets to
+127.0.0.1:<dest_port> (where sensing-server listens with --udp-port).
+
+Usage: csi_node_id_relay.py [--listen-port 5005] [--dest-port 5099]
+"""
+import argparse
+import socket
+import sys
+
+# Source IP -> node_id mapping. Update as new ESPs join.
+IP_TO_NODE_ID = {
+    "192.168.13.222": 1,   # ESP32-S3 #1, MAC 10:b4:1d:ea:eb:a0
+    "192.168.3.246":  2,   # ESP32-S3 #2, MAC b8:f8:62:f9:d5:58
+}
+
+# Magics whose byte 4 is node_id (LE). 0xC5110001 CSI, 0xC5110002 vitals,
+# 0xC5110004 wasm-output (ADR-040). All three put node_id at offset 4.
+NODE_ID_MAGICS = {
+    bytes.fromhex("010011c5"),
+    bytes.fromhex("020011c5"),
+    bytes.fromhex("040011c5"),
+}
+
+def rewrite_packet(data: bytes, src_ip: str,
+                   ip_map: dict | None = None,
+                   magics: set | None = None) -> bytes:
+    """Return packet with byte[4] (node_id) corrected for src_ip, or data unchanged."""
+    if ip_map is None:
+        ip_map = IP_TO_NODE_ID
+    if magics is None:
+        magics = NODE_ID_MAGICS
+    nid = ip_map.get(src_ip)
+    if not (isinstance(nid, int) and 0 <= nid <= 255):
+        return data
+    if len(data) >= 5 and data[:4] in magics and data[4] != nid:
+        return bytes([data[0], data[1], data[2], data[3], nid]) + data[5:]
+    return data
+
+
+def main():
+    ap = argparse.ArgumentParser()
+    ap.add_argument("--listen-port", type=int, default=5005)
+    ap.add_argument("--dest-port",   type=int, default=5099)
+    ap.add_argument("--dest-host",   default="127.0.0.1")
+    args = ap.parse_args()
+
+    rx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    rx.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+    rx.bind(("0.0.0.0", args.listen_port))
+    tx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    dest = (args.dest_host, args.dest_port)
+
+    print(f"relay: 0.0.0.0:{args.listen_port} -> {args.dest_host}:{args.dest_port}", flush=True)
+    print(f"map:   {IP_TO_NODE_ID}", flush=True)
+
+    counts = {}
+    rewrites = 0
+    total_forwarded = 0
+    while True:
+        try:
+            data, addr = rx.recvfrom(4096)
+        except KeyboardInterrupt:
+            break
+        src_ip = addr[0]
+        rewritten = rewrite_packet(data, src_ip)
+        if rewritten is not data:
+            data = rewritten
+            rewrites += 1
+        tx.sendto(data, dest)
+        counts[src_ip] = counts.get(src_ip, 0) + 1
+        total_forwarded += 1
+        if total_forwarded % 200 == 0:
+            print(f"forwarded: {counts}  rewrites={rewrites}", flush=True)
+
+if __name__ == "__main__":
+    main()
diff --git a/scripts/test_csi_node_id_relay.py b/scripts/test_csi_node_id_relay.py
new file mode 100644
index 000000000..ed56ec187
--- /dev/null
+++ b/scripts/test_csi_node_id_relay.py
@@ -0,0 +1,103 @@
+"""
+Tests for csi_node_id_relay.py — verifies node_id rewrite logic.
+
+Covers fix for issues #374 / #386:
+  All ESP32 nodes ship node_id=1 due to firmware clobber bug.
+  Relay must rewrite byte[4] to the correct node_id based on source IP.
+"""
+import sys
+import os
+sys.path.insert(0, os.path.dirname(__file__))
+
+from csi_node_id_relay import rewrite_packet, IP_TO_NODE_ID, NODE_ID_MAGICS
+
+# CSI magic (0xC5110001 LE)
+CSI_MAGIC = bytes.fromhex("010011c5")
+# Vitals magic (0xC5110002 LE)
+VITALS_MAGIC = bytes.fromhex("020011c5")
+
+IP_NODE1 = "192.168.13.222"
+IP_NODE2 = "192.168.3.246"
+IP_UNKNOWN = "10.0.0.99"
+
+
+def make_packet(magic: bytes, node_id: int, payload_len: int = 20) -> bytes:
+    return magic + bytes([node_id]) + bytes(payload_len)
+
+
+# ── node_id rewrite ───────────────────────────────────────────────────────────
+
+def test_node1_ip_rewrites_to_1_when_clobbered():
+    pkt = make_packet(CSI_MAGIC, node_id=1, payload_len=20)  # already correct
+    result = rewrite_packet(pkt, IP_NODE1)
+    assert result[4] == 1
+    assert result is pkt  # no copy when already correct
+
+def test_node2_ip_rewrites_from_1_to_2():
+    # firmware clobber: node 2 sends node_id=1
+    pkt = make_packet(CSI_MAGIC, node_id=1, payload_len=20)
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result[4] == 2, f"expected node_id=2, got {result[4]}"
+
+def test_node2_vitals_rewritten_too():
+    pkt = make_packet(VITALS_MAGIC, node_id=1, payload_len=10)
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result[4] == 2
+
+def test_payload_unchanged_after_rewrite():
+    payload = bytes(range(20))
+    pkt = CSI_MAGIC + bytes([1]) + payload
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result[5:] == payload, "payload must not be modified"
+
+def test_magic_preserved_after_rewrite():
+    pkt = make_packet(CSI_MAGIC, node_id=1)
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result[:4] == CSI_MAGIC
+
+# ── passthrough cases ─────────────────────────────────────────────────────────
+
+def test_unknown_ip_passes_through_unchanged():
+    pkt = make_packet(CSI_MAGIC, node_id=1)
+    result = rewrite_packet(pkt, IP_UNKNOWN)
+    assert result is pkt  # identity — not copied
+
+def test_wrong_magic_passes_through_unchanged():
+    bad_magic = bytes([0xDE, 0xAD, 0xBE, 0xEF])
+    pkt = bad_magic + bytes([1]) + bytes(10)
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result is pkt
+
+def test_packet_too_short_passes_through_unchanged():
+    pkt = CSI_MAGIC  # only 4 bytes, no node_id byte
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result is pkt
+
+def test_already_correct_node_id_not_copied():
+    # node 2 already has node_id=2 (won't happen in practice but must not corrupt)
+    pkt = make_packet(CSI_MAGIC, node_id=2)
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result is pkt  # no copy needed
+
+# ── all three magics are covered ──────────────────────────────────────────────
+
+def test_all_three_magics_are_rewritten():
+    wasm_magic = bytes.fromhex("040011c5")  # ADR-040: 0xC5110004
+    for magic in [CSI_MAGIC, VITALS_MAGIC, wasm_magic]:
+        pkt = magic + bytes([1]) + bytes(10)
+        result = rewrite_packet(pkt, IP_NODE2)
+        assert result[4] == 2, f"magic {magic.hex()} not rewritten"
+
+# ── ip map contents ───────────────────────────────────────────────────────────
+
+def test_both_nodes_in_default_map():
+    assert IP_NODE1 in IP_TO_NODE_ID
+    assert IP_NODE2 in IP_TO_NODE_ID
+    assert IP_TO_NODE_ID[IP_NODE1] == 1
+    assert IP_TO_NODE_ID[IP_NODE2] == 2
+
+def test_custom_ip_map():
+    custom_map = {"10.0.0.1": 3}
+    pkt = make_packet(CSI_MAGIC, node_id=1)
+    result = rewrite_packet(pkt, "10.0.0.1", ip_map=custom_map)
+    assert result[4] == 3
diff --git a/ui/heatmap.html b/ui/heatmap.html
new file mode 100644
index 000000000..d8c271aca
--- /dev/null
+++ b/ui/heatmap.html
@@ -0,0 +1,307 @@
+<!DOCTYPE html>
+<html lang="en">
+<head>
+  <meta charset="UTF-8">
+  <meta name="viewport" content="width=device-width, initial-scale=1.0">
+  <title>WiFi DensePose — Live 2D View</title>
+  <style>
+    * { margin: 0; padding: 0; box-sizing: border-box; }
+    html, body {
+      background: #050510;
+      color: #88bbdd;
+      font-family: 'Courier New', monospace;
+      height: 100%;
+      display: flex;
+      flex-direction: column;
+      align-items: center;
+      justify-content: center;
+      gap: 16px;
+    }
+    h1 {
+      font-size: 13px;
+      letter-spacing: 4px;
+      text-transform: uppercase;
+      color: #446688;
+    }
+    #canvas-wrap {
+      position: relative;
+      border: 1px solid #112233;
+    }
+    canvas { display: block; }
+    #overlay {
+      position: absolute;
+      top: 8px; left: 8px;
+      font-size: 11px;
+      line-height: 1.8;
+      color: #aaddff;
+      text-shadow: 0 0 6px #0066ff;
+      pointer-events: none;
+    }
+    #stats {
+      display: flex;
+      gap: 32px;
+      font-size: 11px;
+      color: #446688;
+    }
+    #stats span { color: #88ccff; }
+    #legend {
+      display: flex;
+      align-items: center;
+      gap: 8px;
+      font-size: 10px;
+      color: #334455;
+    }
+    #legend canvas { border: none; }
+    #status { font-size: 10px; color: #223344; }
+  </style>
+</head>
+<body>
+  <h1>WiFi DensePose — Top-Down RF View</h1>
+
+  <div id="canvas-wrap">
+    <canvas id="heatmap" width="400" height="400"></canvas>
+    <div id="overlay">
+      <div>PERSONS: <span id="ov-count">—</span></div>
+      <div>MOTION: <span id="ov-motion">—</span></div>
+      <div>CONFIDENCE: <span id="ov-conf">—</span></div>
+    </div>
+  </div>
+
+  <div id="stats">
+    <div>NODES: <span id="st-nodes">—</span></div>
+    <div>BREATHING: <span id="st-br">—</span> bpm</div>
+    <div>CALIBRATION: <span id="st-cal">—</span></div>
+  </div>
+
+  <div id="legend">
+    <span>low</span>
+    <canvas id="legend-canvas" width="120" height="10"></canvas>
+    <span>high RF energy</span>
+  </div>
+
+  <div id="status">fetching...</div>
+
+  <script>
+    const canvas = document.getElementById('heatmap');
+    const ctx = canvas.getContext('2d');
+    const W = canvas.width, H = canvas.height;
+
+    // Draw legend gradient
+    const lc = document.getElementById('legend-canvas');
+    const lx = lc.getContext('2d');
+    const lg = lx.createLinearGradient(0, 0, 120, 0);
+    lg.addColorStop(0,   '#050520');
+    lg.addColorStop(0.3, '#002266');
+    lg.addColorStop(0.6, '#0044cc');
+    lg.addColorStop(0.85,'#00aaff');
+    lg.addColorStop(1,   '#ffffff');
+    lx.fillStyle = lg;
+    lx.fillRect(0, 0, 120, 10);
+
+    function rfColor(v) {
+      // deep blue → cyan → white
+      const t = Math.max(0, Math.min(1, v));
+      if (t < 0.3) {
+        const s = t / 0.3;
+        return [Math.round(5 + s * 0), Math.round(5 + s * 30), Math.round(16 + s * 120)];
+      } else if (t < 0.6) {
+        const s = (t - 0.3) / 0.3;
+        return [Math.round(0 + s * 0), Math.round(34 + s * 136), Math.round(136 + s * 64)];
+      } else if (t < 0.85) {
+        const s = (t - 0.6) / 0.25;
+        return [Math.round(0 + s * 0), Math.round(170 + s * 55), Math.round(200 + s * 55)];
+      } else {
+        const s = (t - 0.85) / 0.15;
+        return [Math.round(0 + s * 255), Math.round(225 + s * 30), Math.round(255)];
+      }
+    }
+
+    // ESP32 node positions (normalized to 0-1 in the 1.5m x 1.5m sensing area)
+    // Node 1 at (0,0), Node 2 at (1.5,0) → normalized to room width
+    const NODE_POSITIONS = [
+      { id: 1, nx: 0.0,  ny: 0.5 },
+      { id: 2, nx: 1.0,  ny: 0.5 },
+    ];
+
+    function drawFrame(data) {
+      const sf = data.signal_field || {};
+      const grid = sf.grid_size || [20, 1, 20];
+      const vals = sf.values || [];
+      const GW = grid[0], GH = grid[2];
+      const cellW = W / GW, cellH = H / GH;
+
+      // Draw heatmap cells
+      const imgData = ctx.createImageData(W, H);
+      const px = imgData.data;
+
+      for (let gz = 0; gz < GH; gz++) {
+        for (let gx = 0; gx < GW; gx++) {
+          const idx = gz * GW + gx;
+          const v = vals[idx] || 0;
+          const [r, g, b] = rfColor(v);
+
+          const x0 = Math.floor(gx * cellW);
+          const x1 = Math.floor((gx + 1) * cellW);
+          const y0 = Math.floor(gz * cellH);
+          const y1 = Math.floor((gz + 1) * cellH);
+
+          for (let py = y0; py < y1; py++) {
+            for (let ppx = x0; ppx < x1; ppx++) {
+              const i = (py * W + ppx) * 4;
+              px[i]   = r;
+              px[i+1] = g;
+              px[i+2] = b;
+              px[i+3] = 255;
+            }
+          }
+        }
+      }
+      ctx.putImageData(imgData, 0, 0);
+
+      // Draw room border
+      ctx.strokeStyle = '#112233';
+      ctx.lineWidth = 1;
+      ctx.strokeRect(0, 0, W, H);
+
+      // Draw grid lines (subtle)
+      ctx.strokeStyle = 'rgba(17,34,51,0.4)';
+      ctx.lineWidth = 0.5;
+      for (let i = 1; i < GW; i++) {
+        ctx.beginPath(); ctx.moveTo(i * cellW, 0); ctx.lineTo(i * cellW, H); ctx.stroke();
+      }
+      for (let i = 1; i < GH; i++) {
+        ctx.beginPath(); ctx.moveTo(0, i * cellH); ctx.lineTo(W, i * cellH); ctx.stroke();
+      }
+
+      // Find hotspots and draw person blobs
+      // Build a smoothed list of high-energy spots
+      const spots = [];
+      for (let gz = 0; gz < GH; gz++) {
+        for (let gx = 0; gx < GW; gx++) {
+          const v = vals[gz * GW + gx] || 0;
+          if (v > 0.5) spots.push({ gx, gz, v });
+        }
+      }
+      spots.sort((a, b) => b.v - a.v);
+
+      // Cluster into person blobs (greedy, 3-cell radius)
+      const blobs = [];
+      const used = new Set();
+      for (const s of spots) {
+        const key = `${s.gx},${s.gz}`;
+        if (used.has(key)) continue;
+        const blob = { gx: s.gx, gz: s.gz, v: s.v, count: 1 };
+        for (const s2 of spots) {
+          const k2 = `${s2.gx},${s2.gz}`;
+          if (!used.has(k2) && Math.hypot(s2.gx - s.gx, s2.gz - s.gz) <= 3) {
+            used.add(k2);
+            blob.gx = (blob.gx * blob.count + s2.gx) / (blob.count + 1);
+            blob.gz = (blob.gz * blob.count + s2.gz) / (blob.count + 1);
+            blob.count++;
+          }
+        }
+        blobs.push(blob);
+        if (blobs.length >= (data.estimated_persons || 1)) break;
+      }
+
+      // Draw person blobs
+      const nPersons = data.estimated_persons || 0;
+      blobs.slice(0, nPersons).forEach((blob, i) => {
+        const cx = (blob.gx + 0.5) * cellW;
+        const cy = (blob.gz + 0.5) * cellH;
+
+        // Glow
+        const grad = ctx.createRadialGradient(cx, cy, 0, cx, cy, cellW * 3.5);
+        grad.addColorStop(0,   'rgba(0,200,255,0.35)');
+        grad.addColorStop(0.5, 'rgba(0,100,200,0.15)');
+        grad.addColorStop(1,   'rgba(0,0,100,0)');
+        ctx.beginPath();
+        ctx.arc(cx, cy, cellW * 3.5, 0, Math.PI * 2);
+        ctx.fillStyle = grad;
+        ctx.fill();
+
+        // Center dot
+        ctx.beginPath();
+        ctx.arc(cx, cy, 5, 0, Math.PI * 2);
+        ctx.fillStyle = '#00eeff';
+        ctx.fill();
+
+        // Simple stick figure (scaled to cell size)
+        const s = cellH * 2.5;
+        ctx.strokeStyle = 'rgba(0,220,255,0.85)';
+        ctx.lineWidth = 1.5;
+        ctx.lineCap = 'round';
+
+        // head
+        ctx.beginPath();
+        ctx.arc(cx, cy - s * 0.45, s * 0.14, 0, Math.PI * 2);
+        ctx.stroke();
+        // body
+        ctx.beginPath(); ctx.moveTo(cx, cy - s * 0.31); ctx.lineTo(cx, cy + s * 0.1); ctx.stroke();
+        // arms
+        ctx.beginPath(); ctx.moveTo(cx - s * 0.22, cy - s * 0.1); ctx.lineTo(cx + s * 0.22, cy - s * 0.1); ctx.stroke();
+        // legs
+        ctx.beginPath(); ctx.moveTo(cx, cy + s * 0.1); ctx.lineTo(cx - s * 0.15, cy + s * 0.45); ctx.stroke();
+        ctx.beginPath(); ctx.moveTo(cx, cy + s * 0.1); ctx.lineTo(cx + s * 0.15, cy + s * 0.45); ctx.stroke();
+
+        // label
+        ctx.fillStyle = '#aaddff';
+        ctx.font = '10px Courier New';
+        ctx.fillText(`P${i + 1}`, cx + 8, cy - s * 0.45);
+      });
+
+      // Draw node positions
+      NODE_POSITIONS.forEach(n => {
+        const nx = n.nx * W;
+        const ny = n.ny * H;
+        ctx.beginPath();
+        ctx.arc(nx, ny, 6, 0, Math.PI * 2);
+        ctx.strokeStyle = '#ffaa00';
+        ctx.lineWidth = 1.5;
+        ctx.stroke();
+        ctx.fillStyle = '#ffaa00';
+        ctx.font = '9px Courier New';
+        ctx.fillText(`N${n.id}`, nx + 8, ny + 3);
+      });
+
+      // Update overlay
+      document.getElementById('ov-count').textContent = nPersons;
+      document.getElementById('ov-motion').textContent = (data.classification || {}).motion_level || '—';
+      const conf = ((data.classification || {}).confidence || 0);
+      document.getElementById('ov-conf').textContent = (conf * 100).toFixed(0) + '%';
+
+      // Update stats
+      const nodes = (data.node_features || []).filter(n => !n.stale);
+      document.getElementById('st-nodes').textContent = nodes.length;
+      const br = ((data.vital_signs || {}).breathing_rate_bpm || 0).toFixed(1);
+      document.getElementById('st-br').textContent = br;
+    }
+
+    async function fetchAndDraw() {
+      try {
+        const [senseRes, calRes] = await Promise.all([
+          fetch('/api/v1/sensing/latest'),
+          fetch('/api/v1/calibration/status'),
+        ]);
+        const data = await senseRes.json();
+        const cal = await calRes.json();
+
+        drawFrame(data);
+
+        const calStatus = cal.status || 'none';
+        const calEl = document.getElementById('st-cal');
+        calEl.textContent = calStatus.toUpperCase();
+        calEl.style.color = calStatus === 'Fresh' ? '#00ff88' : calStatus === 'Collecting' ? '#ffaa00' : '#446688';
+
+        document.getElementById('status').textContent =
+          `tick ${data.tick || '?'} · ${new Date().toLocaleTimeString()}`;
+      } catch (e) {
+        document.getElementById('status').textContent = 'error: ' + e.message;
+      }
+    }
+
+    fetchAndDraw();
+    setInterval(fetchAndDraw, 400);
+  </script>
+</body>
+</html>
diff --git a/ui/viz.html b/ui/viz.html
index 54fa0a5fb..896b8f95f 100644
--- a/ui/viz.html
+++ b/ui/viz.html
@@ -85,8 +85,8 @@
   </div>
 
   <!-- Three.js and OrbitControls from CDN -->
-  <script src="https://unpkg.com/three@0.160.0/build/three.min.js"></script>
-  <script src="https://unpkg.com/three@0.160.0/examples/js/controls/OrbitControls.js"></script>
+  <script src="https://unpkg.com/three@0.146.0/build/three.min.js"></script>
+  <script src="https://unpkg.com/three@0.146.0/examples/js/controls/OrbitControls.js"></script>
   <!-- Stats.js for performance monitoring -->
   <script src="https://unpkg.com/stats.js@0.17.0/build/stats.min.js"></script>
 
@@ -177,7 +177,7 @@
 
         // 8. WebSocket client
         state.wsClient = new WebSocketClient({
-          url: 'ws://localhost:8000/ws/pose',
+          url: `ws://${location.hostname || 'localhost'}:${location.port || 8080}/api/v1/stream/pose`,
           onMessage: (msg) => handleWebSocketMessage(msg),
           onStateChange: (newState, oldState) => handleConnectionStateChange(newState, oldState),
           onError: (err) => console.error('[VIZ] WebSocket error:', err)
diff --git a/v2/Cargo.lock b/v2/Cargo.lock
index 2425594e1..080dec397 100644
--- a/v2/Cargo.lock
+++ b/v2/Cargo.lock
@@ -231,6 +231,18 @@ dependencies = [
  "wait-timeout",
 ]
 
+[[package]]
+name = "async-compression"
+version = "0.4.42"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "e79b3f8a79cccc2898f31920fc69f304859b3bd567490f75ebf51ae1c792a9ac"
+dependencies = [
+ "compression-codecs",
+ "compression-core",
+ "pin-project-lite",
+ "tokio",
+]
+
 [[package]]
 name = "async-trait"
 version = "0.1.89"
@@ -318,7 +330,7 @@ dependencies = [
  "sync_wrapper 1.0.2",
  "tokio",
  "tokio-tungstenite",
- "tower",
+ "tower 0.5.3",
  "tower-layer",
  "tower-service",
  "tracing",
@@ -871,6 +883,23 @@ dependencies = [
  "memchr",
 ]
 
+[[package]]
+name = "compression-codecs"
+version = "0.4.38"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "ce2548391e9c1929c21bf6aa2680af86fe4c1b33e6cea9ac1cfeec0bd11218cf"
+dependencies = [
+ "compression-core",
+ "flate2",
+ "memchr",
+]
+
+[[package]]
+name = "compression-core"
+version = "0.4.32"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "cc14f565cf027a105f7a44ccf9e5b424348421a1d8952a8fc9d499d313107789"
+
 [[package]]
 name = "concurrent-queue"
 version = "2.5.0"
@@ -2371,6 +2400,16 @@ version = "0.16.1"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "841d1cc9bed7f9236f321df977030373f4a4163ae1a7dbfe1a51a2c1a51d9100"
 
+[[package]]
+name = "hdrhistogram"
+version = "7.5.4"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "765c9198f173dd59ce26ff9f95ef0aafd0a0fe01fb9d72841bc5066a4c06511d"
+dependencies = [
+ "byteorder",
+ "num-traits",
+]
+
 [[package]]
 name = "heapless"
 version = "0.6.1"
@@ -3892,13 +3931,35 @@ name = "nvsim"
 version = "0.3.0"
 dependencies = [
  "approx 0.5.1",
+ "criterion",
+ "js-sys",
  "rand 0.8.5",
  "rand_chacha 0.3.1",
  "serde",
+ "serde-wasm-bindgen",
  "serde_json",
  "sha2",
  "thiserror 1.0.69",
  "tracing",
+ "wasm-bindgen",
+]
+
+[[package]]
+name = "nvsim-server"
+version = "0.3.0"
+dependencies = [
+ "axum",
+ "clap",
+ "futures-util",
+ "nvsim",
+ "serde",
+ "serde_json",
+ "thiserror 1.0.69",
+ "tokio",
+ "tower 0.4.13",
+ "tower-http 0.5.2",
+ "tracing",
+ "tracing-subscriber",
 ]
 
 [[package]]
@@ -4487,6 +4548,26 @@ dependencies = [
  "siphasher 1.0.2",
 ]
 
+[[package]]
+name = "pin-project"
+version = "1.1.11"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "f1749c7ed4bcaf4c3d0a3efc28538844fb29bcdd7d2b67b2be7e20ba861ff517"
+dependencies = [
+ "pin-project-internal",
+]
+
+[[package]]
+name = "pin-project-internal"
+version = "1.1.11"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "d9b20ed30f105399776b9c883e68e536ef602a16ae6f596d2c473591d6ad64c6"
+dependencies = [
+ "proc-macro2",
+ "quote",
+ "syn 2.0.117",
+]
+
 [[package]]
 name = "pin-project-lite"
 version = "0.2.17"
@@ -5278,7 +5359,7 @@ dependencies = [
  "sync_wrapper 1.0.2",
  "tokio",
  "tokio-native-tls",
- "tower",
+ "tower 0.5.3",
  "tower-http 0.6.8",
  "tower-service",
  "url",
@@ -5311,7 +5392,7 @@ dependencies = [
  "sync_wrapper 1.0.2",
  "tokio",
  "tokio-util",
- "tower",
+ "tower 0.5.3",
  "tower-http 0.6.8",
  "tower-service",
  "url",
@@ -7379,6 +7460,27 @@ dependencies = [
  "zip 0.6.6",
 ]
 
+[[package]]
+name = "tower"
+version = "0.4.13"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "b8fa9be0de6cf49e536ce1851f987bd21a43b771b09473c3549a6c853db37c1c"
+dependencies = [
+ "futures-core",
+ "futures-util",
+ "hdrhistogram",
+ "indexmap 1.9.3",
+ "pin-project",
+ "pin-project-lite",
+ "rand 0.8.5",
+ "slab",
+ "tokio",
+ "tokio-util",
+ "tower-layer",
+ "tower-service",
+ "tracing",
+]
+
 [[package]]
 name = "tower"
 version = "0.5.3"
@@ -7401,8 +7503,10 @@ version = "0.5.2"
 source = "registry+https://github.com/rust-lang/crates.io-index"
 checksum = "1e9cd434a998747dd2c4276bc96ee2e0c7a2eadf3cae88e52be55a05fa9053f5"
 dependencies = [
+ "async-compression",
  "bitflags 2.11.0",
  "bytes",
+ "futures-core",
  "futures-util",
  "http 1.4.0",
  "http-body 1.0.1",
@@ -7433,7 +7537,7 @@ dependencies = [
  "http-body 1.0.1",
  "iri-string",
  "pin-project-lite",
- "tower",
+ "tower 0.5.3",
  "tower-layer",
  "tower-service",
 ]
@@ -8385,6 +8489,7 @@ dependencies = [
  "serde",
  "serde_json",
  "tokio",
+ "tower-http 0.5.2",
 ]
 
 [[package]]
diff --git a/v2/crates/wifi-densepose-sensing-server/Cargo.toml b/v2/crates/wifi-densepose-sensing-server/Cargo.toml
index 0647e8e9d..f754cbc4a 100644
--- a/v2/crates/wifi-densepose-sensing-server/Cargo.toml
+++ b/v2/crates/wifi-densepose-sensing-server/Cargo.toml
@@ -45,10 +45,10 @@ clap = { workspace = true }
 wifi-densepose-wifiscan = { version = "0.3.0", path = "../wifi-densepose-wifiscan" }
 
 # Signal processing with RuvSense pose tracker (accuracy sprint).
-# default-features = false drops the optional ndarray-linalg/BLAS chain so that
-# `--no-default-features` at the workspace root can produce a Windows-friendly
-# build without vcpkg/openblas (issue #366, #415).
-wifi-densepose-signal = { version = "0.3.0", path = "../wifi-densepose-signal", default-features = false }
+# eigenvalue feature enabled on Linux where OpenBLAS is available (/usr/lib/libopenblas.so).
+# default-features = false kept so that `--no-default-features` workspace builds
+# (Windows/CI without vcpkg/openblas) still compile — see issue #366, #415.
+wifi-densepose-signal = { version = "0.3.0", path = "../wifi-densepose-signal", default-features = false, features = ["eigenvalue"] }
 
 [dev-dependencies]
 tempfile = "3.10"
diff --git a/v2/crates/wifi-densepose-sensing-server/src/field_bridge.rs b/v2/crates/wifi-densepose-sensing-server/src/field_bridge.rs
index d6f561067..87b364b26 100644
--- a/v2/crates/wifi-densepose-sensing-server/src/field_bridge.rs
+++ b/v2/crates/wifi-densepose-sensing-server/src/field_bridge.rs
@@ -20,10 +20,11 @@ const ENERGY_THRESH_2: f64 = 12.0;
 const ENERGY_THRESH_3: f64 = 25.0;
 
 /// Create a FieldModelConfig for single-link mode (one ESP32 node = one link).
-/// This avoids the DimensionMismatch error when feeding single-frame observations.
+/// n_subcarriers=64 matches the raw ESP32 CSI frame amplitude vector length.
 pub fn single_link_config() -> FieldModelConfig {
     FieldModelConfig {
         n_links: 1,
+        n_subcarriers: 64,
         ..FieldModelConfig::default()
     }
 }
@@ -83,10 +84,10 @@ pub fn occupancy_or_fallback(
 
 /// Feed the latest frame to the FieldModel during calibration collection.
 ///
-/// Only acts when the model status is `Collecting`. Wraps the latest frame
-/// as a single-link observation (n_links=1) and feeds it.
+/// Acts when status is `Uncalibrated` or `Collecting` — the first feed call
+/// transitions `Uncalibrated` → `Collecting` inside `feed_calibration` itself.
 pub fn maybe_feed_calibration(field: &mut FieldModel, frame_history: &VecDeque<Vec<f64>>) {
-    if field.status() != CalibrationStatus::Collecting {
+    if !matches!(field.status(), CalibrationStatus::Uncalibrated | CalibrationStatus::Collecting) {
         return;
     }
     if let Some(latest) = frame_history.back() {
@@ -129,6 +130,119 @@ pub fn parse_node_positions(input: &str) -> Vec<[f32; 3]> {
 #[cfg(test)]
 mod tests {
     use super::*;
+    use std::collections::VecDeque;
+    use wifi_densepose_signal::ruvsense::field_model::{FieldModel, FieldModelConfig};
+
+    // ── calibration guard fix (issue #496) ───────────────────────────────────
+
+    /// Before the fix, `maybe_feed_calibration` bailed when status==Uncalibrated.
+    /// A freshly created FieldModel starts Uncalibrated, so frame_count stayed 0.
+    #[test]
+    fn test_maybe_feed_calibration_starts_from_uncalibrated() {
+        let mut fm = FieldModel::new(single_link_config()).unwrap();
+        assert_eq!(fm.status(), CalibrationStatus::Uncalibrated);
+
+        let frame = vec![1.0f64; 64];
+        let mut history: VecDeque<Vec<f64>> = VecDeque::new();
+        history.push_back(frame);
+
+        maybe_feed_calibration(&mut fm, &history);
+
+        assert_eq!(fm.status(), CalibrationStatus::Collecting,
+            "status must transition Uncalibrated→Collecting on first feed");
+        assert_eq!(fm.calibration_frame_count(), 1,
+            "frame_count must be 1 after one feed");
+    }
+
+    /// Feeding multiple frames increments the counter correctly.
+    #[test]
+    fn test_maybe_feed_calibration_accumulates_frames() {
+        let mut fm = FieldModel::new(single_link_config()).unwrap();
+        let mut history: VecDeque<Vec<f64>> = VecDeque::new();
+
+        for i in 0..10u32 {
+            let frame: Vec<f64> = (0..64).map(|j| (i * 64 + j) as f64).collect();
+            history.push_back(frame);
+            maybe_feed_calibration(&mut fm, &history);
+        }
+
+        assert_eq!(fm.calibration_frame_count(), 10);
+    }
+
+    /// `maybe_feed_calibration` must NOT run when status is Fresh (calibrated).
+    #[test]
+    fn test_maybe_feed_calibration_skips_when_fresh() {
+        // Build a model that can finalize after just 5 frames.
+        let cfg = FieldModelConfig {
+            n_links: 1,
+            n_subcarriers: 64,
+            min_calibration_frames: 5,
+            ..FieldModelConfig::default()
+        };
+        let mut fm = FieldModel::new(cfg).unwrap();
+
+        // Feed 5 frames directly to reach Collecting state.
+        for _ in 0..5 {
+            fm.feed_calibration(&[vec![0.5f64; 64]]).unwrap();
+        }
+        fm.finalize_calibration(1_000_000, 0xDEAD).unwrap();
+        assert_eq!(fm.status(), CalibrationStatus::Fresh);
+
+        let count_before = fm.calibration_frame_count();
+
+        // Now call maybe_feed_calibration — it must be a no-op when Fresh.
+        let mut history: VecDeque<Vec<f64>> = VecDeque::new();
+        history.push_back(vec![1.0f64; 64]);
+        maybe_feed_calibration(&mut fm, &history);
+
+        assert_eq!(fm.calibration_frame_count(), count_before,
+            "maybe_feed_calibration must not increment frame_count when status is Fresh");
+    }
+
+    // ── subcarrier count fix (issue #496) ────────────────────────────────────
+
+    /// single_link_config must use 64 subcarriers to match ESP32 CSI frame size.
+    /// Before the fix it used the default (56), causing DimensionMismatch on every feed.
+    #[test]
+    fn test_single_link_config_has_64_subcarriers() {
+        let cfg = single_link_config();
+        assert_eq!(cfg.n_subcarriers, 64,
+            "n_subcarriers must be 64 to match ESP32 CSI amplitude vector length");
+        assert_eq!(cfg.n_links, 1);
+    }
+
+    /// Feeding a 64-element frame must succeed (no DimensionMismatch).
+    #[test]
+    fn test_feed_64_subcarrier_frame_succeeds() {
+        let mut fm = FieldModel::new(single_link_config()).unwrap();
+        let frame = vec![0.5f64; 64];
+        let mut history = VecDeque::new();
+        history.push_back(frame);
+
+        maybe_feed_calibration(&mut fm, &history);
+
+        assert_eq!(fm.calibration_frame_count(), 1,
+            "a 64-subcarrier frame must be accepted without DimensionMismatch");
+    }
+
+    /// Feeding a 56-element frame (old default) must NOT increment frame_count.
+    #[test]
+    fn test_feed_56_subcarrier_frame_rejected() {
+        let mut fm = FieldModel::new(single_link_config()).unwrap();
+        // Status starts Uncalibrated — force it to Collecting first with a good frame
+        let good_frame = vec![0.5f64; 64];
+        let mut history = VecDeque::new();
+        history.push_back(good_frame);
+        maybe_feed_calibration(&mut fm, &history);
+        assert_eq!(fm.calibration_frame_count(), 1);
+
+        // Now push a short (56-elem) frame — must be silently dropped, not crash
+        history.clear();
+        history.push_back(vec![0.5f64; 56]);
+        maybe_feed_calibration(&mut fm, &history);
+        assert_eq!(fm.calibration_frame_count(), 1,
+            "56-subcarrier frame must be rejected — count must not increase");
+    }
 
     #[test]
     fn test_parse_node_positions() {