// Tab "Pose 3D" — viewport Three.js, gizmo translate/rotate, drag dei polsi.
// Vanilla JS, ES modules.

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

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

// ----- helpers geometria -----
function makeUCS(size = 0.1) {
  // 3 linee X(rosso) Y(verde) Z(blu) dalla origine
  const g = new THREE.Group();
  const mat = (color) => new THREE.LineBasicMaterial({ color, linewidth: 2 });
  const make = (dir, color) => {
    const geom = new THREE.BufferGeometry().setFromPoints([
      new THREE.Vector3(0, 0, 0),
      new THREE.Vector3(dir.x * size, dir.y * size, dir.z * size),
    ]);
    return new THREE.Line(geom, mat(color));
  };
  g.add(make(new THREE.Vector3(1, 0, 0), 0xe74c3c));
  g.add(make(new THREE.Vector3(0, 1, 0), 0x3ecf8e));
  g.add(make(new THREE.Vector3(0, 0, 1), 0x4da3ff));
  return g;
}

function makeLabelSprite(text, color = '#ffffff') {
  const canvas = document.createElement('canvas');
  canvas.width = 128;
  canvas.height = 64;
  const ctx = canvas.getContext('2d');
  ctx.fillStyle = 'rgba(0, 0, 0, 0.6)';
  ctx.fillRect(0, 0, canvas.width, canvas.height);
  ctx.fillStyle = color;
  ctx.font = 'bold 36px sans-serif';
  ctx.textAlign = 'center';
  ctx.textBaseline = 'middle';
  ctx.fillText(text, canvas.width / 2, canvas.height / 2);
  const texture = new THREE.CanvasTexture(canvas);
  const material = new THREE.SpriteMaterial({ map: texture, depthTest: false });
  const sprite = new THREE.Sprite(material);
  sprite.scale.set(0.10, 0.05, 1);  // ~10cm wide x 5cm tall in scene units
  return sprite;
}

function makeWristMarker(color) {
  const group = new THREE.Group();
  const sphere = new THREE.Mesh(
    new THREE.SphereGeometry(0.04, 16, 12),
    new THREE.MeshStandardMaterial({ color, metalness: 0.2, roughness: 0.5 }),
  );
  group.add(sphere);
  group.add(makeUCS(0.07));
  group.userData.sphere = sphere;
  group.userData.sphere.userData.kind = 'wrist';
  return group;
}

// ----- state -----
const state = {
  three: null,                    // {scene, camera, renderer, orbit, transform}
  arms: { right: null, left: null },         // wrist groups (real)
  ghosts: { right: null, left: null },       // wrist groups (target preview)
  links: { right: null, left: null },        // line shoulder→wrist
  current: { right: null, left: null },      // {position, quaternion}
  target: { right: null, left: null },       // null = nessuna modifica locale
  active_side: 'right',
  mode: 'editor',
  gizmo_mode: 'translate',
  precise: true,
  saved_poses: {},
  live_max_step_m: 0.15,
  robot: null,            // URDF robot loaded via urdf-loader (null se non caricato)
  arm_links_m: null,      // {upper_m, fore_m} computati da URDF al load
};

function checkWorkspace(side, position_m) {
  // Controllo client-side: il target e' nella sfera di raggio ARM_MAX_REACH_M
  // centrata sulla spalla? Ritorna {feasible, distance_m, max_m}.
  const sh = SHOULDER[side];
  if (!sh) return { feasible: true, distance_m: 0, max_m: ARM_MAX_REACH_M };
  const dx = position_m[0] - sh.x;
  const dy = position_m[1] - sh.y;
  const dz = position_m[2] - sh.z;
  const distance = Math.sqrt(dx*dx + dy*dy + dz*dz);
  return {
    feasible: distance <= ARM_MAX_REACH_M,
    distance_m: distance,
    max_m: ARM_MAX_REACH_M,
  };
}

// Posizioni torso e spalle hardcoded (riferimento visivo, non IK)
const TORSO_POS = new THREE.Vector3(0, 0, 0.40);
// Posizioni di riferimento del G1 in base frame (feet on floor convention,
// URDF alzato di 0.793m). Costanti approssimate, non vengono usate per IK
// vera ma per: (1) workspace check (centro = spalla, raggio = braccio teso),
// (2) fallback delle sfere polso quando /state e' offline (posa zero-joint).
const SHOULDER = {
  right: new THREE.Vector3(0.0, -0.18, 1.05),
  left:  new THREE.Vector3(0.0,  0.18, 1.05),
};
const WRIST_ZERO_POSE = {
  right: new THREE.Vector3(0.0, -0.18, 0.45),  // braccia giu', polso ~45cm sopra il suolo
  left:  new THREE.Vector3(0.0,  0.18, 0.45),
};
const ARM_MAX_REACH_M = 0.62;  // braccio teso G1 (upper+forearm+palm ~10+25+25 cm)

