saltylab-firmware/ui/gps_map_panel.js

/**
 * gps_map_panel.js — Robot GPS Live Map (Issue #709 companion)
 *
 * Subscribes via rosbridge to:
 *   saltybot/gps/fix  std_msgs/String JSON — {lat, lon, alt, stat, t}
 *   saltybot/gps/vel  std_msgs/String JSON — {spd, hdg, t}
 *
 * Renders a Leaflet map with:
 *   - Live robot position marker with heading arrow
 *   - Orange breadcrumb trail polyline (up to MAX_TRAIL points)
 *   - Sidebar: speed, altitude, heading compass, fix status, coordinates
 *   - FOLLOW mode (auto-pan) toggled by button or manual drag
 *   - Stale detection at 5 s
 */

'use strict';

// ── Constants ─────────────────────────────────────────────────────────────────

const MAX_TRAIL      = 2000;
const MAX_LOG        = 80;
const STALE_MS       = 5000;
const RECONNECT_BASE = 2000;
const RECONNECT_MAX  = 30000;
const RECONNECT_MUL  = 1.5;

// ── State ─────────────────────────────────────────────────────────────────────

let ros = null;

const gps = {
  lat: null, lon: null, alt: null, stat: null,
  spd: null, hdg: null,
  fixes: 0, msgs: 0,
  tsfix: 0, tsvel: 0,
};

let trail      = [];   // [{lat, lon}]
let logEntries = [];
let followMode = true;

let reconnDelay = RECONNECT_BASE;
let reconnTimer = null;
let reconnTick  = null;

// ── DOM ───────────────────────────────────────────────────────────────────────

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

// ── Leaflet map ───────────────────────────────────────────────────────────────

const map = L.map('map', { center: [43.45, -79.76], zoom: 15, zoomControl: true });

L.tileLayer('https://{s}.tile.openstreetmap.org/{z}/{x}/{y}.png', {
  attribution: '© OpenStreetMap contributors',
  maxZoom: 19,
}).addTo(map);

// Heading-aware SVG marker icon (rotated via CSS transform on the div)
function makeIcon(hdg) {
  const rot = (hdg != null && hdg >= 0) ? hdg : 0;
  return L.divIcon({
    className: '',
    html: `<svg width="28" height="36" viewBox="0 0 28 36" xmlns="http://www.w3.org/2000/svg"
              style="transform:rotate(${rot}deg);transform-origin:14px 20px">
      <!-- body circle -->
      <circle cx="14" cy="20" r="10" fill="#06b6d4" fill-opacity=".3" stroke="#06b6d4" stroke-width="2"/>
      <circle cx="14" cy="20" r="4"  fill="#06b6d4"/>
      <!-- heading arrow (points up = north when hdg=0) -->
      <polygon points="14,2 10,14 18,14" fill="#f97316" opacity=".9"/>
    </svg>`,
    iconSize:   [28, 36],
    iconAnchor: [14, 20],
  });
}

let marker   = null;
let polyline = L.polyline([], {
  color: '#f97316', weight: 2.5, opacity: .8, lineJoin: 'round',
}).addTo(map);

// ── Canvas compass ────────────────────────────────────────────────────────────

function drawCompass(hdg) {
  const canvas = $('compass-canvas');
  const ctx = canvas.getContext('2d');
  const W = canvas.width, H = canvas.height, cx = W / 2, cy = H / 2;
  const r = cx - 4;

  ctx.clearRect(0, 0, W, H);
  ctx.beginPath(); ctx.arc(cx, cy, r, 0, Math.PI * 2);
  ctx.fillStyle = '#070712'; ctx.fill();
  ctx.strokeStyle = '#1e3a5f'; ctx.lineWidth = 1.5; ctx.stroke();

  const cards = ['N', 'E', 'S', 'W'];
  ctx.font = 'bold 8px monospace'; ctx.textAlign = 'center'; ctx.textBaseline = 'middle';
  cards.forEach((c, i) => {
    const a  = i * Math.PI / 2;
    const lx = cx + Math.sin(a) * (r - 9);
    const ly = cy - Math.cos(a) * (r - 9);
    ctx.fillStyle = c === 'N' ? '#ef4444' : '#4b5563';
    ctx.fillText(c, lx, ly);
  });

  if (hdg == null || hdg < 0) {
    ctx.fillStyle = '#374151'; ctx.font = '9px monospace';
    ctx.textAlign = 'center'; ctx.textBaseline = 'middle';
    ctx.fillText('—', cx, cy);
    return;
  }

  const rad     = (hdg - 90) * Math.PI / 180;
  const tipLen  = r - 14;
  const tailLen = 9;

  ctx.save(); ctx.translate(cx, cy);
  ctx.beginPath();
  ctx.moveTo(Math.cos(rad) * tipLen,       Math.sin(rad) * tipLen);
  ctx.lineTo(Math.cos(rad + Math.PI) * tailLen, Math.sin(rad + Math.PI) * tailLen);
  ctx.strokeStyle = '#f97316'; ctx.lineWidth = 2.5; ctx.lineCap = 'round'; ctx.stroke();
  ctx.beginPath(); ctx.arc(0, 0, 3, 0, Math.PI * 2);
  ctx.fillStyle = '#9ca3af'; ctx.fill();
  ctx.restore();
}

