Presa-Mano/web/servo.js

/* Visual-servo tab: live Nuwa RGB with marker + object-centre overlays.
 * Each click: plan → render overlay + stats → user confirms → step → wait →
 * re-plan → render. No 3D viewer in this tab — it's an image-space tool.
 */

import * as THREE from 'three';
import { OrbitControls } from 'three/addons/controls/OrbitControls.js';

const $ = (id) => document.getElementById(id);
const LS_KEY = 'presaMano.servoParams.v1';
const DEG = Math.PI / 180;

// ----- 3D scene (camera-frame) ----------------------------------------------
let scene3d = null, cam3d = null, renderer3d = null, controls3d = null;
let scene3dObjs = {};   // named overlays to clear/update on each plan

function boot3d() {
  if (scene3d) return;
  const canvas = $('servo3dCanvas');
  if (!canvas) return;
  scene3d = new THREE.Scene();
  scene3d.background = new THREE.Color(0x0a0d11);
  // Camera looks from outside the scene; world frame == Nuwa camera frame
  // (OpenCV: +X right, +Y down, +Z forward). We orbit around the origin.
  cam3d = new THREE.PerspectiveCamera(45, canvas.width / canvas.height, 0.05, 20);
  cam3d.position.set(0.8, -0.6, -0.2);
  cam3d.up.set(0, -1, 0);                // Y-down so CV axes match visually
  renderer3d = new THREE.WebGLRenderer({ canvas, antialias: true });
  renderer3d.setPixelRatio(window.devicePixelRatio || 1);
  renderer3d.setSize(canvas.width, canvas.height, false);
  controls3d = new OrbitControls(cam3d, canvas);
  controls3d.target.set(0, 0, 1.2);
  controls3d.update();

  scene3d.add(new THREE.AmbientLight(0xffffff, 0.6));
  const dir = new THREE.DirectionalLight(0xffffff, 0.6);
  dir.position.set(1, -1, 1);
  scene3d.add(dir);

  // Camera-origin marker (small axes triad)
  const ax = new THREE.AxesHelper(0.2);
  scene3d.add(ax);
  // Small grid at Z=1.5 to anchor the eye when there's no plane yet.
  const grid = new THREE.GridHelper(2, 10, 0x222a33, 0x1a2029);
  grid.rotation.x = Math.PI / 2;
  grid.position.z = 1.5;
  scene3d.add(grid);

  // Frustum wireframe (approximate FOV lines up to 2 m depth).
  const fh = 0.7, fw = 1.0, fz = 2.0;
  const frPts = [
    0,0,0,  fw,  fh, fz,
    0,0,0, -fw,  fh, fz,
    0,0,0,  fw, -fh, fz,
    0,0,0, -fw, -fh, fz,
    fw,  fh, fz,  fw, -fh, fz,
    fw, -fh, fz, -fw, -fh, fz,
   -fw, -fh, fz, -fw,  fh, fz,
   -fw,  fh, fz,  fw,  fh, fz,
  ];
  const frGeo = new THREE.BufferGeometry();
  frGeo.setAttribute('position', new THREE.Float32BufferAttribute(frPts, 3));
  const frMat = new THREE.LineBasicMaterial({ color: 0x2a3340 });
  scene3d.add(new THREE.LineSegments(frGeo, frMat));

  animate3d();
}

function animate3d() {
  requestAnimationFrame(animate3d);
  if (controls3d) controls3d.update();
  if (renderer3d && scene3d && cam3d) renderer3d.render(scene3d, cam3d);
}

function clear3dOverlays() {
  for (const k of Object.keys(scene3dObjs)) {
    const o = scene3dObjs[k];
    if (!o) continue;
    scene3d.remove(o);
    if (o.geometry) o.geometry.dispose();
    if (o.material) (Array.isArray(o.material) ? o.material : [o.material]).forEach(m => m.dispose());
    scene3dObjs[k] = null;
  }
}