// ----- URDF loader -----
// Carica il modello G1 reale a partire dal URDF vendored sotto
// /static/vendor/g1/. Le mesh STL referenziate nel URDF (path relativi tipo
// "meshes/pelvis.STL") vengono risolti contro lo stesso prefisso.
async function loadG1Urdf(scene) {
  const loader = new URDFLoader();
  loader.loadMeshCb = function (path, manager, onComplete) {
    // URDFLoader risolve gia' i path relativi rispetto alla URL della URDF,
    // quindi `path` arriva come "/static/vendor/g1/meshes/X.STL". Non
    // ri-prefissare.
    const url = path;
    const stl = new STLLoader(manager);
    stl.load(
      url,
      (geometry) => {
        const mat = new THREE.MeshStandardMaterial({
          color: 0x9099a3, metalness: 0.4, roughness: 0.6,
          transparent: true, opacity: 0.85,
        });
        const mesh = new THREE.Mesh(geometry, mat);
        onComplete(mesh);
      },
      undefined,
      (err) => onComplete(null, err),
    );
  };
  return new Promise((resolve, reject) => {
    loader.load(
      '/static/vendor/g1/g1.urdf',
      (robot) => {
        // G1 URDF uses Z-up (Unitree convention). Pelvi e' all'origine,
        // alziamo il robot di ~0.793m cosi' i piedi toccano la griglia Z=0.
        robot.position.z = 0.793;
        scene.add(robot);
        state.robot = robot;
        state.arm_links_m = measureArmLinkLengths(robot);
        console.log('[pose_3d] G1 URDF caricato:', robot.name || 'g1',
                    'arm links (m):', state.arm_links_m);
        resolve(robot);
      },
      undefined,
      (err) => reject(err),
    );
  });
}

function measureArmLinkLengths(robot) {
  // Misura le lunghezze dei link "upper arm" e "forearm" leggendo le posizioni
  // dei joint right_shoulder_pitch -> right_elbow -> right_wrist_pitch
  // (proxy per la lunghezza dell'avambraccio fino al polso).
  // Default fallback: ~0.25 m upper, ~0.25 m forearm se i joint non si trovano.
  const get = (n) => robot.joints && robot.joints[n] ? robot.joints[n] : null;
  const shp = get('right_shoulder_pitch');
  const elb = get('right_elbow');
  const wrp = get('right_wrist_pitch');
  if (!shp || !elb || !wrp) return { upper_m: 0.25, fore_m: 0.25 };
  // World positions
  const wShoulder = new THREE.Vector3();
  const wElbow = new THREE.Vector3();
  const wWrist = new THREE.Vector3();
  shp.getWorldPosition(wShoulder);
  elb.getWorldPosition(wElbow);
  wrp.getWorldPosition(wWrist);
  return {
    upper_m: wShoulder.distanceTo(wElbow),
    fore_m:  wElbow.distanceTo(wWrist),
  };
}

function solveTwoBoneIK(shoulderWorldPos, wristTargetWorldPos, L1, L2, poleHintDir) {
  // Calcola angolo gomito e direzione shoulder->elbow per posizionare
  // l'end-effector (polso) in `wristTargetWorldPos`.
  // Restituisce {elbow_angle, shoulder_pitch, shoulder_roll, shoulder_yaw}
  // basati su convenzione G1 (Unitree): pitch attorno a Y, roll attorno a X, yaw attorno a Z del frame spalla.
  // I valori sono approssimati, calibrabili in seguito.
  const sw = new THREE.Vector3().subVectors(wristTargetWorldPos, shoulderWorldPos);
  let d = sw.length();
  const dmax = (L1 + L2) * 0.999;
  const dmin = Math.abs(L1 - L2) * 1.001;
  if (d > dmax) d = dmax;
  if (d < dmin) d = dmin;
  sw.normalize();

  // Angolo del gomito (law of cosines), 0 = teso, pi = ripiegato
  const cosE = (L1*L1 + L2*L2 - d*d) / (2*L1*L2);
  const elbow_angle = Math.PI - Math.acos(Math.max(-1, Math.min(1, cosE)));

  // Direzione "verso la target" in coordinate base del polso shoulder
  // Per il G1 right arm in zero pose: shoulder X = down (verso il basso), Z = forward.
  // Approssimazione: shoulder_pitch ruota attorno a Y (sinistra-destra), shoulder_roll attorno a X (avanti-indietro).
  // - pitch: angolo nel piano XZ (front/back vs down)
  // - roll: angolo fuori dal piano (verso medial/lateral)
  // - yaw: tipicamente 0 per drag puro (rotazione attorno a all'asse del braccio)

  // pitch: alzare/abbassare il braccio = ruotare attorno a Y dello shoulder frame
  // In zero pose dx, braccio giu' -> world (0, 0, -1). Vogliamo il braccio puntato lungo `sw`.
  // pitch sposta nel piano X-Z (vista frontale: avanti/dietro)
  // roll sposta lateralmente
  const x = sw.x;  // forward axis (X base = +x)
  const y = sw.y;  // lateral (sinistra +)
  const z = sw.z;  // up (+z)

  // pitch: angolo nel piano XZ. In zero pose (-Z), pitch=0. pitch>0 alza in avanti (+X).
  // arm pointing direction: from shoulder to wrist, in world frame.
  // Se z=-1 (giù) e x=0,y=0 -> pitch=0, roll=0
  // Se x=+1 (avanti) e z=0 -> pitch = +pi/2
  const horiz = Math.sqrt(x*x + y*y);
  const shoulder_pitch = Math.atan2(x, -z);   // 0 = giù, +pi/2 = avanti
  // roll: angolo dalla verticale lungo Y (asse laterale)
  const shoulder_roll = Math.atan2(y, Math.sqrt(x*x + z*z));  // 0 = nel piano sagittale
  // yaw: lasciato a 0 per ora (rotazione attorno al braccio); puo' essere
  // tunato dal quaternion target del polso
  const shoulder_yaw = 0;

  return {
    elbow_angle,
    shoulder_pitch,
    shoulder_roll,
    shoulder_yaw,
  };
}