// ── Render ────────────────────────────────────────────────────────────────────

function setSysBadge(id, level, text) {
  const el = $(id); if (!el) return;
  el.className = `sys-badge badge-${level}`;
  if (text !== undefined) el.textContent = text;
}

function render() {
  const now = Date.now();

  // Stale badges
  const fixStale = now - gps.tsfix > STALE_MS;
  const velStale = now - gps.tsvel > STALE_MS;
  setSysBadge('badge-fix', fixStale ? 'stale' : 'ok', fixStale ? 'STALE' : 'LIVE');
  setSysBadge('badge-vel', velStale ? 'stale' : 'ok', velStale ? 'STALE' : 'LIVE');

  // Speed
  const spdKmh = gps.spd != null ? gps.spd : null;
  const spdTxt = spdKmh != null ? spdKmh.toFixed(1) : '—';
  const spdColor = spdKmh == null ? '#6b7280'
                 : spdKmh < 2    ? '#22c55e'
                 : spdKmh < 30   ? '#f97316'
                 :                  '#ef4444';
  $('val-speed').textContent = spdTxt;
  $('val-speed').style.color = spdColor;

  // Altitude
  $('val-alt').textContent = gps.alt != null ? gps.alt.toFixed(1) : '—';

  // Heading
  const hdg = gps.hdg != null && gps.hdg >= 0 ? Math.round(gps.hdg) : null;
  $('val-hdg').textContent = hdg != null ? hdg + '°' : '—';
  drawCompass(gps.hdg);

  // Fix status
  const statMap = { 0: 'NO FIX', 1: 'GPS', 2: 'DGPS', 3: 'PPS', 4: 'RTK', 5: 'RTK-F', 6: 'EST' };
  $('val-stat').textContent = gps.stat != null ? (statMap[gps.stat] || gps.stat) : '—';
  $('val-stat').style.color = gps.stat ? '#22c55e' : '#ef4444';

  $('val-fixes').textContent = gps.fixes;
  $('bb-fixes').textContent  = gps.fixes;
  $('bb-msgs').textContent   = gps.msgs;
  $('bb-trail').textContent  = trail.length;

  // Coordinates
  $('val-lat').textContent = gps.lat != null ? gps.lat.toFixed(6) : '—';
  $('val-lon').textContent = gps.lon != null ? gps.lon.toFixed(6) : '—';
}

function updateMap(lat, lon, hdg) {
  const icon = makeIcon(hdg);

  if (!marker) {
    marker = L.marker([lat, lon], { icon }).addTo(map);
    if (followMode) map.setView([lat, lon], 16);
  } else {
    marker.setIcon(icon);
    marker.setLatLng([lat, lon]);
    if (followMode) map.panTo([lat, lon]);
  }

  trail.push({ lat, lon });
  if (trail.length > MAX_TRAIL) trail.shift();
  polyline.setLatLngs(trail.map(p => [p.lat, p.lon]));
}

function addLog(lat, lon, spd, t) {
  const time = new Date((t || Date.now() / 1000) * 1000)
    .toLocaleTimeString('en-US', { hour12: false });
  const spdTxt = spd != null ? spd.toFixed(1) + ' km/h' : '?';
  logEntries.unshift({ time, lat, lon, spdTxt });
  if (logEntries.length > MAX_LOG) logEntries.pop();

  $('trail-log').innerHTML = logEntries.map(e =>
    `<div class="trail-entry">${e.time} <span class="te-coord">${e.lat.toFixed(5)},${e.lon.toFixed(5)}</span> <span class="te-spd">${e.spdTxt}</span></div>`
  ).join('');
}

// ── ROS / auto-reconnect ──────────────────────────────────────────────────────

function cancelReconn() {
  if (reconnTimer) { clearTimeout(reconnTimer);  reconnTimer = null; }
  if (reconnTick)  { clearInterval(reconnTick);  reconnTick  = null; }
}