function addSphere3d(key, xyz, color, radius = 0.025) {
  const geom = new THREE.SphereGeometry(radius, 24, 16);
  const mat = new THREE.MeshStandardMaterial({ color, emissive: color, emissiveIntensity: 0.25 });
  const m = new THREE.Mesh(geom, mat);
  m.position.set(xyz[0], xyz[1], xyz[2]);
  scene3d.add(m);
  scene3dObjs[key] = m;
}

function addLine3d(key, a, b, color) {
  const g = new THREE.BufferGeometry().setFromPoints([
    new THREE.Vector3(...a), new THREE.Vector3(...b),
  ]);
  const m = new THREE.Line(g, new THREE.LineBasicMaterial({ color }));
  scene3d.add(m);
  scene3dObjs[key] = m;
}

function addPlaneMesh3d(key, normal, offset, anchor, size = 1.2, color = 0x9b6dff) {
  const n = new THREE.Vector3(...normal).normalize();
  // Build two in-plane orthonormal vectors.
  const up = Math.abs(n.y) < 0.9 ? new THREE.Vector3(0, 1, 0) : new THREE.Vector3(1, 0, 0);
  const u = new THREE.Vector3().crossVectors(up, n).normalize();
  const v = new THREE.Vector3().crossVectors(n, u).normalize();
  // Centre the mesh on the anchor point (foot of camera onto plane if given).
  const anchorVec = new THREE.Vector3(...anchor);
  // Pull anchor onto the plane to be safe.
  const s = anchorVec.dot(n) + offset;
  anchorVec.addScaledVector(n, -s);
  const geom = new THREE.BufferGeometry();
  const h = size / 2;
  const corners = [
    anchorVec.clone().addScaledVector(u, -h).addScaledVector(v, -h),
    anchorVec.clone().addScaledVector(u,  h).addScaledVector(v, -h),
    anchorVec.clone().addScaledVector(u,  h).addScaledVector(v,  h),
    anchorVec.clone().addScaledVector(u, -h).addScaledVector(v,  h),
  ];
  const pos = [];
  for (const c of corners) pos.push(c.x, c.y, c.z);
  geom.setAttribute('position', new THREE.Float32BufferAttribute(pos, 3));
  geom.setIndex([0, 1, 2, 0, 2, 3]);
  geom.computeVertexNormals();
  const mat = new THREE.MeshStandardMaterial({
    color, opacity: 0.35, transparent: true, side: THREE.DoubleSide,
  });
  const mesh = new THREE.Mesh(geom, mat);
  scene3d.add(mesh);
  scene3dObjs[key] = mesh;
  // Edges
  const edgeGeom = new THREE.EdgesGeometry(geom);
  const edgeMat = new THREE.LineBasicMaterial({ color });
  const edges = new THREE.LineSegments(edgeGeom, edgeMat);
  scene3d.add(edges);
  scene3dObjs[key + '_edges'] = edges;
}

function redraw3d() {
  if (!scene3d) return;
  clear3dOverlays();
  const p = lastPlan && lastPlan.plan;
  // Plane (from stored planeInfo, independent of current plan)
  if (planeInfo) {
    const anchor = planeInfo.centroid || [0, 0, 1.5];
    addPlaneMesh3d('plane', planeInfo.normal, planeInfo.offset, anchor);
  }
  if (!p) return;
  addSphere3d('marker', p.marker_position_cam, 0x4dd0e1, 0.022);
  addSphere3d('object', p.object_position_cam, 0xffd54f, 0.028);
  addLine3d('mo_line', p.marker_position_cam, p.object_position_cam, 0xbbbbbb);
  if (p.plane) {
    addSphere3d('marker_foot', p.plane.marker_foot_cam, 0x80ecf7, 0.018);
    addSphere3d('object_foot', p.plane.object_foot_cam, 0xffe998, 0.020);
    addLine3d('m_drop', p.marker_position_cam, p.plane.marker_foot_cam, 0x4dd0e1);
    addLine3d('o_drop', p.object_position_cam, p.plane.object_foot_cam, 0xffd54f);
    addLine3d('foot_line', p.plane.marker_foot_cam, p.plane.object_foot_cam, 0x9b6dff);
  }
}