function applyIKToUrdf(side, target_position, target_quaternion) {
  // Aggiorna i joint dell'URDF G1 per posizionare il polso `side` al target.
  // No-op se l'URDF non e' caricato.
  if (!state.robot || !state.arm_links_m) return;
  const robot = state.robot;
  // Posizione spalla in world frame (legge dall'URDF live)
  const shoulderName = `${side}_shoulder_pitch`;
  const shJoint = robot.joints && robot.joints[shoulderName];
  if (!shJoint) {
    console.warn('[pose_3d] joint non trovato:', shoulderName);
    return;
  }
  const shoulderWorld = new THREE.Vector3();
  shJoint.getWorldPosition(shoulderWorld);
  // Target in world (i target sono gia' in scene/world frame)
  const wristWorld = new THREE.Vector3(target_position[0], target_position[1], target_position[2]);

  const { upper_m, fore_m } = state.arm_links_m;
  const ik = solveTwoBoneIK(shoulderWorld, wristWorld, upper_m, fore_m);

  // Per il braccio sinistro, alcuni segni si invertono (mirror)
  const sign = side === 'right' ? 1 : -1;

  try {
    robot.setJointValue(`${side}_shoulder_pitch`, ik.shoulder_pitch);
    robot.setJointValue(`${side}_shoulder_roll`, ik.shoulder_roll * sign);
    robot.setJointValue(`${side}_shoulder_yaw`, ik.shoulder_yaw);
    robot.setJointValue(`${side}_elbow`, ik.elbow_angle);
    // Wrist orientation: per ora teniamo 0 (raggiunge la posizione, l'orientazione
    // non e' guidata dal target quaternion). Estensione futura: convertire il
    // quaternion target a roll/pitch/yaw del polso URDF.
    robot.setJointValue(`${side}_wrist_roll`, 0);
    robot.setJointValue(`${side}_wrist_pitch`, 0);
    robot.setJointValue(`${side}_wrist_yaw`, 0);
  } catch (e) {
    console.warn('[pose_3d] setJointValue err:', e.message);
  }
}

