Presa-Mano/web/autocal3d.js

/* Auto-calibration tab: reuses G1Viewer for the 3D URDF, overlays the
 * calibration waypoints as colored spheres + triads (target orientation),
 * orchestrates Calcola / Verifica reale / Start / Stop flow, polls
 * /api/calibration/auto/status during execution, shows a per-waypoint attempt
 * bar chart, and persists form settings in localStorage so the user can
 * re-run the same calibration without re-typing.
 */

import * as THREE from 'three';
import { G1Viewer } from './diagnostic3d.js';

const $ = (id) => document.getElementById(id);

const STATE_COLORS = {
  pending:   0xaaaaaa,
  unreach:   0xe74c3c,
  skipped:   0x8a3a33,
  current:   0x4da3ff,
  done:      0x3ecf8e,
  failed:    0xf0ad4e,
};

const LS_KEY = 'presaMano.autocalParams.v1';

let viewer = null;
let booted = false;
let pollTimer = null;
let waypointGroup = null;
let waypointItems = new Map();  // idx -> {sphere, triad}
let hoverTooltip = null;
let lastSnapshot = null;
let showTriads = true;

async function boot() {
  if (booted) return;
  booted = true;
  const canvas = $('autocalCanvas');
  if (!canvas) return;
  $('autocalStatus').textContent = 'caricamento URDF…';
  try {
    viewer = new G1Viewer(canvas);
    await viewer.load();
    // Hide the per-motor spheres here — user doesn't want them in this tab.
    viewer.setShowSpheres(false);
    $('autocalStatus').textContent = 'URDF caricato · imposta parametri e clicca Calcola griglia';
    waypointGroup = new THREE.Group();
    waypointGroup.name = 'autocal:waypoints';
    viewer.scene.add(waypointGroup);
    installHoverTooltip();
    // Expose hook so app.js powerPollOnce can push /diagnostics (live robot motion).
    window.g1AutocalViewer = viewer;
    await previewGrid().catch(() => {});
  } catch (e) {
    $('autocalStatus').textContent = 'errore: ' + e.message;
    console.error(e);
  }
}

function installHoverTooltip() {
  hoverTooltip = document.createElement('div');
  hoverTooltip.className = 'diag3d-tooltip hidden';
  hoverTooltip.id = 'autocalTooltip';
  viewer.canvas.parentElement.appendChild(hoverTooltip);
  const raycaster = new THREE.Raycaster();
  const mouse = new THREE.Vector2();
  viewer.canvas.addEventListener('pointermove', (ev) => {
    if (!waypointItems.size) { hoverTooltip.classList.add('hidden'); return; }
    const rect = viewer.canvas.getBoundingClientRect();
    mouse.x = ((ev.clientX - rect.left) / rect.width) * 2 - 1;
    mouse.y = -((ev.clientY - rect.top) / rect.height) * 2 + 1;
    raycaster.setFromCamera(mouse, viewer.camera);
    const spheres = [...waypointItems.values()].map(it => it.sphere);
    const hits = raycaster.intersectObjects(spheres, false);
    if (!hits.length) { hoverTooltip.classList.add('hidden'); return; }
    const w = hits[0].object.userData.waypoint;
    hoverTooltip.className = 'diag3d-tooltip ' +
      (w.state === 'failed' || !w.reachable ? 'err' : '');
    const rows = [
      ['idx', '#' + w.idx],
      ['stato', w.state + (!w.reachable ? ' (unreach)' : '')],
      ['x (m)', Number(w.position[0]).toFixed(3)],
      ['y (m)', Number(w.position[1]).toFixed(3)],
      ['z (m)', Number(w.position[2]).toFixed(3)],
    ];
    if (w.attempts) rows.push(['attempts', w.attempts]);
    if (w.error) rows.push(['error', w.error.slice(0, 60)]);
    if (w.reason) rows.push(['motivo', w.reason]);
    hoverTooltip.innerHTML = `<div class="tt-title">waypoint #${w.idx}</div>` +
      rows.map(([k, v]) => `<div class="tt-row"><span class="k">${k}</span><span>${v}</span></div>`).join('');
    hoverTooltip.style.left = (ev.clientX - rect.left + 14) + 'px';
    hoverTooltip.style.top = (ev.clientY - rect.top + 14) + 'px';
    hoverTooltip.classList.remove('hidden');
  });
}

