saltylab-firmware/ui/gamepad_panel.js

/* gamepad_panel.js — Saltybot Gamepad Teleop (Issue #598) */
'use strict';

// ── Constants ──────────────────────────────────────────────────────────────
const MAX_LINEAR  = 1.0;   // absolute max m/s
const MAX_ANGULAR = 2.0;   // absolute max rad/s
const PUBLISH_HZ  = 20;
const PUBLISH_MS  = 1000 / PUBLISH_HZ;

// ── State ──────────────────────────────────────────────────────────────────
const state = {
  connected: false,
  estop: false,
  // Speed limits (0..1 fraction of max)
  linLimit:  0.5,
  angLimit:  0.5,
  deadzone:  0.10,
  // Current command
  linVel:    0,
  angVel:    0,
  // Input source: 'none' | 'keyboard' | 'virtual' | 'gamepad'
  inputSrc:  'none',
  // Keyboard keys held
  keys: { w: false, a: false, s: false, d: false, space: false },
  // Virtual stick drags
  sticks: {
    left:  { active: false, dx: 0, dy: 0 },
    right: { active: false, dx: 0, dy: 0 },
  },
  // Gamepad
  gpIndex:   null,
  gpPrevBtns: [],
  // Stats
  pubCount:  0,
  pubTs:     0,
  pubRate:   0,
};

// ── ROS ────────────────────────────────────────────────────────────────────
let ros = null;
let cmdVelTopic = null;

function rosCmdVelSetup() {
  cmdVelTopic = new ROSLIB.Topic({
    ros,
    name:        '/cmd_vel',
    messageType: 'geometry_msgs/Twist',
  });
}

function publishTwist(lin, ang) {
  if (!ros || !cmdVelTopic || !state.connected) return;
  cmdVelTopic.publish(new ROSLIB.Message({
    linear:  { x: lin, y: 0, z: 0 },
    angular: { x: 0,   y: 0, z: ang },
  }));
  state.pubCount++;
  const now = performance.now();
  if (state.pubTs) {
    const dt = (now - state.pubTs) / 1000;
    state.pubRate = 1 / dt;
  }
  state.pubTs = now;
}

function sendStop() { publishTwist(0, 0); }

// ── Connection ─────────────────────────────────────────────────────────────
const $connDot   = document.getElementById('conn-dot');
const $connLabel = document.getElementById('conn-label');
const $btnConn   = document.getElementById('btn-connect');
const $wsInput   = document.getElementById('ws-input');

function connect() {
  const url = $wsInput.value.trim() || 'ws://localhost:9090';
  if (ros) { try { ros.close(); } catch(_){} }

  $connLabel.textContent = 'Connecting…';
  $connLabel.style.color = '#d97706';
  $connDot.className = '';

  ros = new ROSLIB.Ros({ url });

  ros.on('connection', () => {
    state.connected = true;
    $connDot.className = 'connected';
    $connLabel.style.color = '#22c55e';
    $connLabel.textContent = 'Connected';
    rosCmdVelSetup();
    localStorage.setItem('gp_ws_url', url);
  });

  ros.on('error', () => {
    state.connected = false;
    $connDot.className = 'error';
    $connLabel.style.color = '#ef4444';
    $connLabel.textContent = 'Error';
  });

  ros.on('close', () => {
    state.connected = false;
    $connDot.className = '';
    $connLabel.style.color = '#6b7280';
    $connLabel.textContent = 'Disconnected';
  });
}

$btnConn.addEventListener('click', connect);

// Restore saved URL
const savedUrl = localStorage.getItem('gp_ws_url');
if (savedUrl) {
  $wsInput.value = savedUrl;
  document.getElementById('footer-ws').textContent = savedUrl;
}

// ── E-stop ─────────────────────────────────────────────────────────────────
const $btnEstop  = document.getElementById('btn-estop');
const $btnResume = document.getElementById('btn-resume');
const $estopPanel = document.getElementById('estop-panel');
const $sbEstop   = document.getElementById('sb-estop');