let lastPlan = null;
let busy = false;
let imgRefreshTimer = null;
// Image native dimensions (from /api/intrinsics or from the first load).
let imgW = 0, imgH = 0;
// Pan/zoom state: scale and translate applied to the inner pan-zoom wrapper.
const view = { scale: 1, tx: 0, ty: 0 };
let drag = null;
// Work plane selection state.
let planeInfo = null;             // last response from /plane/fit or /plane GET
let roiMode = false;              // true when user clicked "Seleziona area"
let roi = null;                   // {u0,v0,u1,v1} in image coords (committed)
let roiDrag = null;               // in-progress drag

function setStatus(msg, cls) {
  const el = $('servoStatus');
  if (!el) return;
  el.textContent = msg;
  el.style.color = cls === 'err' ? 'var(--danger)' : cls === 'ok' ? 'var(--ok)' : 'var(--dim)';
}

function readParams() {
  const side = $('svSide').value || null;
  const cls = $('svClass').value.trim();
  const conf = $('svConf').value.trim();
  const body = {
    min_distance_m: (+$('svMinDist').value) / 1000,
    step_fraction: +$('svStepFrac').value,
  };
  if (cls) body.class_name = cls;
  if (conf) body.conf = +conf;
  if (side) body.side = side;
  const rx = +$('svRx').value, ry = +$('svRy').value, rz = +$('svRz').value;
  if (rx || ry || rz) {
    body.wrist_marker_rvec = [rx * DEG, ry * DEG, rz * DEG];
  }
  return body;
}

function persistParams() {
  try {
    localStorage.setItem(LS_KEY, JSON.stringify({
      cls: $('svClass').value, conf: $('svConf').value,
      minDist: $('svMinDist').value, step: $('svStepFrac').value,
      timeout: $('svTimeout').value, side: $('svSide').value,
      dryRun: $('svDryRun').checked,
      rx: $('svRx').value, ry: $('svRy').value, rz: $('svRz').value,
    }));
  } catch {}
}

function restoreParams() {
  try {
    const raw = localStorage.getItem(LS_KEY);
    if (!raw) return;
    const p = JSON.parse(raw);
    const setV = (id, v) => { if ($(id) && v != null) $(id).value = v; };
    setV('svClass', p.cls); setV('svConf', p.conf);
    setV('svMinDist', p.minDist); setV('svStepFrac', p.step);
    setV('svTimeout', p.timeout); setV('svSide', p.side);
    setV('svRx', p.rx); setV('svRy', p.ry); setV('svRz', p.rz);
    if ($('svDryRun') && p.dryRun != null) $('svDryRun').checked = !!p.dryRun;
  } catch {}
}

async function api(method, path, body = null) {
  const opts = { method, headers: {} };
  if (body !== null) {
    opts.headers['Content-Type'] = 'application/json';
    opts.body = JSON.stringify(body);
  }
  const r = await fetch(path, opts);
  const text = await r.text();
  let data; try { data = JSON.parse(text); } catch { data = text; }
  if (!r.ok) throw new Error(`${r.status} ${(data && data.detail) || text}`);
  return data;
}

async function saveRvecToConfig() {
  const rx = +$('svRx').value, ry = +$('svRy').value, rz = +$('svRz').value;
  const rvec = [rx * DEG, ry * DEG, rz * DEG];
  try {
    await api('PUT', '/api/config', {
      grasp: { visual_servo: { wrist_marker_rvec: rvec } },
    });
    setStatus(`rvec salvato: [${rx}°, ${ry}°, ${rz}°]`, 'ok');
  } catch (e) {
    setStatus('salvataggio rvec fallito: ' + e.message, 'err');
  }
}

