/* LiDAR - live 3D point cloud from the chest sensor.

   Off until switched on, like the cameras: a scan is 816 KB at 2 Hz and there
   is no reason to carry that when nobody is looking.

   Rendered with a hand-rolled projection onto a 2D canvas rather than a 3D
   library. Points need no lighting, no materials and no scene graph - just a
   matrix multiply and a fillRect - so a library would be a few hundred KB
   fetched over the same link we are trying not to saturate. The robot has no
   internet either, which rules out a CDN.
*/

import { store, get, post, num, toast } from '../core.js';
import { el, card, pageHead, button, badge, note, stat, segmented, toggle } from '../ui.js';

export default {
  id: 'lidar',
  label: 'LiDAR',
  icon: 'scan',

  async render() {
    const spec = store.spec?.lidar || {};
    const key = spec.key || 'lidar_chest_front';

    const root = el('div.stack');
    let active = false;
    let busy = false;
    let timer = null;

    /* -- View state -------------------------------------------------------- */

    const view = {
      yaw: -0.6,          // radians, orbit around Z (up)
      pitch: 0.45,        // radians above the horizon
      distance: 12,       // metres from the origin
      colourBy: 'height',
      accumulate: false,
    };

    let live = [];        // newest scan: [x, y, z, intensity]
    const trailScans = [];  // recent scans, when accumulating
    const MAX_TRAIL = 12;

    /* -- Controls ---------------------------------------------------------- */

    const statusBadge = badge('off');
    const powerBox = el('input', { type: 'checkbox' });
    powerBox.addEventListener('change', () => setActive(powerBox.checked));
    const power = el('label.switch', {}, powerBox, el('span.switch-track'),
                     el('span.switch-label', { text: 'Off' }));
    const powerLabel = power.querySelector('.switch-label');

    root.appendChild(pageHead(
      'LiDAR',
      'Chest LiDAR point cloud, decimated on the robot before it crosses the network. '
      + 'Drag to orbit, scroll to zoom.',
      [power, statusBadge],
    ));

    const tiles = el('div.grid.cols-4');
    const refs = {};
    for (const [tileKey, label] of [
      ['points', 'Points drawn'],
      ['scan', 'Points per scan'],
      ['rate', 'Scan rate'],
      ['range', 'Furthest return'],
    ]) {
      const node = stat(label, '—', { sub: ' ' });
      refs[tileKey] = node;
      tiles.appendChild(node);
    }
    root.appendChild(tiles);

    function setTile(tileKey, value, sub, unit = '') {
      const node = refs[tileKey];
      if (!node) return;
      const valueNode = node.querySelector('.stat-value');
      valueNode.textContent = String(value);
      if (unit) valueNode.appendChild(el('span.unit', { text: unit }));
      node.querySelector('.stat-sub').textContent = sub || '';
    }

    const canvas = el('canvas', {
      width: 1200, height: 720,
      style: { width: '100%', height: 'auto', display: 'block', cursor: 'grab',
               borderRadius: 'var(--radius-sm)', touchAction: 'none' },
    });

    root.appendChild(card('Point cloud', {
      sub: spec.topic || '',
      actions: [
        segmented(
          [{ value: 'height', label: 'Height' },
           { value: 'distance', label: 'Distance' },
           { value: 'intensity', label: 'Intensity' }],
          view.colourBy,
          (value) => { view.colourBy = value; draw(); },
        ),
        toggle('Trail', false, (on) => {
          view.accumulate = on;
          trailScans.length = 0;
          draw();
        }),
        button('Reset view', () => {
          view.yaw = -0.6; view.pitch = 0.45; view.distance = 12;
          draw();
        }, { size: 'sm', style: 'ghost' }),
      ],
      flush: true,
    }, canvas));

    root.appendChild(note(
      'Points are in the sensor frame: X forward, Y left, Z up, metres. The robot decimates '
      + `each scan to ${spec.max_points || 4000} points — the full scan is ~25 000, which looks `
      + 'identical on screen and costs ten times the bandwidth.',
      'info',
    ));

    /* -- Switching --------------------------------------------------------- */

    async function setActive(on) {
      if (busy) return;
      busy = true;
      powerBox.disabled = true;
      statusBadge.textContent = on ? 'starting…' : 'stopping…';
      statusBadge.dataset.tone = 'default';

      try {
        await post(`/api/streams/${key}`, { active: on });
        active = on;
        powerLabel.textContent = on ? 'On' : 'Off';
        if (on) {
          schedule();
        } else {
          if (timer) clearTimeout(timer);
          timer = null;
          live = [];
          trailScans.length = 0;
          statusBadge.textContent = 'off';
          statusBadge.dataset.tone = 'default';
          setTile('points', '—', '');
          setTile('scan', '—', '');
          setTile('rate', '—', '');
          setTile('range', '—', '');
          draw();
        }
      } catch (err) {
        toast('Could not switch the LiDAR', err.message, 'critical');
        powerBox.checked = active;
      } finally {
        busy = false;
        powerBox.disabled = false;
      }
    }

    /* -- Polling ----------------------------------------------------------- */

    // The sensor publishes at 2 Hz, so asking faster than that just re-fetches
    // the same scan. 400 ms keeps the view current without waste.
    const POLL_MS = 400;
    let lastTs = 0;
    let scanTimes = [];

    function schedule() {
      if (timer) clearTimeout(timer);
      timer = active ? setTimeout(fetchPoints, POLL_MS) : null;
    }

    async function fetchPoints() {
      if (!active) return;
      try {
        const data = await get('/api/lidar/points');
        if (data.off) {
          // Something else turned it off - follow rather than fight it.
          active = false;
          powerBox.checked = false;
          powerLabel.textContent = 'Off';
          statusBadge.textContent = 'off';
          statusBadge.dataset.tone = 'default';
          return;
        }
        if (!data.ok) {
          statusBadge.textContent = data.message || 'waiting';
          statusBadge.dataset.tone = 'warning';
          return;
        }

        if (data.ts && data.ts !== lastTs) {
          scanTimes.push(data.ts);
          if (scanTimes.length > 8) scanTimes.shift();
          lastTs = data.ts;

          if (view.accumulate) {
            trailScans.push(live);
            while (trailScans.length > MAX_TRAIL) trailScans.shift();
          }
          live = data.points || [];
        }

        statusBadge.textContent = 'live';
        statusBadge.dataset.tone = 'good';

        const hz = scanTimes.length > 1
          ? (scanTimes.length - 1) / (scanTimes.at(-1) - scanTimes[0])
          : 0;
        const drawn = live.length + (view.accumulate
          ? trailScans.reduce((sum, scan) => sum + scan.length, 0) : 0);
        let furthest = 0;
        for (const p of live) {
          const d = Math.hypot(p[0], p[1], p[2]);
          if (d > furthest) furthest = d;
        }

        setTile('points', drawn.toLocaleString(), view.accumulate ? 'including trail' : 'this scan');
        setTile('scan', (data.count || 0).toLocaleString(), 'after decimation');
        setTile('rate', num(hz, 1), 'measured', ' Hz');
        setTile('range', num(furthest, 1), 'from the sensor', ' m');

        draw();
      } catch {
        statusBadge.textContent = 'no answer';
        statusBadge.dataset.tone = 'warning';
      } finally {
        schedule();
      }
    }

    /* -- Interaction ------------------------------------------------------- */

    let dragging = null;

    canvas.addEventListener('pointerdown', (event) => {
      dragging = { x: event.clientX, y: event.clientY };
      canvas.setPointerCapture(event.pointerId);
      canvas.style.cursor = 'grabbing';
    });

    canvas.addEventListener('pointermove', (event) => {
      if (!dragging) return;
      view.yaw += (event.clientX - dragging.x) * 0.007;
      // Clamped just short of straight down so the view never flips over.
      view.pitch = Math.max(-1.5, Math.min(1.5, view.pitch + (event.clientY - dragging.y) * 0.007));
      dragging = { x: event.clientX, y: event.clientY };
      draw();
    });

    for (const type of ['pointerup', 'pointercancel']) {
      canvas.addEventListener(type, () => { dragging = null; canvas.style.cursor = 'grab'; });
    }

    canvas.addEventListener('wheel', (event) => {
      event.preventDefault();
      view.distance = Math.max(1.5, Math.min(60, view.distance * (1 + Math.sign(event.deltaY) * 0.12)));
      draw();
    }, { passive: false });

    /* -- Rendering --------------------------------------------------------- */

    const context = canvas.getContext('2d');

    // Robot frame is X forward, Y left, Z up. Screen is X right, Y down. The
    // camera orbits the origin looking at it.
    function project(x, y, z, cam) {
      // World -> camera: yaw about Z, then pitch.
      const cy = Math.cos(cam.yaw), sy = Math.sin(cam.yaw);
      const rx = x * cy - y * sy;
      const ry = x * sy + y * cy;

      const cp = Math.cos(cam.pitch), sp = Math.sin(cam.pitch);
      const depth = ry * cp + z * sp + cam.distance;
      const up = -ry * sp + z * cp;

      // Behind the camera - drop it rather than projecting it to a mirrored
      // point somewhere silly on screen.
      if (depth <= 0.05) return null;

      const scale = cam.focal / depth;
      return { sx: cam.cx + rx * scale, sy: cam.cy - up * scale, depth };
    }

    function draw() {
      const width = canvas.width;
      const height = canvas.height;
      const styles = getComputedStyle(document.documentElement);
      const surface = styles.getPropertyValue('--surface-2').trim() || '#1b1e22';
      const gridColor = styles.getPropertyValue('--grid').trim() || '#22262c';
      const axisColor = styles.getPropertyValue('--axis').trim() || '#333941';
      const muted = styles.getPropertyValue('--text-3').trim() || '#6d7681';

      context.fillStyle = surface;
      context.fillRect(0, 0, width, height);

      const cam = {
        yaw: view.yaw, pitch: view.pitch, distance: view.distance,
        focal: height * 0.9, cx: width / 2, cy: height / 2,
      };

      // Ground grid, 1 m squares, so distances are readable at a glance.
      const half = 10;
      context.lineWidth = 1;
      for (let i = -half; i <= half; i += 1) {
        for (const [a, b] of [[[i, -half], [i, half]], [[-half, i], [half, i]]]) {
          const p1 = project(a[0], a[1], 0, cam);
          const p2 = project(b[0], b[1], 0, cam);
          if (!p1 || !p2) continue;
          context.strokeStyle = i === 0 ? axisColor : gridColor;
          context.beginPath();
          context.moveTo(p1.sx, p1.sy);
          context.lineTo(p2.sx, p2.sy);
          context.stroke();
        }
      }

      if (!active) {
        context.fillStyle = muted;
        context.font = '16px system-ui, sans-serif';
        context.textAlign = 'center';
        context.fillText('LiDAR is off — switch it on to see the point cloud',
                         width / 2, height / 2);
        return;
      }

      if (!live.length) {
        context.fillStyle = muted;
        context.font = '15px system-ui, sans-serif';
        context.textAlign = 'center';
        context.fillText('Waiting for the first scan…', width / 2, height / 2);
        return;
      }

      // Older scans first so the newest sits on top.
      const batches = view.accumulate
        ? [...trailScans.map((scan, i) => ({ scan, fade: 0.25 + 0.45 * (i / MAX_TRAIL) })),
           { scan: live, fade: 1 }]
        : [{ scan: live, fade: 1 }];

      for (const { scan, fade } of batches) {
        for (const point of scan) {
          const projected = project(point[0], point[1], point[2], cam);
          if (!projected) continue;
          if (projected.sx < 0 || projected.sx > width
              || projected.sy < 0 || projected.sy > height) continue;

          context.fillStyle = colourFor(point, projected.depth, fade);
          // Nearer points draw larger - the only depth cue a flat point cloud
          // has once perspective alone stops being obvious.
          const size = Math.max(1, Math.min(3.5, 9 / projected.depth));
          context.fillRect(projected.sx, projected.sy, size, size);
        }
      }

      // Sensor origin, so "where is the robot" is never in doubt.
      const origin = project(0, 0, 0, cam);
      if (origin) {
        context.strokeStyle = styles.getPropertyValue('--series-2').trim() || '#d95926';
        context.lineWidth = 2;
        context.beginPath();
        context.arc(origin.sx, origin.sy, 6, 0, Math.PI * 2);
        context.stroke();
      }

      context.fillStyle = muted;
      context.font = '11px system-ui, sans-serif';
      context.textAlign = 'left';
      context.fillText('1 m grid · X forward, Y left, Z up', 12, height - 12);
    }

    function colourFor(point, depth, fade) {
      let t;
      if (view.colourBy === 'height') {
        t = (point[2] + 1.5) / 4.0;              // -1.5 m .. 2.5 m
      } else if (view.colourBy === 'intensity') {
        t = (point[3] || 0) / 150;
      } else {
        t = Math.hypot(point[0], point[1], point[2]) / 15;
      }
      t = Math.max(0, Math.min(1, t));

      // Blue -> cyan -> green -> yellow -> red. Enough steps that a wall at a
      // constant height reads as one colour rather than a gradient.
      const stops = [[59, 130, 246], [34, 211, 238], [74, 222, 128],
                     [250, 204, 21], [239, 68, 68]];
      const scaled = t * (stops.length - 1);
      const index = Math.min(stops.length - 2, Math.floor(scaled));
      const frac = scaled - index;
      const a = stops[index], b = stops[index + 1];
      const r = Math.round(a[0] + (b[0] - a[0]) * frac);
      const g = Math.round(a[1] + (b[1] - a[1]) * frac);
      const bl = Math.round(a[2] + (b[2] - a[2]) * frac);
      return `rgba(${r},${g},${bl},${fade})`;
    }

    /* -- Follow the robot's own state -------------------------------------- */

    function applyStreams(streams) {
      if (!streams || busy) return;
      const on = !!streams[key]?.active;
      if (on === active) return;
      active = on;
      powerBox.checked = on;
      powerLabel.textContent = on ? 'On' : 'Off';
      if (on) {
        schedule();
      } else {
        if (timer) clearTimeout(timer);
        timer = null;
        live = [];
        trailScans.length = 0;
        statusBadge.textContent = 'off';
        statusBadge.dataset.tone = 'default';
        draw();
      }
    }

    applyStreams(store.state?.custom?.streams);
    const unsubscribe = store.on('state', () => applyStreams(store.state?.custom?.streams));

    draw();

    return {
      node: root,
      dispose: () => {
        unsubscribe();
        if (timer) clearTimeout(timer);
        // Never leave the sensor streaming into a page that has gone away.
        if (active) post(`/api/streams/${key}`, { active: false }).catch(() => {});
      },
    };
  },
};