function activateEstop() {
  state.estop = true;
  sendStop();
  $btnEstop.style.display  = 'none';
  $btnResume.style.display = '';
  $btnEstop.classList.add('active');
  $estopPanel.style.display = 'flex';
  $sbEstop.textContent  = 'ACTIVE';
  $sbEstop.style.color  = '#ef4444';
}

function resumeFromEstop() {
  state.estop = false;
  $btnEstop.style.display  = '';
  $btnResume.style.display = 'none';
  $btnEstop.classList.remove('active');
  $estopPanel.style.display = 'none';
  $sbEstop.textContent = 'OFF';
  $sbEstop.style.color = '#6b7280';
}

$btnEstop.addEventListener('click',  activateEstop);
$btnResume.addEventListener('click', resumeFromEstop);

// ── Sliders ────────────────────────────────────────────────────────────────
function setupSlider(id, valId, minVal, maxVal, onUpdate) {
  const slider = document.getElementById(id);
  const valEl  = document.getElementById(valId);
  slider.addEventListener('input', () => {
    const frac = parseInt(slider.value) / 100;
    onUpdate(frac, valEl);
  });
  // Init
  const frac = parseInt(slider.value) / 100;
  onUpdate(frac, valEl);
}

setupSlider('slider-linear', 'val-linear', 0, MAX_LINEAR, (frac, el) => {
  state.linLimit = frac;
  const ms = (frac * MAX_LINEAR).toFixed(2);
  el.textContent = ms + ' m/s';
  document.getElementById('lim-linear').textContent = ms + ' m/s';
});

setupSlider('slider-angular', 'val-angular', 0, MAX_ANGULAR, (frac, el) => {
  state.angLimit = frac;
  const rs = (frac * MAX_ANGULAR).toFixed(2);
  el.textContent = rs + ' rad/s';
  document.getElementById('lim-angular').textContent = rs + ' rad/s';
});

// Deadzone slider (0–40%)
const $dzSlider = document.getElementById('slider-dz');
const $dzVal    = document.getElementById('val-dz');
const $limDz    = document.getElementById('lim-dz');
$dzSlider.addEventListener('input', () => {
  state.deadzone = parseInt($dzSlider.value) / 100;
  $dzVal.textContent  = parseInt($dzSlider.value) + '%';
  $limDz.textContent  = parseInt($dzSlider.value) + '%';
});

// ── Keyboard input ─────────────────────────────────────────────────────────
const KEY_EL = {
  w: document.getElementById('key-w'),
  a: document.getElementById('key-a'),
  s: document.getElementById('key-s'),
  d: document.getElementById('key-d'),
  space: document.getElementById('key-space'),
};

function setKeyState(key, down) {
  if (!(key in state.keys)) return;
  state.keys[key] = down;
  if (KEY_EL[key]) KEY_EL[key].classList.toggle('pressed', down);
}

document.addEventListener('keydown', (e) => {
  if (e.target.tagName === 'INPUT') return;
  switch (e.code) {
    case 'KeyW': case 'ArrowUp':    setKeyState('w', true);     break;
    case 'KeyS': case 'ArrowDown':  setKeyState('s', true);     break;
    case 'KeyA': case 'ArrowLeft':  setKeyState('a', true);     break;
    case 'KeyD': case 'ArrowRight': setKeyState('d', true);     break;
    case 'Space':
      e.preventDefault();
      if (state.estop) { resumeFromEstop(); } else { activateEstop(); }
      break;
  }
});

document.addEventListener('keyup', (e) => {
  switch (e.code) {
    case 'KeyW': case 'ArrowUp':    setKeyState('w', false); break;
    case 'KeyS': case 'ArrowDown':  setKeyState('s', false); break;
    case 'KeyA': case 'ArrowLeft':  setKeyState('a', false); break;
    case 'KeyD': case 'ArrowRight': setKeyState('d', false); break;
  }
});

function getKeyboardCommand() {
  let lin = 0, ang = 0;
  if (state.keys.w) lin += 1;
  if (state.keys.s) lin -= 1;
  if (state.keys.a) ang += 1;
  if (state.keys.d) ang -= 1;
  return { lin, ang };
}