function refreshImg() {
  const img = $('servoImg');
  if (!img) return;
  // Cache-bust so we always see the current frame.
  img.src = `/api/calibration/rgb.jpg?t=${Date.now()}`;
}

function resizeOverlay() {
  const img = $('servoImg');
  const cvs = $('servoOverlay');
  if (!img || !cvs) return;
  if (img.naturalWidth) { imgW = img.naturalWidth; imgH = img.naturalHeight; }
  if (!imgW || !imgH) return;
  // Match canvas pixel size to the native image size; CSS transform on the
  // wrapper handles zoom/pan, keeping overlay and image perfectly aligned.
  const dpr = window.devicePixelRatio || 1;
  cvs.width = Math.round(imgW * dpr);
  cvs.height = Math.round(imgH * dpr);
  cvs.style.width = imgW + 'px';
  cvs.style.height = imgH + 'px';
  img.style.width = imgW + 'px';
  img.style.height = imgH + 'px';
  // Fit-to-viewport on first sizing.
  if (view.scale === 1 && view.tx === 0 && view.ty === 0) fitView();
  else applyTransform();
  redrawOverlay();
}

function applyTransform() {
  const wrap = $('servoPanZoom');
  if (!wrap) return;
  wrap.style.transform = `translate(${view.tx}px, ${view.ty}px) scale(${view.scale})`;
}

function fitView() {
  const vp = $('servoViewport');
  if (!vp || !imgW || !imgH) return;
  const r = vp.getBoundingClientRect();
  const s = Math.min(r.width / imgW, r.height / imgH);
  view.scale = s;
  view.tx = (r.width - imgW * s) / 2;
  view.ty = (r.height - imgH * s) / 2;
  applyTransform();
}

function zoomAt(clientX, clientY, factor) {
  const vp = $('servoViewport');
  const r = vp.getBoundingClientRect();
  const px = clientX - r.left, py = clientY - r.top;
  // Image coords under the cursor BEFORE scaling:
  const ix = (px - view.tx) / view.scale;
  const iy = (py - view.ty) / view.scale;
  const newScale = Math.min(16, Math.max(0.1, view.scale * factor));
  // Keep (ix, iy) under the cursor after the new scale.
  view.tx = px - ix * newScale;
  view.ty = py - iy * newScale;
  view.scale = newScale;
  applyTransform();
}

function clientToImage(clientX, clientY) {
  const vp = $('servoViewport');
  const r = vp.getBoundingClientRect();
  const px = clientX - r.left, py = clientY - r.top;
  return [(px - view.tx) / view.scale, (py - view.ty) / view.scale];
}