// ----- scene init -----
function initScene() {
  const canvas = $('p3Canvas');
  const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
  const rect = canvas.parentElement.getBoundingClientRect();
  renderer.setSize(rect.width, rect.height, false);
  renderer.setPixelRatio(window.devicePixelRatio || 1);

  const scene = new THREE.Scene();
  scene.background = new THREE.Color(0x0d1014);

  const camera = new THREE.PerspectiveCamera(45, rect.width / rect.height, 0.05, 20);
  camera.position.set(1, -1, 0.8);
  camera.up.set(0, 0, 1);  // Z up (coerente col base frame del robot)
  camera.lookAt(0, 0, 0.3);

  scene.add(new THREE.HemisphereLight(0xffffff, 0x202030, 0.6));
  const dir = new THREE.DirectionalLight(0xffffff, 0.8);
  dir.position.set(2, 2, 3);
  scene.add(dir);

  // Grid + base UCS
  const grid = new THREE.GridHelper(2, 20, 0x444444, 0x222222);
  grid.rotation.x = Math.PI / 2;  // piano XY (z=0)
  scene.add(grid);
  const baseUCS = makeUCS(0.2);
  scene.add(baseUCS);

  // "FRONT" label sul piano del pavimento (Z=0.01, sotto tutto) per indicare
  // la direzione +X del base frame. Solo marker visivo, NON interagibile
  // (la raycaster considera solo le sfere dei polsi).
  const frontArrow = makeLabelSprite('▲ FRONT (base +X)', '#ffd54f');
  frontArrow.position.set(0.30, 0, 0.01);
  frontArrow.scale.set(0.18, 0.045, 1);  // più largo ma sottile, "etichetta a terra"
  scene.add(frontArrow);

  // Modello G1 reale via URDF (best-effort: se il loader fallisce, niente
  // body — la tab funziona comunque coi soli polsi/UCS).
  loadG1Urdf(scene).catch((e) => {
    console.warn('[pose_3d] URDF G1 non caricato:', e.message);
  });

  // Arms
  for (const side of ['right', 'left']) {
    const wrist = makeWristMarker(side === 'right' ? 0x4da3ff : 0xff5e7e);
    wrist.position.copy(WRIST_ZERO_POSE[side]);  // posizione fallback "braccia giù"
    wrist.userData.side = side;
    scene.add(wrist);
    state.arms[side] = wrist;

    // Etichetta DX / SX sopra la sfera
    const label = makeLabelSprite(side === 'right' ? 'DX' : 'SX',
                                   side === 'right' ? '#4da3ff' : '#ff5e7e');
    label.position.set(0, 0, 0.08);  // 8cm sopra la sfera (locale al gruppo)
    wrist.add(label);

    // Linea shoulder → wrist
    const lineGeom = new THREE.BufferGeometry().setFromPoints([
      SHOULDER[side].clone(),
      wrist.position.clone(),
    ]);
    const line = new THREE.Line(lineGeom, new THREE.LineBasicMaterial({ color: 0xaaaaaa }));
    scene.add(line);
    state.links[side] = line;

    // Ghost (preview target), inizialmente nascosto
    const ghost = makeWristMarker(side === 'right' ? 0x4da3ff : 0xff5e7e);
    ghost.userData.sphere.material = ghost.userData.sphere.material.clone();
    ghost.userData.sphere.material.transparent = true;
    ghost.userData.sphere.material.opacity = 0.4;
    ghost.visible = false;
    scene.add(ghost);
    state.ghosts[side] = ghost;
  }

  const orbit = new OrbitControls(camera, renderer.domElement);
  orbit.target.set(0, 0, 0.3);
  orbit.update();

  const transform = new TransformControls(camera, renderer.domElement);
  transform.setMode('translate');
  transform.size = 0.6;
  // Disable orbit while dragging gizmo
  transform.addEventListener('dragging-changed', (e) => { orbit.enabled = !e.value; });
  // In r160 the visual helper is exposed via getHelper(); fall back to the
  // transform object itself for older versions.
  const transformHelper = typeof transform.getHelper === 'function'
    ? transform.getHelper()
    : transform;
  scene.add(transformHelper);

  state.three = { scene, camera, renderer, orbit, transform };

  // Resize handler
  const onResize = () => {
    const r = canvas.parentElement.getBoundingClientRect();
    renderer.setSize(r.width, r.height, false);
    camera.aspect = r.width / r.height;
    camera.updateProjectionMatrix();
  };
  window.addEventListener('resize', onResize);

  // Render loop
  function animate() {
    requestAnimationFrame(animate);
    renderer.render(scene, camera);
  }
  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];
}

function rpyDegToQuat(roll, pitch, yaw) {
  const r = roll * Math.PI / 180;
  const p = pitch * Math.PI / 180;
  const y = yaw * Math.PI / 180;
  const cr = Math.cos(r / 2), sr = Math.sin(r / 2);
  const cp = Math.cos(p / 2), sp = Math.sin(p / 2);
  const cy = Math.cos(y / 2), sy = Math.sin(y / 2);
  // ZYX intrinsic (matches quatToRPYdeg)
  const qx = sr * cp * cy - cr * sp * sy;
  const qy = cr * sp * cy + sr * cp * sy;
  const qz = cr * cp * sy - sr * sp * cy;
  const qw = cr * cp * cy + sr * sp * sy;
  return [qx, qy, qz, qw];
}

// ----- 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 syncGhostsToTargetOrReal() {
  for (const side of ['right', 'left']) {
    const ghost = state.ghosts[side];
    if (!ghost) continue;
    if (state.target[side]) {
      // ghost segue il target locale (mosso dal gizmo)
      ghost.visible = true;
    } else if (side === state.active_side) {
      // ghost allineato al reale (preparato per drag).
      // Fallback se /state e' offline: copia dall'arm visibile.
      const real = state.current[side];
      const arm = state.arms[side];
      if (real) {
        ghost.position.set(real.position[0], real.position[1], real.position[2]);
        ghost.quaternion.set(real.quaternion[0], real.quaternion[1],
                             real.quaternion[2], real.quaternion[3]);
      } else if (arm) {
        ghost.position.copy(arm.position);
        ghost.quaternion.copy(arm.quaternion);
      }
    } else {
      ghost.visible = false;
    }
  }
}