function colorForWaypoint(w) {
  if (!w.reachable) return STATE_COLORS.unreach;
  switch (w.state) {
    case 'current': return STATE_COLORS.current;
    case 'done':    return STATE_COLORS.done;
    case 'failed':  return STATE_COLORS.failed;
    case 'skipped': return STATE_COLORS.skipped;
    default:        return STATE_COLORS.pending;
  }
}

// Quaternion (xyzw) → 3-axis triad: 3 short coloured segments, length = 4 cm.
function makeTriad() {
  const L = 0.04;
  const geom = new THREE.BufferGeometry();
  const verts = new Float32Array([
    0,0,0,  L,0,0,    // X
    0,0,0,  0,L,0,    // Y
    0,0,0,  0,0,L,    // Z
  ]);
  const colors = new Float32Array([
    1,0.2,0.2,  1,0.2,0.2,
    0.2,1,0.3,  0.2,1,0.3,
    0.3,0.6,1,  0.3,0.6,1,
  ]);
  geom.setAttribute('position', new THREE.BufferAttribute(verts, 3));
  geom.setAttribute('color', new THREE.BufferAttribute(colors, 3));
  const mat = new THREE.LineBasicMaterial({ vertexColors: true, linewidth: 2, depthTest: false, transparent: true, opacity: 0.9 });
  const lines = new THREE.LineSegments(geom, mat);
  lines.renderOrder = 998;
  return lines;
}

function renderWaypoints(waypoints) {
  if (!waypointGroup) return;
  const seen = new Set();
  for (const w of waypoints) {
    seen.add(w.idx);
    let item = waypointItems.get(w.idx);
    if (!item) {
      const geom = new THREE.SphereGeometry(0.012, 18, 14);
      const mat = new THREE.MeshStandardMaterial({
        color: colorForWaypoint(w),
        emissive: 0x111111,
        depthTest: false,
        transparent: true,
        opacity: 0.85,
      });
      const sphere = new THREE.Mesh(geom, mat);
      sphere.renderOrder = 999;
      const triad = makeTriad();
      const group = new THREE.Group();
      group.add(sphere);
      group.add(triad);
      waypointGroup.add(group);
      item = { group, sphere, triad };
      waypointItems.set(w.idx, item);
    }
    item.group.position.set(w.position[0], w.position[1], w.position[2]);
    const q = new THREE.Quaternion(w.quaternion[0], w.quaternion[1], w.quaternion[2], w.quaternion[3]);
    item.triad.quaternion.copy(q);
    item.triad.visible = showTriads;
    item.sphere.material.color.setHex(colorForWaypoint(w));
    item.sphere.userData.waypoint = w;
    if (w.state === 'current') {
      item.sphere.material.emissive.setHex(0x1e5a8a);
      item.sphere.material.emissiveIntensity = 0.7;
      const s = 1.0 + 0.2 * Math.sin(Date.now() / 120);
      item.sphere.scale.setScalar(s);
    } else {
      item.sphere.material.emissive.setHex(0x000000);
      item.sphere.material.emissiveIntensity = 0;
      item.sphere.scale.setScalar(1.0);
    }
  }
  // Remove stale.
  for (const [idx, item] of waypointItems.entries()) {
    if (!seen.has(idx)) {
      waypointGroup.remove(item.group);
      item.sphere.geometry.dispose();
      item.sphere.material.dispose();
      item.triad.geometry.dispose();
      item.triad.material.dispose();
      waypointItems.delete(idx);
    }
  }
}

function readParams() {
  const side = $('acSide').value || null;
  return {
    delta_x_mm: +$('acDx').value,
    delta_y_mm: +$('acDy').value,
    delta_z_mm: +$('acDz').value,
    offset_x_mm: +$('acOx').value,
    offset_y_mm: +$('acOy').value,
    offset_z_mm: +$('acOz').value,
    n_x: +$('acNx').value,
    n_y: +$('acNy').value,
    n_z: +$('acNz').value,
    wrist_rot_delta_deg: +$('acRotDeg').value,
    min_z_m: +$('acMinZ').value,
    settle_s: +$('acSettle').value,
    side: side || undefined,
    auto_solve: $('acAutoSolve').checked,
    dry_run: $('acDryRun').checked,
    order: $('acOrder').value,
  };
}