function wireViewport() {
  const vp = $('servoViewport');
  if (!vp) return;
  vp.addEventListener('wheel', (e) => {
    e.preventDefault();
    const factor = e.deltaY < 0 ? 1.15 : 1 / 1.15;
    zoomAt(e.clientX, e.clientY, factor);
  }, { passive: false });
  vp.addEventListener('pointerdown', (e) => {
    // ROI selection takes priority when enabled, OR when user holds Shift.
    if (roiMode || e.shiftKey) {
      const [ix, iy] = clientToImage(e.clientX, e.clientY);
      roiDrag = { u0: ix, v0: iy, u1: ix, v1: iy, id: e.pointerId };
      vp.setPointerCapture(e.pointerId);
      redrawOverlay();
      return;
    }
    drag = { x: e.clientX, y: e.clientY, tx: view.tx, ty: view.ty };
    vp.setPointerCapture(e.pointerId);
    vp.style.cursor = 'grabbing';
  });
  vp.addEventListener('pointermove', (e) => {
    if (roiDrag) {
      const [ix, iy] = clientToImage(e.clientX, e.clientY);
      roiDrag.u1 = ix; roiDrag.v1 = iy;
      redrawOverlay();
      return;
    }
    if (!drag) return;
    view.tx = drag.tx + (e.clientX - drag.x);
    view.ty = drag.ty + (e.clientY - drag.y);
    applyTransform();
  });
  const endDrag = (e) => {
    if (roiDrag) {
      try { vp.releasePointerCapture(e.pointerId); } catch {}
      const r = roiDrag;
      const u0 = Math.max(0, Math.min(r.u0, r.u1));
      const v0 = Math.max(0, Math.min(r.v0, r.v1));
      const u1 = Math.min(imgW || r.u1, Math.max(r.u0, r.u1));
      const v1 = Math.min(imgH || r.v1, Math.max(r.v0, r.v1));
      if (u1 - u0 > 5 && v1 - v0 > 5) {
        roi = { u0: u0|0, v0: v0|0, u1: u1|0, v1: v1|0 };
      }
      roiDrag = null;
      roiMode = false;
      vp.style.cursor = 'grab';
      setStatus(roi ? `ROI ${roi.u1-roi.u0}x${roi.v1-roi.v0} px · premi "Calcola piano"` : 'ROI troppo piccola', roi ? 'ok' : 'err');
      redrawOverlay();
      return;
    }
    if (drag) { try { vp.releasePointerCapture(e.pointerId); } catch {} }
    drag = null;
    vp.style.cursor = 'grab';
  };
  vp.addEventListener('pointerup', endDrag);
  vp.addEventListener('pointercancel', endDrag);
  vp.addEventListener('dblclick', () => fitView());
}

function drawCross(ctx, x, y, color) {
  // Sizes expressed in IMAGE pixels so they stay legible after CSS zoom.
  const r = 18;
  ctx.strokeStyle = color;
  ctx.lineWidth = 3;
  ctx.beginPath();
  ctx.arc(x, y, r, 0, Math.PI * 2);
  ctx.stroke();
  ctx.beginPath();
  ctx.moveTo(x - r - 6, y); ctx.lineTo(x + r + 6, y);
  ctx.moveTo(x, y - r - 6); ctx.lineTo(x, y + r + 6);
  ctx.stroke();
}

function drawRect(ctx, u0, v0, u1, v1, color, label) {
  ctx.strokeStyle = color;
  ctx.lineWidth = 2;
  ctx.setLineDash([6, 4]);
  ctx.strokeRect(u0, v0, u1 - u0, v1 - v0);
  ctx.setLineDash([]);
  ctx.fillStyle = 'rgba(0,0,0,0.35)';
  ctx.fillRect(u0, v0, u1 - u0, v1 - v0);
  if (label) {
    ctx.font = 'bold 16px ui-monospace, monospace';
    const w = ctx.measureText(label).width + 10;
    ctx.fillStyle = color;
    ctx.fillRect(u0, v0, w, 20);
    ctx.fillStyle = '#000';
    ctx.fillText(label, u0 + 5, v0 + 15);
  }
}

function redrawOverlay() {
  const cvs = $('servoOverlay');
  if (!cvs) return;
  const ctx = cvs.getContext('2d');
  const dpr = window.devicePixelRatio || 1;
  ctx.setTransform(dpr, 0, 0, dpr, 0, 0);
  ctx.clearRect(0, 0, imgW || cvs.width, imgH || cvs.height);
  if (!imgW || !imgH) return;

  // Committed ROI
  if (roi) drawRect(ctx, roi.u0, roi.v0, roi.u1, roi.v1, '#9b6dff', 'ROI piano');
  // In-progress drag
  if (roiDrag) {
    const u0 = Math.min(roiDrag.u0, roiDrag.u1);
    const v0 = Math.min(roiDrag.v0, roiDrag.v1);
    const u1 = Math.max(roiDrag.u0, roiDrag.u1);
    const v1 = Math.max(roiDrag.v0, roiDrag.v1);
    drawRect(ctx, u0, v0, u1, v1, '#9b6dff', null);
  }

  if (!lastPlan) return;
  const p = lastPlan.plan;
  const mx = p.marker_pixel[0], my = p.marker_pixel[1];
  const ox = p.object_pixel[0], oy = p.object_pixel[1];
  ctx.strokeStyle = 'rgba(255,255,255,0.6)';
  ctx.setLineDash([8, 6]);
  ctx.lineWidth = 2;
  ctx.beginPath(); ctx.moveTo(mx, my); ctx.lineTo(ox, oy); ctx.stroke();
  ctx.setLineDash([]);
  drawCross(ctx, mx, my, '#4dd0e1');
  drawCross(ctx, ox, oy, '#ffd54f');
}