// ── Virtual Joysticks ──────────────────────────────────────────────────────
function setupVirtualStick(canvasId, stickKey, onValue) {
  const canvas = document.getElementById(canvasId);
  const R = canvas.width / 2;
  const ctx = canvas.getContext('2d');
  const stick = state.sticks[stickKey];
  let pointerId = null;

  function draw() {
    const W = canvas.width, H = canvas.height;
    ctx.clearRect(0, 0, W, H);

    // Base circle
    ctx.beginPath();
    ctx.arc(R, R, R - 2, 0, Math.PI * 2);
    ctx.fillStyle = 'rgba(10,10,26,0.9)';
    ctx.fill();
    ctx.strokeStyle = '#1e3a5f';
    ctx.lineWidth = 1.5;
    ctx.stroke();

    // Crosshair
    ctx.strokeStyle = '#374151';
    ctx.lineWidth = 0.5;
    ctx.beginPath(); ctx.moveTo(R, 4); ctx.lineTo(R, H - 4); ctx.stroke();
    ctx.beginPath(); ctx.moveTo(4, R); ctx.lineTo(W - 4, R); ctx.stroke();

    // Deadzone ring
    const dzR = state.deadzone * (R - 10);
    ctx.beginPath();
    ctx.arc(R, R, dzR, 0, Math.PI * 2);
    ctx.strokeStyle = '#374151';
    ctx.lineWidth = 1;
    ctx.setLineDash([3, 3]);
    ctx.stroke();
    ctx.setLineDash([]);

    // Knob
    const kx = R + stick.dx * (R - 16);
    const ky = R + stick.dy * (R - 16);
    const kColor = stick.active ? '#06b6d4' : '#1e3a5f';
    ctx.beginPath();
    ctx.arc(kx, ky, 16, 0, Math.PI * 2);
    ctx.fillStyle = kColor + '44';
    ctx.fill();
    ctx.strokeStyle = kColor;
    ctx.lineWidth = 2;
    ctx.stroke();
  }

  function getOffset(e) {
    const rect = canvas.getBoundingClientRect();
    return {
      x: (e.clientX - rect.left) / rect.width  * canvas.width,
      y: (e.clientY - rect.top)  / rect.height * canvas.height,
    };
  }

  function pointerdown(e) {
    canvas.setPointerCapture(e.pointerId);
    pointerId = e.pointerId;
    stick.active = true;
    canvas.classList.add('active');
    update(e);
  }

  function pointermove(e) {
    if (!stick.active || e.pointerId !== pointerId) return;
    update(e);
  }

  function pointerup(e) {
    if (e.pointerId !== pointerId) return;
    stick.active = false;
    stick.dx = 0;
    stick.dy = 0;
    pointerId = null;
    canvas.classList.remove('active');
    onValue(0, 0);
    draw();
  }

  function update(e) {
    const { x, y } = getOffset(e);
    let dx = (x - R) / (R - 16);
    let dy = (y - R) / (R - 16);
    const dist = Math.sqrt(dx * dx + dy * dy);
    if (dist > 1) { dx /= dist; dy /= dist; }
    stick.dx = dx;
    stick.dy = dy;
    // Apply deadzone
    const norm = Math.sqrt(dx * dx + dy * dy);
    if (norm < state.deadzone) { onValue(0, 0); }
    else {
      const scale = (norm - state.deadzone) / (1 - state.deadzone);
      onValue((-dy / norm) * scale, (-dx / norm) * scale); // up=+lin, left=+ang
    }
    draw();
  }

  canvas.addEventListener('pointerdown', pointerdown);
  canvas.addEventListener('pointermove', pointermove);
  canvas.addEventListener('pointerup',   pointerup);
  canvas.addEventListener('pointercancel', pointerup);

  draw();
  return { draw };
}

let virtLeftLin = 0, virtLeftAng = 0;
let virtRightLin = 0, virtRightAng = 0;

const leftStickDraw  = setupVirtualStick('left-stick',  'left',  (fwd, _)   => { virtLeftLin  = fwd; updateSidebarInput(); }).draw;
const rightStickDraw = setupVirtualStick('right-stick', 'right', (_, turn)  => { virtRightAng = turn; updateSidebarInput(); }).draw;