function scheduleReconn(url) {
  cancelReconn();
  let secs = Math.round(reconnDelay / 1000);
  $('conn-label').textContent = `Retry in ${secs}s…`;
  reconnTick = setInterval(() => {
    secs = Math.max(0, secs - 1);
    if (secs > 0) $('conn-label').textContent = `Retry in ${secs}s…`;
  }, 1000);
  reconnTimer = setTimeout(() => { reconnTimer = null; connect(url, true); }, reconnDelay);
  reconnDelay = Math.min(reconnDelay * RECONNECT_MUL, RECONNECT_MAX);
}

function connect(url, isRetry) {
  cancelReconn();
  if (!isRetry) reconnDelay = RECONNECT_BASE;
  if (ros) { try { ros.close(); } catch(_) {} }
  ros = new ROSLIB.Ros({ url });

  ros.on('connection', () => {
    reconnDelay = RECONNECT_BASE;
    cancelReconn();
    $('conn-dot').className = 'connected';
    $('conn-label').style.color = '#22c55e';
    $('conn-label').textContent = 'Connected';
    $('ws-input').value = url;
    localStorage.setItem('gps_map_ws_url', url);
    setupTopics();
  });

  ros.on('error', () => {
    $('conn-dot').className = 'error';
    $('conn-label').style.color = '#ef4444';
    $('conn-label').textContent = 'Error';
  });

  ros.on('close', () => {
    $('conn-dot').className = '';
    $('conn-label').style.color = '#6b7280';
    scheduleReconn($('ws-input').value.trim());
  });
}

function setupTopics() {
  // saltybot/gps/fix — {lat, lon, alt, stat, t}
  new ROSLIB.Topic({
    ros, name: 'saltybot/gps/fix',
    messageType: 'std_msgs/String', throttle_rate: 500,
  }).subscribe(msg => {
    try {
      const d = JSON.parse(msg.data);
      gps.lat  = d.lat  ?? gps.lat;
      gps.lon  = d.lon  ?? gps.lon;
      gps.alt  = d.alt  ?? gps.alt;
      gps.stat = d.stat ?? gps.stat;
      gps.fixes++;
      gps.msgs++;
      gps.tsfix = Date.now();

      if (gps.lat != null && gps.lon != null) {
        updateMap(gps.lat, gps.lon, gps.hdg);
        addLog(gps.lat, gps.lon, gps.spd, d.t);
      }

      $('last-update').textContent = 'Last: ' + new Date().toLocaleTimeString('en-US', { hour12: false });
      $('last-msg').textContent    = 'fix @ ' + new Date().toLocaleTimeString('en-US', { hour12: false });
      render();
    } catch(_) {}
  });

  // saltybot/gps/vel — {spd, hdg, t}
  new ROSLIB.Topic({
    ros, name: 'saltybot/gps/vel',
    messageType: 'std_msgs/String', throttle_rate: 500,
  }).subscribe(msg => {
    try {
      const d = JSON.parse(msg.data);
      gps.spd = d.spd ?? gps.spd;
      gps.hdg = d.hdg ?? gps.hdg;
      gps.msgs++;
      gps.tsvel = Date.now();

      // Update marker heading if we have a position
      if (marker && gps.lat != null) {
        marker.setIcon(makeIcon(gps.hdg));
      }
      render();
    } catch(_) {}
  });
}

// ── Stale checker (1 Hz) ──────────────────────────────────────────────────────

setInterval(render, 1000);

// ── Controls ──────────────────────────────────────────────────────────────────

$('btn-connect').addEventListener('click', () => connect($('ws-input').value.trim(), false));
$('ws-input').addEventListener('keydown', e => {
  if (e.key === 'Enter') connect($('ws-input').value.trim(), false);
});

$('btn-follow').addEventListener('click', () => {
  followMode = !followMode;
  $('btn-follow').classList.toggle('active', followMode);
  $('btn-follow').textContent = followMode ? '⊙ FOLLOW' : '⊙ FREE';
  if (followMode && gps.lat != null) map.panTo([gps.lat, gps.lon]);
});

$('btn-clear').addEventListener('click', () => {
  trail = []; logEntries = []; gps.fixes = 0;
  if (marker) { marker.remove(); marker = null; }
  polyline.setLatLngs([]);
  $('trail-log').innerHTML = '';
  render();
});

map.on('dragstart', () => {
  if (followMode) {
    followMode = false;
    $('btn-follow').classList.remove('active');
    $('btn-follow').textContent = '⊙ FREE';
  }
});

// ── Init ──────────────────────────────────────────────────────────────────────

(function init() {
  const saved = localStorage.getItem('gps_map_ws_url') || 'ws://localhost:9090';
  $('ws-input').value = saved;
  drawCompass(null);
  connect(saved, false);
})();