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

// ── Renderer ──────────────────────────────────────────────────────────────────
const renderer = new THREE.WebGLRenderer({ antialias: true });
renderer.setPixelRatio(Math.min(devicePixelRatio, 2));
renderer.setSize(innerWidth, innerHeight);
document.body.appendChild(renderer.domElement);

// ── Scene ─────────────────────────────────────────────────────────────────────
const scene = new THREE.Scene();
scene.background = new THREE.Color(0x06060f);
scene.fog = new THREE.Fog(0x06060f, 500, 2000);
scene.add(new THREE.GridHelper(1000, 100, 0x111133, 0x0a0a22));
scene.add(new THREE.AxesHelper(5));

// ── Camera + Controls ─────────────────────────────────────────────────────────
const camera = new THREE.PerspectiveCamera(60, innerWidth / innerHeight, 0.1, 5000);
camera.position.set(0, 150, 250);
const controls = new OrbitControls(camera, renderer.domElement);
controls.enableDamping = true;
controls.dampingFactor = 0.08;
controls.minDistance = 1;
controls.maxDistance = 3000;

// ── Layer registry ────────────────────────────────────────────────────────────
const layers = {};

function addLayer(name, obj) {
  layers[name] = layers[name] || [];
  layers[name].push(obj);
  scene.add(obj);
}

// ── Geometry helpers ──────────────────────────────────────────────────────────
function makeLine(xArr, yArr, zArr, color) {
  const pts = xArr.map((x, i) => new THREE.Vector3(x, zArr[i], -yArr[i]));
  const geo = new THREE.BufferGeometry().setFromPoints(pts);
  return new THREE.Line(geo, new THREE.LineBasicMaterial({ color }));
}

function makePoints(xArr, yArr, zArr, colorFn) {
  const pos = new Float32Array(xArr.length * 3);
  const col = new Float32Array(xArr.length * 3);
  for (let i = 0; i < xArr.length; i++) {
    pos[i*3]   = xArr[i];
    pos[i*3+1] = zArr[i];
    pos[i*3+2] = -yArr[i];
    const [r,g,b] = colorFn(i);
    col[i*3]=r; col[i*3+1]=g; col[i*3+2]=b;
  }
  const geo = new THREE.BufferGeometry();
  geo.setAttribute('position', new THREE.BufferAttribute(pos, 3));
  geo.setAttribute('color',    new THREE.BufferAttribute(col, 3));
  return new THREE.Points(geo, new THREE.PointsMaterial({ size: 4, vertexColors: true, sizeAttenuation: true }));
}

function makeFrustum(T_flat) {
  // Wireframe camera pyramid: apex at origin, 1m×0.75m rect at z=1.5m
  const verts = new Float32Array([
    0,0,0,  0.5,0.375,1.5,   0.5,0.375,1.5, -0.5,0.375,1.5,
   -0.5,0.375,1.5,-0.5,-0.375,1.5,  -0.5,-0.375,1.5,0.5,-0.375,1.5,
    0.5,-0.375,1.5,0,0,0,   0,0,0,0.5,0.375,1.5,
  ]);
  const geo = new THREE.BufferGeometry();
  geo.setAttribute('position', new THREE.BufferAttribute(verts, 3));
  const mesh = new THREE.LineSegments(geo,
    new THREE.LineBasicMaterial({ color: 0xffd700, transparent: true, opacity: 0.5 }));
  const m = new THREE.Matrix4();
  // T_flat is row-major 4x4 from Python (need to transpose for Three.js column-major)
  m.set(
    T_flat[0][0],T_flat[0][1],T_flat[0][2],T_flat[0][3],
    T_flat[1][0],T_flat[1][1],T_flat[1][2],T_flat[1][3],
    T_flat[2][0],T_flat[2][1],T_flat[2][2],T_flat[2][3],
    T_flat[3][0],T_flat[3][1],T_flat[3][2],T_flat[3][3],
  );
  mesh.applyMatrix4(m);
  return mesh;
}

// ── RTK color mapping ─────────────────────────────────────────────────────────
const RTK_COLORS = [[0.8,0.3,0.1],[0.9,0.7,0.1],[0.1,0.9,0.2]];

// ── Load data ─────────────────────────────────────────────────────────────────
fetch('/api/trajectory')
  .then(r => r.json())
  .then(d => {
    document.getElementById('status').textContent =
      `traj: ${d.traj_status || 'n/a'}`;

    if (d.usv_gps) {
      const { x, y, z, rtk } = d.usv_gps;
      addLayer('usv_gps', makeLine(x, y, z, 0x3a9fff));
      addLayer('usv_gps', makePoints(x, y, z, i => RTK_COLORS[Math.min(rtk[i], 2)]));
    }

    if (d.auv_depth) {
      const { x, y, z } = d.auv_depth;
      addLayer('auv_depth', makeLine(x, y, z, 0x44ffaa));
    }

    if (d.usbl) {
      const { x, y, z } = d.usbl;
      addLayer('usbl', makePoints(x, y, z, () => [1, 0.42, 0.21]));
    }

    const trajData = d.fused || d.lingbot_local;
    const trajLayer = d.fused ? 'fused' : 'lingbot_local';
    if (trajData) {
      const { x, y, z, T_4x4 } = trajData;
      addLayer(trajLayer, makeLine(x, y, z, d.fused ? 0xffffff : 0xffd700));

      if (T_4x4 && T_4x4.length > 0) {
        const step = Math.max(1, Math.floor(T_4x4.length / 100)); // max 100 frustums
        for (let i = 0; i < T_4x4.length; i += step) {
          addLayer('frustums', makeFrustum(T_4x4[i]));
        }
      }

      // Center camera
      if (x.length > 0) {
        controls.target.set(x[0], z[0], -y[0]);
        camera.position.set(x[0], z[0] + 50, -y[0] + 100);
        controls.update();
      }
    }

    if (d.ply) {
      const { x, y, z } = d.ply;
      addLayer('ply', makePoints(x, y, z, (i) => {
        const t = Math.min(1, Math.max(0, (-z[i]) / 20));
        return [0.2+0.3*t, 0.35+0.2*t, 0.5-0.2*t];
      }));
    }

    // If only USV GPS, center on that
    if (!trajData && d.usv_gps) {
      const { x, y, z } = d.usv_gps;
      const cx = x.reduce((a,b)=>a+b,0)/x.length;
      const cy = y.reduce((a,b)=>a+b,0)/y.length;
      controls.target.set(cx, 0, -cy);
      camera.position.set(cx, 50, -cy + 80);
      controls.update();
    }
  })
  .catch(err => {
    document.getElementById('status').textContent = `Error: ${err}`;
  });

// ── Layer toggles ─────────────────────────────────────────────────────────────
document.querySelectorAll('[data-layer]').forEach(cb => {
  cb.addEventListener('change', e => {
    const name = e.target.dataset.layer;
    (layers[name] || []).forEach(obj => { obj.visible = e.target.checked; });
  });
});

// ── Resize ────────────────────────────────────────────────────────────────────
window.addEventListener('resize', () => {
  camera.aspect = innerWidth / innerHeight;
  camera.updateProjectionMatrix();
  renderer.setSize(innerWidth, innerHeight);
});

// ── Render loop ───────────────────────────────────────────────────────────────
(function animate() {
  requestAnimationFrame(animate);
  controls.update();
  renderer.render(scene, camera);
})();