// Redraw sticks when deadzone changes (for dz ring)
$dzSlider.addEventListener('input', () => {
  if (leftStickDraw)  leftStickDraw();
  if (rightStickDraw) rightStickDraw();
});

// ── Gamepad API ────────────────────────────────────────────────────────────
const $gpDot   = document.getElementById('gp-dot');
const $gpLabel = document.getElementById('gp-label');
const $gpRaw   = document.getElementById('gp-raw');

window.addEventListener('gamepadconnected', (e) => {
  state.gpIndex = e.gamepad.index;
  $gpDot.classList.add('active');
  $gpLabel.textContent = e.gamepad.id.substring(0, 28);
  updateGpRaw();
});

window.addEventListener('gamepaddisconnected', (e) => {
  if (e.gamepad.index === state.gpIndex) {
    state.gpIndex = null;
    state.gpPrevBtns = [];
    $gpDot.classList.remove('active');
    $gpLabel.textContent = 'No gamepad';
    $gpRaw.innerHTML = '<div style="color:#374151;font-size:10px;text-align:center;padding:12px 0">No gamepad connected.<br>Press any button to activate.</div>';
  }
});

function applyDeadzone(v, dz) {
  const sign = v >= 0 ? 1 : -1;
  const abs  = Math.abs(v);
  if (abs < dz) return 0;
  return sign * (abs - dz) / (1 - dz);
}

function getGamepadCommand() {
  if (state.gpIndex === null) return null;
  const gp = navigator.getGamepads()[state.gpIndex];
  if (!gp) return null;

  // Standard mapping:
  // Axes: 0=LX, 1=LY, 2=RX, 3=RY
  // Buttons: 0=A/X, 1=B/Circle, 6=LT, 7=RT, 9=Start
  const dz = state.deadzone;
  const ly  = applyDeadzone(gp.axes[1] || 0, dz);
  const rx  = applyDeadzone(gp.axes[2] || 0, dz);

  // LT/RT fine speed override (reduce max by up to 50%)
  const lt = gp.buttons[6] ? (gp.buttons[6].value || 0) : 0;
  const rt = gp.buttons[7] ? (gp.buttons[7].value || 0) : 0;
  const triggerScale = 1 - (lt * 0.5) - (rt * 0.5) + (rt * 0.5); // RT = full, LT = half
  const speedScale = lt > 0.1 ? (1 - lt * 0.5) : 1.0;

  // B/Circle = e-stop
  const bBtn = gp.buttons[1] && gp.buttons[1].pressed;
  const prevBBtn = state.gpPrevBtns[1] || false;
  if (bBtn && !prevBBtn) {
    if (!state.estop) activateEstop(); else resumeFromEstop();
  }

  // Start = resume
  const startBtn = gp.buttons[9] && gp.buttons[9].pressed;
  const prevStart = state.gpPrevBtns[9] || false;
  if (startBtn && !prevStart && state.estop) resumeFromEstop();

  state.gpPrevBtns = gp.buttons.map(b => b.pressed);

  return { lin: -ly * speedScale, ang: -rx };
}

function updateGpRaw() {
  if (state.gpIndex === null) return;
  const gp = navigator.getGamepads()[state.gpIndex];
  if (!gp) return;

  const AXIS_NAMES = ['LX', 'LY', 'RX', 'RY'];
  const BTN_NAMES  = ['A','B','X','Y','LB','RB','LT','RT','Back','Start',
                      'LS','RS','↑','↓','←','→','Home'];

  let html = '';
  // Axes
  for (let i = 0; i < Math.min(gp.axes.length, 4); i++) {
    const v  = gp.axes[i];
    const pct = ((v + 1) / 2 * 100).toFixed(0);
    const fill = v >= 0
      ? `left:50%;width:${(v/2*100).toFixed(0)}%`
      : `left:${((v+1)/2*100).toFixed(0)}%;width:${(Math.abs(v)/2*100).toFixed(0)}%`;
    html += `<div class="gp-axis-row">
      <span class="gp-axis-label">${AXIS_NAMES[i]||'A'+i}</span>
      <div class="gp-bar-track">
        <div class="gp-bar-center"></div>
        <div class="gp-bar-fill" style="${fill}"></div>
      </div>
      <span class="gp-axis-val">${v.toFixed(2)}</span>
    </div>`;
  }
  // Buttons
  html += '<div class="gp-btn-row">';
  for (let i = 0; i < gp.buttons.length; i++) {
    const pressed = gp.buttons[i].pressed;
    const name = BTN_NAMES[i] || 'B'+i;
    html += `<span class="gp-btn-chip${pressed?' pressed':''}">${name}</span>`;
  }
  html += '</div>';
  $gpRaw.innerHTML = html;
}