async function planeSelectStart() {
  roiMode = true;
  setStatus('traccia un rettangolo sull\'immagine (shift+drag o dopo questo click, drag)', 'ok');
}

async function planeFit() {
  if (!roi) { setStatus('seleziona prima un\'area', 'err'); return; }
  try {
    setStatus('fit piano…');
    const body = {
      u_min: roi.u0, v_min: roi.v0,
      u_max: roi.u1, v_max: roi.v1,
    };
    const r = await api('POST', '/api/grasp/visual_servo/plane/fit', body);
    planeInfo = r.plane;
    updatePlaneInfo();
    redraw3d();
    setStatus(`piano OK · rms=${r.plane.fit_rms_mm.toFixed(1)} mm · ${r.plane.n_inliers} punti`, 'ok');
  } catch (e) {
    setStatus('fit fallito: ' + e.message, 'err');
  }
}

async function planeClear() {
  try {
    await api('DELETE', '/api/grasp/visual_servo/plane');
    planeInfo = null;
    roi = null;
    updatePlaneInfo();
    redrawOverlay();
    redraw3d();
    setStatus('piano rimosso', 'ok');
  } catch (e) {
    setStatus('rimozione piano fallita: ' + e.message, 'err');
  }
}

function updatePlaneInfo() {
  const el = $('svPlaneInfo');
  if (!el) return;
  if (!planeInfo) { el.textContent = 'piano: —'; return; }
  const n = planeInfo.normal.map(v => v.toFixed(3)).join(', ');
  el.innerHTML = `piano: n=[${n}] · offset=${planeInfo.offset.toFixed(3)} m · rms=${planeInfo.fit_rms_mm.toFixed(1)} mm · ${planeInfo.n_inliers} pts`;
}

async function planeLoadCurrent() {
  try {
    const r = await api('GET', '/api/grasp/visual_servo/plane');
    if (r.set) {
      planeInfo = r.plane;
      roi = { u0: r.plane.roi[0], v0: r.plane.roi[1], u1: r.plane.roi[2], v1: r.plane.roi[3] };
      updatePlaneInfo();
      redrawOverlay();
      redraw3d();
    }
  } catch {}
}

function fmtMm(m) { return (m * 1000).toFixed(1) + ' mm'; }

