// 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';

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 makeWristMarker(color) {
  const group = new THREE.Group();
  const sphere = new THREE.Mesh(
    new THREE.SphereGeometry(0.025, 16, 12),
    new THREE.MeshStandardMaterial({ color, metalness: 0.2, roughness: 0.5 }),
  );
  group.add(sphere);
  group.add(makeUCS(0.07));
  group.userData.sphere = sphere;
  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,
};

// Posizioni torso e spalle hardcoded (riferimento visivo, non IK)
const TORSO_POS = new THREE.Vector3(0, 0, 0.40);
const SHOULDER = {
  right: new THREE.Vector3(0, -0.20, 0.60),
  left:  new THREE.Vector3(0,  0.20, 0.60),
};

// ----- 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);

  // Torso
  const torso = new THREE.Mesh(
    new THREE.BoxGeometry(0.20, 0.40, 0.50),
    new THREE.MeshStandardMaterial({ color: 0x4a5560, transparent: true, opacity: 0.7 }),
  );
  torso.position.copy(TORSO_POS);
  scene.add(torso);

  // Arms
  for (const side of ['right', 'left']) {
    const wrist = makeWristMarker(side === 'right' ? 0x4da3ff : 0xff5e7e);
    wrist.position.copy(SHOULDER[side]);  // posizione di partenza, sarà aggiornata dal polling
    wrist.userData.side = side;
    scene.add(wrist);
    state.arms[side] = wrist;

    // 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');
  // Disable orbit while dragging gizmo
  transform.addEventListener('dragging-changed', (e) => { orbit.enabled = !e.value; });
  scene.add(transform);

  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];
}

// ----- 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)
      const real = state.current[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 {
      ghost.visible = false;
    }
  }
}

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();
    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);
    if (hits.length > 0) {
      const sphere = hits[0].object;
      const side = sphere.parent.userData.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]);
  state.ghosts[side].visible = true;
  updateReadout();
}

function ensureGhostInit(side) {
  // Se non c'è target locale, allinea ghost al reale (preparato per drag)
  if (!state.target[side]) {
    const real = state.current[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]);
    }
  }
}

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

// ----- 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() {
  if (inited) return;
  if (!$('p3Canvas')) return;
  inited = true;
  initScene();
  setupSelection();
  bindSideControls();
  bindGizmoControls();
  bindGizmoEvents();
  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();
}