function setInputIfNotFocused(id, value) {
  const el = $(id);
  if (!el) return;
  if (document.activeElement === el) return;  // user is typing, don't stomp
  el.value = value;
}

function updateReadout() {
  const side = state.active_side;
  const pose = state.target[side] || state.current[side];
  if (!pose) {
    setInputIfNotFocused('p3X', '');
    setInputIfNotFocused('p3Y', '');
    setInputIfNotFocused('p3Z', '');
    setInputIfNotFocused('p3R', '');
    setInputIfNotFocused('p3P', '');
    setInputIfNotFocused('p3W', '');
    return;
  }
  const p = pose.position, q = pose.quaternion;
  setInputIfNotFocused('p3X', (p[0] * 1000).toFixed(1));
  setInputIfNotFocused('p3Y', (p[1] * 1000).toFixed(1));
  setInputIfNotFocused('p3Z', (p[2] * 1000).toFixed(1));
  const rpy = quatToRPYdeg(q);
  setInputIfNotFocused('p3R', rpy[0].toFixed(1));
  setInputIfNotFocused('p3P', rpy[1].toFixed(1));
  setInputIfNotFocused('p3W', rpy[2].toFixed(1));
  $('p3Status').textContent = state.target[side] ? '● modificata' : '— polso fermo';
  $('p3Status').className = 'p3-status' + (state.target[side] ? ' modified' : '');
  // Workspace feasibility coloring + indicator
  applyFeasibilityToActiveGhost();
}

function applyFeasibilityToActiveGhost() {
  const side = state.active_side;
  if (!side) return;
  const pose = state.target[side] || state.current[side];
  if (!pose) {
    // Niente posa, niente indicazione
    setFeasibilityIndicator(null);
    return;
  }
  const check = checkWorkspace(side, pose.position);
  const ghost = state.ghosts[side];
  if (ghost && ghost.userData.sphere) {
    // verde se feasible, rosso se fuori workspace; opacita' invariata
    const color = check.feasible ? 0x3ecf8e : 0xe74c3c;
    ghost.userData.sphere.material.color.setHex(color);
  }
  setFeasibilityIndicator(check);
}

function setFeasibilityIndicator(check) {
  const el = $('p3Feasibility');
  if (!el) return;
  if (!check) {
    el.textContent = '';
    el.className = 'p3-feasibility';
    return;
  }
  const dist_mm = (check.distance_m * 1000).toFixed(0);
  const max_mm = (check.max_m * 1000).toFixed(0);
  if (check.feasible) {
    el.textContent = `✓ raggiungibile (dist spalla ${dist_mm}/${max_mm} mm)`;
    el.className = 'p3-feasibility ok';
  } else {
    const over_mm = (check.distance_m - check.max_m) * 1000;
    el.textContent = `⚠ fuori workspace (${dist_mm}/${max_mm} mm — ${over_mm.toFixed(0)} mm oltre)`;
    el.className = 'p3-feasibility error';
  }
}

// ----- 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();
    syncGhostsToTargetOrReal();
    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;
}

// ----- selezione polso via click -----
function setupSelection() {
  const { renderer, camera, transform } = state.three;
  const raycaster = new THREE.Raycaster();
  const mouse = new THREE.Vector2();
  const canvas = renderer.domElement;

  canvas.addEventListener('pointerdown', (e) => {
    // Se sta interagendo con il gizmo, non fare nulla
    if (transform.dragging) return;
    const rect = canvas.getBoundingClientRect();
    mouse.x = ((e.clientX - rect.left) / rect.width) * 2 - 1;
    mouse.y = -((e.clientY - rect.top) / rect.height) * 2 + 1;
    raycaster.setFromCamera(mouse, camera);
    // Verifica intersezione con sphere destra o sinistra
    const candidates = [state.arms.right.userData.sphere,
                        state.arms.left.userData.sphere];
    const hits = raycaster.intersectObjects(candidates, false);
    console.log('[pose_3d] click on canvas, raycast hits:', hits.length);
    if (hits.length > 0) {
      const sphere = hits[0].object;
      const side = sphere.parent.userData.side;
      console.log('[pose_3d] activating side:', side);
      activateSide(side);
    }
  });
}