function persistParams() {
  try { localStorage.setItem(LS_KEY, JSON.stringify(readParams())); } 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; };
    const setC = (id, v) => { if ($(id) && v != null) $(id).checked = !!v; };
    setV('acDx', p.delta_x_mm); setV('acDy', p.delta_y_mm); setV('acDz', p.delta_z_mm);
    setV('acOx', p.offset_x_mm); setV('acOy', p.offset_y_mm); setV('acOz', p.offset_z_mm);
    setV('acNx', p.n_x); setV('acNy', p.n_y); setV('acNz', p.n_z);
    setV('acRotDeg', p.wrist_rot_delta_deg); setV('acMinZ', p.min_z_m); setV('acSettle', p.settle_s);
    if (p.side) setV('acSide', p.side);
    setC('acAutoSolve', p.auto_solve);
    setC('acDryRun', p.dry_run);
    if (p.order) setV('acOrder', p.order);
  } catch (e) { console.warn('restoreParams:', e); }
}

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 previewGrid() {
  persistParams();
  const body = readParams();
  setStatus('Calcolo griglia…');
  try {
    const r = await api('POST', '/api/calibration/auto/feasibility', body);
    renderWaypoints(r.waypoints);
    drawChart(r.waypoints);
    updateStats(r.waypoints, null);
    const reach = r.waypoints.filter(w => w.reachable).length;
    setStatus(`griglia: ${r.waypoints.length} punti · ${reach} raggiungibili · ${r.waypoints.length - reach} scartati`);
    $('acStartBtn').disabled = reach === 0;
    $('acVerifyBtn').disabled = reach === 0;
  } catch (e) {
    setStatus('errore: ' + e.message, 'err');
  }
}

async function startProcedure() {
  persistParams();
  const body = readParams();
  const reach = [...waypointItems.values()].filter(it => it.sphere.userData.waypoint.reachable).length;
  if (!reach) return;
  if (!body.dry_run && !confirm(`Avvio calibrazione su ${reach} punti. Il robot si muoverà. Confermi?`)) return;
  try {
    setStatus('avvio procedura…');
    await api('POST', '/api/calibration/auto/start', body);
    $('acStartBtn').disabled = true;
    $('acVerifyBtn').disabled = true;
    $('acStopBtn').disabled = false;
    startPolling();
  } catch (e) {
    setStatus('start fallito: ' + e.message, 'err');
  }
}

async function verifyReachability() {
  persistParams();
  const body = readParams();
  const reach = [...waypointItems.values()].filter(it => it.sphere.userData.waypoint.reachable).length;
  if (!reach) return;
  if (!body.dry_run && !confirm(`Verifica raggiungibilità reale: il robot si muoverà a ${reach} punti senza acquisire campioni. Confermi?`)) return;
  try {
    setStatus('avvio verifica reachability…');
    await api('POST', '/api/calibration/auto/verify_reachability', body);
    $('acStartBtn').disabled = true;
    $('acVerifyBtn').disabled = true;
    $('acStopBtn').disabled = false;
    startPolling();
  } catch (e) {
    setStatus('verify fallito: ' + e.message, 'err');
  }
}

async function stopProcedure() {
  try {
    await api('POST', '/api/calibration/auto/stop');
    setStatus('stop richiesto… il robot tornerà a home prima di fermarsi');
  } catch (e) {
    setStatus('stop fallito: ' + e.message, 'err');
  }
}

function startPolling() {
  stopPolling();
  pollTick();
  pollTimer = setInterval(pollTick, 500);
}
function stopPolling() {
  if (pollTimer) { clearInterval(pollTimer); pollTimer = null; }
}

