/* Navigation - odometry, heading and travelled path.

   This firmware does not expose the documented SLAM command interface
   (/integrated_command, /relocalization_pose, GetStoredMapByName are all absent
   from the live graph), so no mapping controls are offered - a button that
   silently does nothing is worse than no button. What the robot does publish is
   leg odometry, and that is what this tab shows.
*/

import { store, num, RAD2DEG } from '../core.js';
import { el, card, pageHead, button, note, badge, stat, kv, emptyState } from '../ui.js';
import { compass, attitude } from '../charts.js';

export default {
  id: 'navigation',
  label: 'Navigation',
  icon: 'nav',

  async render() {
    const root = el('div.stack');
    const trail = [];
    let following = true;

    root.appendChild(pageHead(
      'Navigation',
      'Where the robot has been, from leg odometry. Values advance while it walks and hold '
      + 'steady when it is parked.',
      [
        button('Clear path', () => { trail.length = 0; drawTrail(); }, { size: 'sm', style: 'ghost' }),
      ],
    ));

    /* -- Tiles ------------------------------------------------------------ */

    const tiles = el('div.grid.cols-4');
    const refs = {};
    for (const [key, label] of [
      ['x', 'Position X'],
      ['y', 'Position Y'],
      ['heading', 'Heading'],
      ['speed', 'Ground speed'],
      ['distance', 'Distance travelled'],
      ['points', 'Path points'],
      ['mode', 'Motion mode'],
      ['status', 'Odometry'],
    ]) {
      const node = stat(label, '—', { sub: ' ' });
      refs[key] = node;
      tiles.appendChild(node);
    }
    root.appendChild(tiles);

    function setTile(key, value, sub, tone = 'default', unit = '') {
      const node = refs[key];
      if (!node) return;
      node.dataset.tone = tone;
      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 || '';
    }

    /* -- Heading and attitude --------------------------------------------- */

    const compassHost = el('div.attitude');
    const attitudeHost = el('div.attitude');
    const readoutHost = el('div');
    const trailCanvas = el('canvas', {
      width: 960, height: 620,
      style: { width: '100%', height: 'auto', display: 'block', borderRadius: 'var(--radius-sm)' },
    });

    root.appendChild(el('div.grid.split', {},
      card('Heading and attitude', { sub: 'Odometry yaw · chest IMU' },
        el('div.stack', { style: { alignItems: 'center' } },
          el('div.row', { style: { justifyContent: 'center', gap: '14px', flexWrap: 'wrap' } },
            compassHost, attitudeHost,
          ),
          readoutHost,
        ),
      ),
      card('Travelled path', {
        sub: 'Top-down, 1 m grid',
        actions: [
          (() => {
            const box = el('input', { type: 'checkbox', checked: true });
            box.addEventListener('change', () => { following = box.checked; drawTrail(); });
            return el('label.switch', {}, box, el('span.switch-track'),
                      el('span.switch-label', { text: 'Follow robot' }));
          })(),
        ],
        flush: true,
      }, trailCanvas),
    ));

    root.appendChild(note(
      'SLAM mapping and relocalization are not offered here: this firmware does not advertise the '
      + '/integrated_command topic or the GetStoredMapByName service, so there is nothing for the '
      + 'dashboard to call. Use the AGIBOT mobile app for map building on this unit.',
      'info',
    ));

    /* -- Trail rendering --------------------------------------------------- */

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

    function drawTrail() {
      const width = trailCanvas.width;
      const height = trailCanvas.height;
      const styles = getComputedStyle(document.documentElement);
      const surface = styles.getPropertyValue('--surface-2').trim() || '#1b1e22';
      const gridColor = styles.getPropertyValue('--grid').trim() || '#22262c';
      const axis = styles.getPropertyValue('--axis').trim() || '#333941';
      const line = styles.getPropertyValue('--series-1').trim() || '#3987e5';
      const marker = styles.getPropertyValue('--series-2').trim() || '#d95926';
      const muted = styles.getPropertyValue('--text-3').trim() || '#6d7681';

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

      if (!trail.length) {
        context.fillStyle = muted;
        context.font = '15px system-ui, sans-serif';
        context.textAlign = 'center';
        context.fillText('Waiting for odometry — walk the robot to draw a path',
                         width / 2, height / 2);
        return;
      }

      const xs = trail.map((p) => p.x);
      const ys = trail.map((p) => p.y);
      const last = trail.at(-1);

      const spanX = Math.max(2, Math.max(...xs) - Math.min(...xs));
      const spanY = Math.max(2, Math.max(...ys) - Math.min(...ys));
      const pad = 44;
      const scale = Math.min((width - pad * 2) / spanX, (height - pad * 2) / spanY, 110);

      const centreX = following ? last.x : (Math.min(...xs) + Math.max(...xs)) / 2;
      const centreY = following ? last.y : (Math.min(...ys) + Math.max(...ys)) / 2;

      // Robot X is forward and Y is left; canvas y grows downward, so flip it.
      const X = (x) => width / 2 + (x - centreX) * scale;
      const Y = (y) => height / 2 - (y - centreY) * scale;

      const step = scale >= 60 ? 1 : scale >= 25 ? 2 : 5;
      context.lineWidth = 1;
      const firstX = Math.floor(centreX - width / 2 / scale) - 1;
      const lastX = Math.ceil(centreX + width / 2 / scale) + 1;
      const firstY = Math.floor(centreY - height / 2 / scale) - 1;
      const lastY = Math.ceil(centreY + height / 2 / scale) + 1;

      for (let m = Math.ceil(firstX / step) * step; m <= lastX; m += step) {
        context.strokeStyle = m === 0 ? axis : gridColor;
        context.beginPath(); context.moveTo(X(m), 0); context.lineTo(X(m), height); context.stroke();
      }
      for (let m = Math.ceil(firstY / step) * step; m <= lastY; m += step) {
        context.strokeStyle = m === 0 ? axis : gridColor;
        context.beginPath(); context.moveTo(0, Y(m)); context.lineTo(width, Y(m)); context.stroke();
      }

      // Scale key.
      context.fillStyle = muted;
      context.font = '11px system-ui, sans-serif';
      context.textAlign = 'left';
      context.fillText(`${step} m grid`, 12, height - 12);

      if (trail.length > 1) {
        context.strokeStyle = line;
        context.lineWidth = 2;
        context.lineJoin = 'round';
        context.lineCap = 'round';
        context.beginPath();
        trail.forEach((point, index) => {
          const px = X(point.x), py = Y(point.y);
          if (index === 0) context.moveTo(px, py); else context.lineTo(px, py);
        });
        context.stroke();

        // Start marker with a surface ring so it stays legible over the line.
        const start = trail[0];
        context.beginPath();
        context.arc(X(start.x), Y(start.y), 5, 0, Math.PI * 2);
        context.fillStyle = surface;
        context.fill();
        context.strokeStyle = line;
        context.lineWidth = 2;
        context.stroke();
      }

      // Robot marker, pointing along its heading.
      context.save();
      context.translate(X(last.x), Y(last.y));
      context.rotate(-last.yaw);
      context.fillStyle = marker;
      context.strokeStyle = surface;
      context.lineWidth = 2.5;
      context.beginPath();
      context.moveTo(14, 0); context.lineTo(-9, 8); context.lineTo(-4, 0); context.lineTo(-9, -8);
      context.closePath();
      context.fill();
      context.stroke();
      context.restore();
    }

    /* -- Painting ---------------------------------------------------------- */

    let distance = 0;

    function paint() {
      const s = store.state;
      if (!s) return;

      const odom = s.odom || { x: 0, y: 0, yaw: 0 };
      const vel = s.velocity || {};
      const chest = s.imu?.chest || {};

      const previous = trail.at(-1);
      const moved = !previous || Math.hypot(odom.x - previous.x, odom.y - previous.y) > 0.015;
      if (moved) {
        if (previous) distance += Math.hypot(odom.x - previous.x, odom.y - previous.y);
        trail.push({ x: odom.x, y: odom.y, yaw: odom.yaw });
        if (trail.length > 4000) trail.shift();
      } else if (previous) {
        previous.yaw = odom.yaw;
      }

      const speed = Math.hypot(vel.forward || 0, vel.lateral || 0);
      const yawDeg = ((odom.yaw || 0) * RAD2DEG + 360) % 360;

      setTile('x', num(odom.x, 2), 'forward positive', 'default', ' m');
      setTile('y', num(odom.y, 2), 'left positive', 'default', ' m');
      setTile('heading', num(yawDeg, 0), 'counter-clockwise positive', 'default', '°');
      setTile('speed', num(speed, 2), `yaw ${num(vel.angular, 2)} rad/s`, 'default', ' m/s');
      setTile('distance', num(distance, 1), 'since this tab opened', 'default', ' m');
      setTile('points', trail.length, 'samples held');

      const modeSpec = store.spec?.modes?.find((m) => m.id === s.mode);
      setTile('mode', modeSpec?.label || s.mode || '—', modeSpec?.group || '',
              modeSpec?.danger ? 'critical' : 'default');

      const stat = s.topic_stats?.['/aima/mc/leg_odometry'];
      const age = stat ? (Date.now() / 1000) - stat.last : null;
      const live = age !== null && age < 2;
      setTile('status', live ? 'Live' : 'Idle',
              live ? `${num(stat.hz, 0)} Hz` : 'no recent messages',
              live ? 'good' : 'default');

      compassHost.replaceChildren(compass(odom.yaw || 0, { size: 150 }));
      attitudeHost.replaceChildren(attitude(chest.roll || 0, chest.pitch || 0, { size: 150 }));

      readoutHost.replaceChildren(kv([
        ['Roll', `${num((chest.roll || 0) * RAD2DEG, 2)}°`],
        ['Pitch', `${num((chest.pitch || 0) * RAD2DEG, 2)}°`],
        ['Yaw (odometry)', `${num(yawDeg, 1)}°`],
        ['Forward', `${num(vel.forward, 3)} m/s`],
        ['Lateral', `${num(vel.lateral, 3)} m/s`],
        ['Yaw rate', `${num(vel.angular, 3)} rad/s`],
      ]));

      drawTrail();
    }

    paint();

    let last = 0;
    const unsubscribe = store.on('state', () => {
      const now = Date.now();
      if (now - last < 250) return;
      last = now;
      paint();
    });

    return { node: root, dispose: unsubscribe };
  },
};