function renderStats(resp) {
  const p = resp.plan;
  const cls = p.detection && p.detection.class;
  const conf = p.detection && p.detection.confidence;
  const reproj = p.marker_reprojection_error_px;
  const doneBadge = p.done
    ? '<span style="color:var(--ok);font-weight:600">RAGGIUNTO</span>'
    : '<span>in avvicinamento</span>';
  $('servoStats').innerHTML = `
    <table class="kvtable">
      <tbody>
        <tr><td>stato</td><td>${doneBadge}</td></tr>
        <tr><td>marker · pixel</td><td>(${p.marker_pixel[0].toFixed(0)}, ${p.marker_pixel[1].toFixed(0)})</td></tr>
        <tr><td>marker · Z / dist / scala</td><td>${fmtMm(p.marker_depth_z_m)} · ${fmtMm(p.marker_distance_cam_m)} · ${p.marker_mm_per_px ? p.marker_mm_per_px.toFixed(2) + ' mm/px' : '—'}</td></tr>
        <tr><td>oggetto · pixel</td><td>(${p.object_pixel[0].toFixed(0)}, ${p.object_pixel[1].toFixed(0)})</td></tr>
        <tr><td>oggetto · Z / dist / scala</td><td>${fmtMm(p.object_depth_z_m)} · ${fmtMm(p.object_distance_cam_m)} · ${p.object_mm_per_px ? p.object_mm_per_px.toFixed(2) + ' mm/px' : '—'}</td></tr>
        <tr><td>marker ↔ oggetto (3D)</td><td><strong>${fmtMm(p.distance_m)}</strong></td></tr>
        ${p.plane ? `
          <tr><td>piano · rms / punti</td><td>${p.plane.fit_rms_mm.toFixed(1)} mm · ${planeInfo ? planeInfo.n_inliers : '—'}</td></tr>
          <tr><td>marker · altezza dal piano</td><td><strong>${(p.plane.marker_signed_distance_m >= 0 ? '+' : '−')}${fmtMm(Math.abs(p.plane.marker_signed_distance_m))}</strong> <span class="dim">(${p.plane.marker_signed_distance_m >= 0 ? 'sopra' : 'sotto'})</span></td></tr>
          <tr><td>oggetto · altezza dal piano</td><td><strong>${(p.plane.object_signed_distance_m >= 0 ? '+' : '−')}${fmtMm(Math.abs(p.plane.object_signed_distance_m))}</strong> <span class="dim">(${p.plane.object_signed_distance_m >= 0 ? 'sopra' : 'sotto'})</span></td></tr>
          <tr><td>Δ altezza marker−oggetto</td><td>${fmtMm(Math.abs(p.plane.marker_signed_distance_m - p.plane.object_signed_distance_m))}</td></tr>
          <tr><td>in-piano (perp-foot)</td><td><strong>${fmtMm(p.plane.in_plane_distance_m)}</strong></td></tr>
          <tr><td>in-piano (pixel-hit)</td><td>${fmtMm(p.plane.in_plane_pixel_distance_m)}</td></tr>
        ` : ''}
        <tr><td>prossimo step (base)</td><td>${p.target_position_base ? fmtMm(p.planned_step_m) : '—'}</td></tr>
        <tr><td>limite stop</td><td>${fmtMm(resp.min_distance_m)}</td></tr>
        <tr><td>frazione step</td><td>${resp.step_fraction}</td></tr>
        <tr><td>lato</td><td>${resp.side}</td></tr>
        <tr><td>oggetto · classe</td><td>${cls || '—'} ${conf != null ? `(${(conf*100).toFixed(0)}%)` : ''}</td></tr>
        <tr><td>punti bbox</td><td>${p.n_object_points}</td></tr>
        <tr><td>reproj err marker</td><td>${reproj != null ? reproj.toFixed(2) + ' px' : '—'}</td></tr>
        <tr><td>sorgente posizione marker</td><td>${p.marker_translation_source || '—'}</td></tr>
        <tr><td>sorgente punto oggetto</td><td>${p.object_point_source || '—'}</td></tr>
        <tr><td>note</td><td>${p.reason || '—'}</td></tr>
      </tbody>
    </table>`;
  $('servoResult').textContent = JSON.stringify({
    wrist_position_base: p.wrist_position_base,
    marker_position_cam: p.marker_position_cam,
    object_position_cam: p.object_position_cam,
    target_position_base: p.target_position_base,
    distance_m: p.distance_m,
  }, null, 2);
}

async function doPlan() {
  if (busy) return;
  busy = true;
  persistParams();
  try {
    setStatus('rilevamento…');
    refreshImg();
    const body = readParams();
    const resp = await api('POST', '/api/grasp/visual_servo/plan', body);
    lastPlan = resp;
    renderStats(resp);
    redrawOverlay();
    redraw3d();
    setStatus(resp.plan.done ? 'target raggiunto' : 'piano pronto · premi "Vai al prossimo"', 'ok');
  } catch (e) {
    setStatus('errore plan: ' + e.message, 'err');
  } finally { busy = false; }
}