function activateSide(side) {
  state.active_side = side;
  // sync radio buttons
  for (const r of document.querySelectorAll('input[name=p3Side]')) {
    r.checked = (r.value === side);
  }
  // Attacca gizmo al ghost (lo creiamo se non esiste, posizionandolo sul reale)
  ensureGhostInit(side);
  const { transform } = state.three;
  transform.attach(state.ghosts[side]);
  console.log('[pose_3d] gizmo attached to ghost', side, 'at', state.ghosts[side].position);
  state.ghosts[side].visible = true;
  updateReadout();
}

function ensureGhostInit(side) {
  // Se non c'è target locale, allinea ghost al reale (preparato per drag).
  // Se /state non e' disponibile (G1 offline), fallback alla posizione
  // dell'arm visibile (che usa SHOULDER come default).
  if (state.target[side]) return;
  const real = state.current[side];
  const arm = state.arms[side];
  if (real) {
    state.ghosts[side].position.set(real.position[0], real.position[1], real.position[2]);
    state.ghosts[side].quaternion.set(real.quaternion[0], real.quaternion[1],
                                       real.quaternion[2], real.quaternion[3]);
  } else if (arm) {
    state.ghosts[side].position.copy(arm.position);
    state.ghosts[side].quaternion.copy(arm.quaternion);
  }
}

// ----- gizmo mode toggle -----
function bindGizmoControls() {
  for (const r of document.querySelectorAll('input[name=p3Gizmo]')) {
    r.addEventListener('change', () => {
      state.gizmo_mode = r.value;
      state.three.transform.setMode(r.value);
    });
  }
  // tasti t/r
  window.addEventListener('keydown', (e) => {
    if (!$('tab-pose-3d')?.classList.contains('active')) return;
    if (e.target.tagName === 'INPUT' || e.target.tagName === 'TEXTAREA') return;
    if (e.key === 't') { setGizmoMode('translate'); }
    if (e.key === 'r') { setGizmoMode('rotate'); }
  });
}

function setGizmoMode(mode) {
  state.gizmo_mode = mode;
  state.three.transform.setMode(mode);
  for (const r of document.querySelectorAll('input[name=p3Gizmo]')) {
    r.checked = (r.value === mode);
  }
}

// ----- target tracking durante drag gizmo -----
function bindGizmoEvents() {
  const { transform } = state.three;
  transform.addEventListener('change', () => {
    const side = state.active_side;
    const ghost = state.ghosts[side];
    if (!ghost) return;
    state.target[side] = {
      position: [ghost.position.x, ghost.position.y, ghost.position.z],
      quaternion: [ghost.quaternion.x, ghost.quaternion.y,
                   ghost.quaternion.z, ghost.quaternion.w],
    };
    updateReadout();
  });
}

function readReadoutInputs() {
  const num = (id, dflt) => {
    const v = parseFloat($(id).value);
    return Number.isFinite(v) ? v : dflt;
  };
  const side = state.active_side;
  if (!side) return;
  // fallback values: current pose if any, else SHOULDER position via arm
  const fallback = state.current[side] || (state.arms[side] ? {
    position: [state.arms[side].position.x, state.arms[side].position.y, state.arms[side].position.z],
    quaternion: [0, 0, 0, 1],
  } : { position: [0, 0, 0], quaternion: [0, 0, 0, 1] });
  const x_mm = num('p3X', fallback.position[0] * 1000);
  const y_mm = num('p3Y', fallback.position[1] * 1000);
  const z_mm = num('p3Z', fallback.position[2] * 1000);
  const r_deg = num('p3R', 0);
  const p_deg = num('p3P', 0);
  const w_deg = num('p3W', 0);
  const position = [x_mm / 1000, y_mm / 1000, z_mm / 1000];
  const quaternion = rpyDegToQuat(r_deg, p_deg, w_deg);
  state.target[side] = { position, quaternion };
  // Aggiorna ghost in scena
  const ghost = state.ghosts[side];
  if (ghost) {
    ghost.position.set(position[0], position[1], position[2]);
    ghost.quaternion.set(quaternion[0], quaternion[1], quaternion[2], quaternion[3]);
    ghost.visible = true;
  }
  // Status
  $('p3Status').textContent = '● modificata (da input)';
  $('p3Status').className = 'p3-status modified';
}

function bindReadoutInputs() {
  for (const id of ['p3X', 'p3Y', 'p3Z', 'p3R', 'p3P', 'p3W']) {
    const el = $(id);
    if (!el) continue;
    el.addEventListener('change', readReadoutInputs);  // fire on blur / Enter
  }
}

// ----- side change listener -----
function bindSideControls() {
  for (const r of document.querySelectorAll('input[name=p3Side]')) {
    r.addEventListener('change', () => {
      state.active_side = r.value;
      updateReadout();
    });
  }
}