// ── Velocity display helpers ───────────────────────────────────────────────
const $dispLinear  = document.getElementById('disp-linear');
const $dispAngular = document.getElementById('disp-angular');
const $leftVals    = document.getElementById('left-vals');
const $rightVals   = document.getElementById('right-vals');
const $sbInput     = document.getElementById('sb-input');
const $sbRate      = document.getElementById('sb-rate');

function updateSidebarInput() {
  const src = state.inputSrc;
  $sbInput.textContent = src === 'gamepad' ? 'Gamepad' :
                          src === 'keyboard' ? 'Keyboard' :
                          src === 'virtual'  ? 'Virtual sticks' : '—';
}

// ── Main publish loop ──────────────────────────────────────────────────────
let lastPublish = 0;

function loop(ts) {
  requestAnimationFrame(loop);

  // Gamepad raw display (60fps)
  if (state.gpIndex !== null) updateGpRaw();

  // Publish at limited rate
  if (ts - lastPublish < PUBLISH_MS) return;
  lastPublish = ts;

  // Determine command from highest-priority active input
  let lin = 0, ang = 0;
  let src = 'none';

  // 1. Gamepad (highest priority if connected)
  const gpCmd = getGamepadCommand();
  if (gpCmd && (Math.abs(gpCmd.lin) > 0.001 || Math.abs(gpCmd.ang) > 0.001)) {
    lin = gpCmd.lin;
    ang = gpCmd.ang;
    src = 'gamepad';
  }
  // 2. Keyboard
  else {
    const kbCmd = getKeyboardCommand();
    if (kbCmd.lin !== 0 || kbCmd.ang !== 0) {
      lin = kbCmd.lin;
      ang = kbCmd.ang;
      src = 'keyboard';
    }
    // 3. Virtual sticks
    else if (state.sticks.left.active || state.sticks.right.active) {
      lin = virtLeftLin;
      ang = virtRightAng;
      src = 'virtual';
    }
  }

  // Apply speed limits
  lin = lin * state.linLimit * MAX_LINEAR;
  ang = ang * state.angLimit * MAX_ANGULAR;

  // Clamp
  lin = Math.max(-MAX_LINEAR,  Math.min(MAX_LINEAR,  lin));
  ang = Math.max(-MAX_ANGULAR, Math.min(MAX_ANGULAR, ang));

  state.linVel = lin;
  state.angVel = ang;
  state.inputSrc = src;

  if (state.estop) { lin = 0; ang = 0; }

  // Publish
  publishTwist(lin, ang);

  // Update displays
  $dispLinear.textContent  = lin.toFixed(2) + ' m/s';
  $dispAngular.textContent = ang.toFixed(2) + ' rad/s';
  $leftVals.textContent    = '↕ ' + lin.toFixed(2) + ' m/s';
  $rightVals.textContent   = '↔ ' + ang.toFixed(2) + ' rad/s';

  updateSidebarInput();
  $sbRate.textContent = state.pubRate > 0 ? state.pubRate.toFixed(1) + ' Hz' : '—';

  // Color command display
  const linColor  = Math.abs(lin)  > 0.01 ? '#22c55e' : '#6b7280';
  const angColor  = Math.abs(ang)  > 0.01 ? '#06b6d4' : '#6b7280';
  $dispLinear.style.color  = linColor;
  $dispAngular.style.color = angColor;
  $leftVals.style.color    = linColor;
  $rightVals.style.color   = angColor;
}

requestAnimationFrame(loop);

// ── Init auto-connect if URL saved ────────────────────────────────────────
if (savedUrl) {
  connect();
}