async function doStep() {
  if (busy) return;
  busy = true;
  persistParams();
  const dryRun = $('svDryRun').checked;
  try {
    setStatus(dryRun ? 'step (dry-run)…' : 'movimento in corso…');
    refreshImg();
    const body = readParams();
    body.dry_run = dryRun;
    body.timeout_s = +$('svTimeout').value;
    const resp = await api('POST', '/api/grasp/visual_servo/step', body);
    lastPlan = resp;
    renderStats(resp);
    redrawOverlay();
    if (!resp.moved) {
      setStatus('target raggiunto, nessun moto', 'ok');
      return;
    }
    setStatus('ricalcolo post-moto…');
    refreshImg();
    const replan = await api('POST', '/api/grasp/visual_servo/plan', readParams());
    lastPlan = replan;
    renderStats(replan);
    redrawOverlay();
    redraw3d();
    setStatus(replan.plan.done ? 'target raggiunto' : 'pronto per il prossimo step', 'ok');
  } catch (e) {
    setStatus('errore step: ' + e.message, 'err');
  } finally { busy = false; }
}

function doReset() {
  lastPlan = null;
  const ctx = $('servoOverlay').getContext('2d');
  ctx.clearRect(0, 0, $('servoOverlay').width, $('servoOverlay').height);
  $('servoStats').innerHTML = '<div class="dim">nessun piano calcolato</div>';
  $('servoResult').textContent = '—';
  setStatus('reset');
}

function startLiveImg() {
  stopLiveImg();
  imgRefreshTimer = setInterval(refreshImg, 500);
  refreshImg();
}
function stopLiveImg() {
  if (imgRefreshTimer) { clearInterval(imgRefreshTimer); imgRefreshTimer = null; }
}

document.addEventListener('DOMContentLoaded', () => {
  if (!$('servoImg')) return;
  restoreParams();
  ['svClass','svConf','svMinDist','svStepFrac','svTimeout','svSide','svDryRun',
   'svRx','svRy','svRz']
    .forEach(id => { const el = $(id); if (el) el.addEventListener('change', persistParams); });
  $('svPlanBtn').addEventListener('click', doPlan);
  $('svStepBtn').addEventListener('click', doStep);
  $('svResetBtn').addEventListener('click', doReset);
  $('svSaveRvecBtn').addEventListener('click', saveRvecToConfig);
  $('svPlaneSelBtn').addEventListener('click', planeSelectStart);
  $('svPlaneFitBtn').addEventListener('click', planeFit);
  $('svPlaneClearBtn').addEventListener('click', planeClear);
  boot3d();
  planeLoadCurrent();

  const img = $('servoImg');
  img.addEventListener('load', () => {
    imgW = img.naturalWidth; imgH = img.naturalHeight;
    resizeOverlay();
  });
  wireViewport();
  window.addEventListener('resize', () => { resizeOverlay(); });
});

function resize3d() {
  const canvas = $('servo3dCanvas');
  if (!canvas || !renderer3d || !cam3d) return;
  const rect = canvas.getBoundingClientRect();
  const w = Math.max(100, Math.round(rect.width));
  const h = Math.max(60, Math.round(rect.height));
  renderer3d.setSize(w, h, false);
  cam3d.aspect = w / h;
  cam3d.updateProjectionMatrix();
}

// Only stream the RGB while the servo tab is active (avoid hammering Nuwa).
document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
  const active = btn.dataset.tab === 'servo';
  if (active) {
    startLiveImg();
    // The 3D canvas may have rendered at 0x0 while the panel was display:none.
    // Re-boot (no-op if already ready) and resize once visible.
    boot3d();
    setTimeout(() => { resizeOverlay(); resize3d(); redraw3d(); }, 100);
  } else { stopLiveImg(); }
}));
window.addEventListener('resize', () => { resize3d(); });