// ----- Editor mode actions -----
async function onApplyClick() {
  if (state.mode !== 'editor') return;  // in Jog non c'è Apply
  const side = state.active_side;
  const tgt = state.target[side];
  if (!tgt) return;
  try {
    await jsonFetch('/api/pose_3d/apply', {
      method: 'POST', headers: { 'Content-Type': 'application/json' },
      body: JSON.stringify({
        side, position: tgt.position, quaternion: tgt.quaternion,
        dry_run: true, precise: state.precise,
      }),
    });
    // commit locale: il target diventa il nuovo current (simulato)
    state.current[side] = { position: tgt.position, quaternion: tgt.quaternion };
    state.target[side] = null;
    applyIKToUrdf(side, tgt.position, tgt.quaternion);
    updateRealWrists();
    syncGhostsToTargetOrReal();
    updateReadout();
    $('p3Status').textContent = '✓ Editor: target committato (simulato)';
    $('p3Status').className = 'p3-status ok';
  } catch (e) {
    showError(e.message);
  }
}

function onCancelClick() {
  const side = state.active_side;
  state.target[side] = null;
  syncGhostsToTargetOrReal();
  updateReadout();
}

async function onSyncWithRobot() {
  // Force-refresh dallo stato del robot e ricopia la posa attuale nel target
  // (cosi' gli input riflettono il robot, e l'utente puo' partire da li').
  clearError();
  try {
    await pollState();  // refresha state.current dal backend
  } catch (e) {
    // soft fail; pollState gia' logga internamente
  }
  const side = state.active_side;
  if (!side) return;
  state.target[side] = null;
  // Forza il ghost (e quindi gli input) ad allinearsi sul reale (con fallback arm)
  syncGhostsToTargetOrReal();
  updateReadout();
  $('p3Status').textContent = '✓ Sincronizzato con posa robot';
  $('p3Status').className = 'p3-status ok';
}

function showError(msg) {
  $('p3ErrorBox').style.display = 'block';
  $('p3ErrorText').textContent = msg;
  $('p3Status').textContent = '✗ errore';
  $('p3Status').className = 'p3-status error';
}

function clearError() {
  $('p3ErrorBox').style.display = 'none';
  $('p3ErrorText').textContent = '';
}

function bindEditorButtons() {
  $('p3ApplyBtn').addEventListener('click', onApplyClick);
  $('p3CancelBtn').addEventListener('click', onCancelClick);
  $('p3SyncBtn').addEventListener('click', onSyncWithRobot);
}

// ----- mode toggle -----
function bindModeControls() {
  for (const r of document.querySelectorAll('input[name=p3Mode]')) {
    r.addEventListener('change', () => {
      state.mode = r.value;
      const badge = $('p3ModeBadge');
      if (state.mode === 'jog') {
        badge.textContent = '🔴 JOG (live)';
        badge.classList.add('jog');
        $('p3EditorActions').style.display = 'none';
      } else {
        badge.textContent = 'Editor';
        badge.classList.remove('jog');
        $('p3EditorActions').style.display = '';
      }
    });
  }
}

function bindPreciseControl() {
  $('p3Precise').addEventListener('change', (e) => {
    state.precise = e.target.checked;
  });
}

// ----- jog auto-apply al mouseup gizmo -----
function bindJogAutoApply() {
  const { transform } = state.three;
  transform.addEventListener('dragging-changed', async (e) => {
    if (e.value) return;  // inizio drag, niente
    if (state.mode !== 'jog') return;
    const side = state.active_side;
    const tgt = state.target[side];
    if (!tgt) return;
    const cur = state.current[side];
    if (!cur) return;
    const dx = tgt.position[0] - cur.position[0];
    const dy = tgt.position[1] - cur.position[1];
    const dz = tgt.position[2] - cur.position[2];
    const delta = Math.sqrt(dx*dx + dy*dy + dz*dz);
    if (delta > state.live_max_step_m) {
      const ok = confirm(
        `Movimento richiesto ${(delta*1000).toFixed(0)} mm > soglia ${(state.live_max_step_m*1000).toFixed(0)} mm.\n` +
        `Procedere comunque?`,
      );
      if (!ok) return;
    }
    clearError();
    try {
      const res = await jsonFetch('/api/pose_3d/apply', {
        method: 'POST', headers: { 'Content-Type': 'application/json' },
        body: JSON.stringify({
          side, position: tgt.position, quaternion: tgt.quaternion,
          dry_run: false, precise: state.precise,
        }),
      });
      const r = res.result || {};
      if (r.reached_target === false || r.stalled === true) {
        showError(`Move non completato: residual ${(r.position_error_mm||0).toFixed(1)} mm. ` +
                  `Sblocca braccia e riprova, oppure disattiva Precise.`);
      } else {
        $('p3Status').textContent = `✓ Jog: raggiunto (residual ${(r.position_error_mm||0).toFixed(1)} mm)`;
        $('p3Status').className = 'p3-status ok';
        applyIKToUrdf(side, tgt.position, tgt.quaternion);
        state.target[side] = null;
        syncGhostsToTargetOrReal();
      }
    } catch (e) {
      showError(e.message);
    }
  });
}