async function pollTick() {
  try {
    const s = await api('GET', '/api/calibration/auto/status');
    lastSnapshot = s;
    if (s.waypoints && s.waypoints.length) {
      renderWaypoints(s.waypoints);
      drawChart(s.waypoints);
    }
    updateStats(s.waypoints, s);
    if (s.status !== 'running') {
      stopPolling();
      $('acStartBtn').disabled = false;
      $('acVerifyBtn').disabled = false;
      $('acStopBtn').disabled = true;
      if (s.calibration) {
        $('autocalResult').textContent = JSON.stringify({
          method: s.calibration.method,
          n_samples: s.calibration.n_samples,
          residual_translation_mm: s.calibration.residual_translation_mm,
          residual_rotation_deg: s.calibration.residual_rotation_deg,
          T_base_camera: s.calibration.T_base_camera,
          sanity_check: s.calibration.sanity_check,
        }, null, 2);
      }
      const mode = (s.params || {}).mode || 'calibration';
      setStatus(`${mode}: ${s.status} · done=${s.stats.done} failed=${s.stats.failed} skipped=${s.stats.skipped}`,
        s.status === 'done' ? 'ok' : s.status === 'error' ? 'err' : '');
    }
  } catch (e) {
    console.warn('autocal poll:', e);
  }
}

function updateStats(waypoints, snapshot) {
  const host = $('autocalStats');
  if (!host) return;
  const total = waypoints.length;
  const reach = waypoints.filter(w => w.reachable).length;
  const unreach = total - reach;
  const done = waypoints.filter(w => w.state === 'done').length;
  const failed = waypoints.filter(w => w.state === 'failed').length;
  const skipped = waypoints.filter(w => w.state === 'skipped').length;
  const attempts = waypoints.filter(w => w.attempts > 0).map(w => w.attempts);
  const avgAtt = attempts.length ? (attempts.reduce((a, b) => a + b, 0) / attempts.length).toFixed(2) : '—';
  const maxAtt = attempts.length ? Math.max(...attempts) : '—';
  const current = snapshot && snapshot.current_index >= 0 ? snapshot.current_index : null;
  host.innerHTML = `
    <table><tbody>
      <tr><td>totale</td><td><strong>${total}</strong></td></tr>
      <tr><td>raggiungibili</td><td>${reach}</td></tr>
      <tr><td>non raggiungibili</td><td>${unreach}</td></tr>
      <tr><td>campioni ok</td><td>${done}</td></tr>
      <tr><td>falliti</td><td>${failed}</td></tr>
      <tr><td>saltati</td><td>${skipped}</td></tr>
      <tr><td>tentativi medi</td><td>${avgAtt}</td></tr>
      <tr><td>tentativi max</td><td>${maxAtt}</td></tr>
      <tr><td>corrente</td><td>${current != null ? '#' + current : '—'}</td></tr>
      <tr><td>stato job</td><td>${snapshot ? snapshot.status : '—'}</td></tr>
    </tbody></table>`;
}

// Per-waypoint bar chart: bar height = attempts, color by state.
function drawChart(waypoints) {
  try {
    _drawChartImpl(waypoints);
  } catch (e) {
    console.error('drawChart failed:', e);
    setStatus('drawChart error: ' + (e && e.message || e), 'err');
  }
}

