From 7a561cfa67c587ab33f807602d968f988c0dc9b5 Mon Sep 17 00:00:00 2001
From: Adriano Dal Pastro <adrianodalpastro@tielogic.com>
Date: Thu, 21 May 2026 15:56:34 +0200
Subject: [PATCH] feat(pose_3d): polling /state + readout sidebar

---
 web/pose_3d.js | 108 +++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 108 insertions(+)

diff --git a/web/pose_3d.js b/web/pose_3d.js
index c82ae17..e0511c1 100644
--- a/web/pose_3d.js
+++ b/web/pose_3d.js
@@ -153,6 +153,111 @@ function initScene() {
   animate();
 }
 
+// ----- fetch helpers -----
+async function jsonFetch(url, opts) {
+  const r = await fetch(url, opts);
+  const body = await r.json().catch(() => ({}));
+  if (!r.ok) {
+    const err = new Error(body.detail || `HTTP ${r.status}`);
+    err.status = r.status;
+    throw err;
+  }
+  return body;
+}
+
+// ----- quaternion / RPY conversion (xyzw, deg) -----
+function quatToRPYdeg(q) {
+  const [x, y, z, w] = q;
+  // ZYX intrinsic = RPY (roll=x, pitch=y, yaw=z)
+  const sinr_cosp = 2 * (w * x + y * z);
+  const cosr_cosp = 1 - 2 * (x * x + y * y);
+  const roll = Math.atan2(sinr_cosp, cosr_cosp);
+  const sinp = 2 * (w * y - z * x);
+  const pitch = Math.abs(sinp) >= 1 ? Math.sign(sinp) * Math.PI / 2 : Math.asin(sinp);
+  const siny_cosp = 2 * (w * z + x * y);
+  const cosy_cosp = 1 - 2 * (y * y + z * z);
+  const yaw = Math.atan2(siny_cosp, cosy_cosp);
+  return [roll * 180 / Math.PI, pitch * 180 / Math.PI, yaw * 180 / Math.PI];
+}
+
+// ----- state apply to scene -----
+function updateRealWrists() {
+  for (const side of ['right', 'left']) {
+    const pose = state.current[side];
+    if (!pose) continue;
+    const arm = state.arms[side];
+    arm.position.set(pose.position[0], pose.position[1], pose.position[2]);
+    arm.quaternion.set(pose.quaternion[0], pose.quaternion[1],
+                       pose.quaternion[2], pose.quaternion[3]);
+    // aggiorna line shoulder→wrist
+    const line = state.links[side];
+    const pts = [SHOULDER[side].clone(), arm.position.clone()];
+    line.geometry.setFromPoints(pts);
+    line.geometry.attributes.position.needsUpdate = true;
+  }
+}
+
+function updateReadout() {
+  const side = state.active_side;
+  const pose = state.target[side] || state.current[side];
+  if (!pose) {
+    $('p3X').textContent = '—';
+    return;
+  }
+  const p = pose.position, q = pose.quaternion;
+  $('p3X').textContent = (p[0] * 1000).toFixed(1);
+  $('p3Y').textContent = (p[1] * 1000).toFixed(1);
+  $('p3Z').textContent = (p[2] * 1000).toFixed(1);
+  const rpy = quatToRPYdeg(q);
+  $('p3R').textContent = rpy[0].toFixed(1);
+  $('p3P').textContent = rpy[1].toFixed(1);
+  $('p3W').textContent = rpy[2].toFixed(1);
+  $('p3Status').textContent = state.target[side] ? '● modificata' : '— polso fermo';
+  $('p3Status').className = 'p3-status' + (state.target[side] ? ' modified' : '');
+}
+
+// ----- polling -----
+let pollTimer = null;
+async function pollState() {
+  try {
+    const s = await jsonFetch('/api/pose_3d/state');
+    state.current.right = s.wrist.right;
+    state.current.left = s.wrist.left;
+    state.saved_poses = s.poses || {};
+    state.live_max_step_m = s.live_max_step_m || 0.15;
+    updateRealWrists();
+    updateReadout();
+    // Aggiorna dropdown pose salvate
+    refreshPoseSelect();
+  } catch (e) {
+    // soft fail: log + segna lo stato
+    console.warn('pose_3d /state fail:', e.message);
+  }
+}
+
+function refreshPoseSelect() {
+  const sel = $('p3PoseSelect');
+  if (!sel) return;
+  const cur = sel.value;
+  sel.innerHTML = '<option value="">(seleziona)</option>';
+  for (const name of Object.keys(state.saved_poses).sort()) {
+    const opt = document.createElement('option');
+    opt.value = name; opt.textContent = name;
+    sel.appendChild(opt);
+  }
+  sel.value = cur;
+}
+
+// ----- side change listener -----
+function bindSideControls() {
+  for (const r of document.querySelectorAll('input[name=p3Side]')) {
+    r.addEventListener('change', () => {
+      state.active_side = r.value;
+      updateReadout();
+    });
+  }
+}
+
 // ----- init triggered on first activation -----
 let inited = false;
 function ensureInit() {
@@ -160,6 +265,9 @@ function ensureInit() {
   if (!$('p3Canvas')) return;
   inited = true;
   initScene();
+  bindSideControls();
+  pollState();
+  pollTimer = setInterval(pollState, POLL_MS);
 }
 
 // Observe quando la tab diventa visibile per inizializzare