// ----- named poses -----
async function onSavePose() {
  const name = ($('p3PoseName').value || '').trim();
  if (!name) { alert('Inserisci un nome'); return; }
  if (!/^[A-Za-z0-9_-]{1,40}$/.test(name)) {
    alert('Nome: solo [A-Za-z0-9_-], 1-40 caratteri');
    return;
  }
  const side = state.active_side;
  const pose = state.target[side] || state.current[side];
  if (!pose) { alert('Nessuna posa da salvare'); return; }
  if (state.saved_poses[name]) {
    if (!confirm(`Posa "${name}" esiste già — sovrascrivere?`)) return;
  }
  try {
    await jsonFetch(`/api/pose_3d/poses/${encodeURIComponent(name)}`, {
      method: 'PUT', headers: { 'Content-Type': 'application/json' },
      body: JSON.stringify({
        side, position: pose.position, quaternion: pose.quaternion,
      }),
    });
    $('p3PoseName').value = '';
    await pollState();  // refresh dropdown
    $('p3PoseSelect').value = name;
  } catch (e) {
    showError(`Salvataggio fallito: ${e.message}`);
  }
}

async function onGoPose() {
  const name = $('p3PoseSelect').value;
  if (!name) { alert('Seleziona una posa dal dropdown'); return; }
  if (state.mode === 'jog') {
    if (!confirm(`Eseguire posa "${name}" SUL ROBOT REALE? (Jog mode)`)) return;
  }
  const dry_run = (state.mode === 'editor');
  clearError();
  try {
    const res = await jsonFetch(
      `/api/pose_3d/poses/${encodeURIComponent(name)}/go`,
      {
        method: 'POST', headers: { 'Content-Type': 'application/json' },
        body: JSON.stringify({ dry_run, precise: state.precise }),
      },
    );
    const r = res.result || {};
    if (r.reached_target === false || r.stalled === true) {
      showError(`Vai a "${name}": residual ${(r.position_error_mm||0).toFixed(1)} mm`);
    } else {
      $('p3Status').textContent = `✓ Posa "${name}" applicata` +
        (dry_run ? ' (simulato)' : '');
      $('p3Status').className = 'p3-status ok';
      const pose = state.saved_poses[name];
      if (pose) applyIKToUrdf(pose.side, pose.position, pose.quaternion);
    }
  } catch (e) {
    showError(e.message);
  }
}

async function onDeletePose() {
  const name = $('p3PoseSelect').value;
  if (!name) { alert('Seleziona una posa dal dropdown'); return; }
  if (!confirm(`Cancellare definitivamente la posa "${name}"?`)) return;
  try {
    await jsonFetch(`/api/pose_3d/poses/${encodeURIComponent(name)}`, {
      method: 'DELETE',
    });
    await pollState();
  } catch (e) {
    showError(e.message);
  }
}

function bindPoseButtons() {
  $('p3PoseSaveBtn').addEventListener('click', onSavePose);
  $('p3PoseGoBtn').addEventListener('click', onGoPose);
  $('p3PoseDelBtn').addEventListener('click', onDeletePose);
}

async function onUnlock() {
  clearError();
  try {
    await jsonFetch('/api/pose_3d/unlock', { method: 'POST' });
    $('p3Status').textContent = '✓ Braccia sbloccate';
    $('p3Status').className = 'p3-status ok';
  } catch (e) {
    showError(`Unlock fallito: ${e.message}`);
  }
}

function bindUnlockButton() {
  $('p3UnlockBtn').addEventListener('click', onUnlock);
}

// ----- init triggered on first activation -----
let inited = false;
function ensureInit() {
  if (inited) return;
  if (!$('p3Canvas')) return;
  inited = true;
  initScene();
  setupSelection();
  bindSideControls();
  bindGizmoControls();
  bindGizmoEvents();
  bindEditorButtons();
  bindModeControls();
  bindPreciseControl();
  bindJogAutoApply();
  bindPoseButtons();
  bindUnlockButton();
  bindReadoutInputs();
  pollState();
  pollTimer = setInterval(pollState, POLL_MS);
}

// Observe quando la tab diventa visibile per inizializzare
const tabObserver = new MutationObserver(() => {
  if ($('tab-pose-3d')?.classList.contains('active')) ensureInit();
});
if ($('tab-pose-3d')) {
  tabObserver.observe($('tab-pose-3d'), { attributes: true, attributeFilter: ['class'] });
  // Se è già attivo all'avvio
  if ($('tab-pose-3d').classList.contains('active')) ensureInit();
}