function _drawChartImpl(waypoints) {
  const canvas = $('autocalChart');
  if (!canvas) return;
  const dpr = window.devicePixelRatio || 1;
  const w = canvas.clientWidth || canvas.width;
  const h = canvas.height;
  canvas.width = w * dpr;
  canvas.height = h * dpr;
  const ctx = canvas.getContext('2d');
  ctx.setTransform(dpr, 0, 0, dpr, 0, 0);
  ctx.clearRect(0, 0, w, h);
  ctx.fillStyle = '#0d1014';
  ctx.fillRect(0, 0, w, h);
  if (!waypoints || !waypoints.length) {
    ctx.fillStyle = '#666';
    ctx.font = '11px ui-monospace, monospace';
    ctx.fillText('nessun dato', 10, 20);
    return;
  }
  const n = waypoints.length;
  const maxA = Math.max(1, ...waypoints.map(x => x.attempts || 0));
  const pad = { l: 26, r: 6, t: 8, b: 18 };
  const plotW = w - pad.l - pad.r;
  const plotH = h - pad.t - pad.b;
  const barW = plotW / n;
  // grid
  ctx.strokeStyle = '#2a2f37';
  ctx.lineWidth = 1;
  const ticks = [1, 2, 3, 5, maxA].filter((v, i, a) => v <= maxA && a.indexOf(v) === i);
  ctx.font = '10px ui-monospace, monospace';
  ctx.fillStyle = '#666';
  for (const t of ticks) {
    const y = pad.t + plotH - (t / maxA) * plotH;
    ctx.beginPath(); ctx.moveTo(pad.l, y); ctx.lineTo(w - pad.r, y); ctx.stroke();
    ctx.fillText(String(t), 2, y + 3);
  }
  // bars
  const colorFor = (w) => {
    if (!w.reachable) return '#e74c3c';
    if (w.state === 'done' && w.attempts === 1) return '#3ecf8e';
    if (w.state === 'done') return '#ffd54f';
    if (w.state === 'failed') return '#f0ad4e';
    if (w.state === 'current') return '#4da3ff';
    if (w.state === 'skipped') return '#8a3a33';
    return '#555';
  };
  for (let i = 0; i < n; i++) {
    const wp = waypoints[i];
    const a = wp.attempts || 0;
    const bh = (a / maxA) * plotH;
    const x = pad.l + i * barW + 1;
    const y = pad.t + plotH - bh;
    ctx.fillStyle = colorFor(wp);
    ctx.fillRect(x, y, Math.max(1, barW - 2), bh);
  }
  // x label
  ctx.fillStyle = '#9aa0a6';
  ctx.fillText(`waypoints (${n})`, pad.l, h - 4);
  ctx.fillText(`tentativi`, 2, 10);
}

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

async function solveNow() {
  try {
    setStatus('calcolo matrice…');
    const side = $('acSide').value || undefined;
    const r = await api('POST', '/api/calibration/solve', { method: 'TSAI', side });
    $('autocalResult').textContent = JSON.stringify({
      method: r.method, n_samples: r.n_samples,
      residual_translation_mm: r.residual_translation_mm,
      residual_rotation_deg: r.residual_rotation_deg,
      T_base_camera: r.T_base_camera,
      sanity_check: r.sanity_check,
    }, null, 2);
    setStatus(`matrice calcolata · residuo ${r.residual_translation_mm.toFixed(1)} mm / ${r.residual_rotation_deg.toFixed(2)}°`, 'ok');
  } catch (e) { setStatus('solve: ' + e.message, 'err'); }
}

async function clearSamples() {
  const side = $('acSide').value || 'right';
  if (!confirm(`Rimuovere tutti i sample del braccio ${side}?`)) return;
  try {
    await api('DELETE', `/api/calibration/samples?side=${encodeURIComponent(side)}`);
    setStatus('sample svuotati', 'ok');
  } catch (e) { setStatus(e.message, 'err'); }
}

// Wire up events AFTER DOM is parsed.
document.addEventListener('DOMContentLoaded', () => {
  if (!$('autocalCanvas')) return;
  restoreParams();
  // persist on any input change
  ['acDx','acDy','acDz','acOx','acOy','acOz','acNx','acNy','acNz','acRotDeg','acMinZ','acSettle',
   'acSide','acAutoSolve','acDryRun','acOrder'].forEach(id => {
    const el = $(id); if (el) el.addEventListener('change', persistParams);
  });
  $('acPreviewBtn').addEventListener('click', previewGrid);
  $('acVerifyBtn').addEventListener('click', verifyReachability);
  $('acStartBtn').addEventListener('click', startProcedure);
  $('acStopBtn').addEventListener('click', stopProcedure);
  $('acSolveBtn').addEventListener('click', solveNow);
  $('acClearBtn').addEventListener('click', clearSamples);
  $('acShowTriads').addEventListener('change', (e) => {
    showTriads = e.target.checked;
    for (const it of waypointItems.values()) it.triad.visible = showTriads;
  });
  $('acFitBtn').addEventListener('click', () => viewer && viewer.fitView());
  $('acResetViewBtn').addEventListener('click', () => viewer && viewer.resetView());
});

// Lazy boot when tab first opened.
document.querySelectorAll('.tab').forEach(btn => btn.addEventListener('click', () => {
  if (btn.dataset.tab === 'autocal') {
    boot();
    setTimeout(() => viewer && viewer.resize(), 50);
  }
}));

if (document.getElementById('tab-autocal')?.classList.contains('active')) boot();