/**
 * can_monitor_panel.js — CAN Sensor Remote Monitor (Issue #681)
 *
 * Subscribes (via rosbridge WebSocket) to:
 *   /saltybot/imu           sensor_msgs/Imu        — quaternion, ang-vel, lin-accel, cov
 *   /saltybot/balance_state std_msgs/String (JSON) — state, motor_cmd, pid_error_deg, pitch
 *   /saltybot/barometer     std_msgs/String (JSON) — pressure_pa, temp_c, altitude_m
 *   /vesc/left/state        std_msgs/String (JSON) — rpm, current_a, temp_fet_c, voltage_v …
 *   /vesc/right/state       std_msgs/String (JSON) — same
 *
 * Each topic drives a card with update indicator (flashing dot).
 * Stale detection at 3 s; badges flip to STALE automatically.
 */

'use strict';

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

const RPM_MAX         = 6000;
const CURRENT_MAX_A   = 60.0;
const CMD_MAX         = 1000;
const STALE_MS        = 3000;
const TEMP_FET_WARN   = 70;
const TEMP_MOTOR_WARN = 80;

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

let ros = null;

const state = {
  vescL: null,
  vescR: null,
  imu:   null,  // { pitch, roll, yaw, wp, wr, wy, ax, ay, az, cov0 }
  bal:   null,  // parsed JSON from /saltybot/balance_state
  baro:  null,  // { pressure_pa, temp_c, altitude_m }
  ts: { vescL: 0, vescR: 0, imu: 0, balance: 0, baro: 0 },
  msgCount: 0,
  rateLast: performance.now(),
};

// ── DOM helpers ───────────────────────────────────────────────────────────────

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

function setText(id, text, color) {
  const el = $(id);
  if (!el) return;
  el.textContent = text;
  if (color !== undefined) el.style.color = color;
}

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

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

function setGauge(barId, norm, color) {
  const el = $(barId);
  if (!el) return;
  el.style.width = `${Math.max(0, Math.min(1, norm)) * 100}%`;
  if (color) el.style.background = color;
}

function setBidir(barId, value, max, colorPos, colorNeg) {
  const el = $(barId);
  if (!el) return;
  const norm = Math.max(-1, Math.min(1, value / max));
  const pct  = Math.abs(norm) * 50;
  if (norm >= 0) {
    el.style.left  = '50%';
    el.style.width = pct + '%';
    el.style.background = colorPos || '#f97316';
  } else {
    el.style.left  = (50 - pct) + '%';
    el.style.width = pct + '%';
    el.style.background = colorNeg || '#3b82f6';
  }
}

function flashDot(id) {
  const el = $(id);
  if (!el) return;
  el.classList.add('active');
  setTimeout(() => el.classList.remove('active'), 350);
}

function fmtTs(ts) {
  return new Date(ts).toLocaleTimeString('en-US', {
    hour12: false, hour: '2-digit', minute: '2-digit', second: '2-digit',
  });
}

function fmt1(v) { return v == null ? '—' : Number(v).toFixed(1); }
function fmt0(v) { return v == null ? '—' : Math.round(Number(v)).toString(); }

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

function checkStale() {
  const now = Date.now();
  const s   = state.ts;
  [
    ['badge-vesc-l', s.vescL,   'ALIVE', 'STALE'],
    ['badge-vesc-r', s.vescR,   'ALIVE', 'STALE'],
    ['badge-imu',    s.imu,     'LIVE',  'STALE'],
    ['badge-balance',s.balance, 'LIVE',  'STALE'],
    ['badge-baro',   s.baro,    'LIVE',  'STALE'],
  ].forEach(([id, ts, okText, staleText]) => {
    const stale = now - ts > STALE_MS;
    setSysBadge(id, stale ? 'stale' : 'ok', stale ? staleText : okText);
  });
}

setInterval(() => {
  const now  = performance.now();
  const dt   = (now - state.rateLast) / 1000;
  $('hz-display').textContent = dt > 0 ? (state.msgCount / dt).toFixed(1) + ' msg/s' : '—';
  state.msgCount = 0;
  state.rateLast = now;
  checkStale();
}, 1000);

// ── VESC render ───────────────────────────────────────────────────────────────

function renderVesc(side, d) {
  const p = `vesc-${side}`;
  const card = $(`card-vesc-${side}`);

  if (!d) { setText(`${p}-rpm`, '—'); setText(`${p}-voltage`, '—'); setBadge(`${p}-fault-badge`, 'stale', 'STALE'); return; }

  // Fault / alive badge
  let lvl = 'ok', txt = 'OK';
  if (!d.alive)          { lvl = 'error'; txt = 'OFFLINE'; }
  else if (d.fault_code) { lvl = 'error'; txt = d.fault_name || `FAULT ${d.fault_code}`; }
  setBadge(`${p}-fault-badge`, lvl, txt);
  card.className = 'card' + (lvl === 'error' ? ' border-error' : '');

  // Big numerics
  const rpmColor = d.rpm > 0 ? '#f97316' : d.rpm < 0 ? '#3b82f6' : '#6b7280';
  setText(`${p}-rpm`, fmt0(d.rpm), rpmColor);
  const soc    = Math.max(0, Math.min(100, ((d.voltage_v - 12.0) / (16.8 - 12.0)) * 100));
  const vColor = soc > 50 ? '#22c55e' : soc > 20 ? '#f59e0b' : '#ef4444';
  setText(`${p}-voltage`, fmt1(d.voltage_v), vColor);

  // Gauges
  setText(`${p}-rpm-val`, fmt0(d.rpm) + ' rpm');
  setBidir(`${p}-rpm-bar`, d.rpm, RPM_MAX, '#f97316', '#3b82f6');

  const currNorm  = Math.abs(d.current_a) / CURRENT_MAX_A;
  const currColor = currNorm > 0.85 ? '#ef4444' : currNorm > 0.6 ? '#f59e0b' : '#06b6d4';
  setText(`${p}-current-val`, fmt1(d.current_a) + ' A');
  setGauge(`${p}-current-bar`, currNorm, currColor);

  // Stat cells
  setText(`${p}-current-in`, fmt1(d.current_in_a) + ' A');
  setText(`${p}-duty`,       (d.duty_cycle * 100).toFixed(1) + '%');
  setText(`${p}-temp-fet`,   fmt1(d.temp_fet_c)   + '°C', d.temp_fet_c   > TEMP_FET_WARN   ? '#f59e0b' : '#9ca3af');
  setText(`${p}-temp-motor`, fmt1(d.temp_motor_c) + '°C', d.temp_motor_c > TEMP_MOTOR_WARN ? '#f59e0b' : '#9ca3af');
}

// ── Balance render ────────────────────────────────────────────────────────────

const BAL_LEVEL = { running: 'ok', startup: 'warn', tilt_warning: 'warn', tilt_kill: 'error', error: 'error' };

function renderBalance(d) {
  if (!d) { setBadge('balance-state-badge', 'stale', '—'); return; }
  const key = (d.state || '').toLowerCase();
  setBadge('balance-state-badge', BAL_LEVEL[key] || 'stale', (d.state || '—').toUpperCase());

  const cmd = d.motor_cmd ?? 0;
  setText('balance-cmd-val', cmd + ' / ±' + CMD_MAX);
  setBidir('balance-cmd-bar', cmd, CMD_MAX, '#f97316', '#3b82f6');

  const pe = d.pid_error_deg;
  setText('balance-pid-error', pe != null ? fmt1(pe) + '°' : '—');
  setText('balance-integral',  d.integral  != null ? fmt1(d.integral)  : '—');
  setText('balance-pitch',     d.pitch     != null ? fmt1(d.pitch) + '°' : '—');
  setText('balance-frames',    d.frames    != null ? String(d.frames)  : '—');
}

// ── Barometer render ──────────────────────────────────────────────────────────

function renderBaro(d) {
  if (!d) return;
  setText('baro-alt',      d.altitude_m  != null ? fmt1(d.altitude_m)  : '—');
  setText('baro-pressure', d.pressure_pa != null ? fmt0(d.pressure_pa) : '—');
  setText('baro-temp',     d.temp_c      != null ? fmt1(d.temp_c)      : '—');
}

// ── IMU render ────────────────────────────────────────────────────────────────

function renderImu(imu) {
  if (!imu) return;
  setText('imu-pitch', fmt1(imu.pitch), Math.abs(imu.pitch) > 20 ? '#f59e0b' : '#06b6d4');
  setText('imu-roll',  fmt1(imu.roll),  Math.abs(imu.roll)  > 20 ? '#f59e0b' : '#06b6d4');
  setText('imu-yaw',   fmt1((imu.yaw + 360) % 360));
  setText('imu-wp',    fmt1(imu.wp));
  setText('imu-wr',    fmt1(imu.wr));
  setText('imu-wy',    fmt1(imu.wy));
  setText('imu-ax',    fmt1(imu.ax));
  setText('imu-ay',    fmt1(imu.ay));
  setText('imu-az',    fmt1(imu.az));

  // Calibration badge from orientation_covariance[0]
  const c = imu.cov0;
  if (c === -1 || c == null) setBadge('imu-cal-badge', 'error',  'CAL: UNKNOWN');
  else if (c < 0.0001)       setBadge('imu-cal-badge', 'ok',     'CAL: GOOD');
  else if (c < 0.001)        setBadge('imu-cal-badge', 'warn',   'CAL: PARTIAL');
  else                       setBadge('imu-cal-badge', 'error',  'CAL: POOR');

  drawHorizon(imu.pitch, imu.roll);
  drawCompass(imu.yaw);
}

// ── 2D canvas: artificial horizon ────────────────────────────────────────────

function drawHorizon(pitch, roll) {
  const hCanvas = $('horizon-canvas');
  if (!hCanvas) return;
  const ctx = hCanvas.getContext('2d');
  const W = hCanvas.width, H = hCanvas.height;
  const cx = W / 2, cy = H / 2;
  const rollRad = roll  * Math.PI / 180;
  const pitchPx = pitch * (H / 60);

  ctx.clearRect(0, 0, W, H);
  ctx.fillStyle = '#051a30'; ctx.fillRect(0, 0, W, H);
  ctx.save();
  ctx.translate(cx, cy); ctx.rotate(-rollRad);

  ctx.fillStyle = '#1a0f00'; ctx.fillRect(-W, pitchPx, W * 2, H * 2);
  ctx.strokeStyle = '#00ffff'; ctx.lineWidth = 1.5;
  ctx.beginPath(); ctx.moveTo(-W, pitchPx); ctx.lineTo(W, pitchPx); ctx.stroke();

  for (let d = -30; d <= 30; d += 10) {
    if (d === 0) continue;
    const y = pitchPx + d * (H / 60);
    const lw = Math.abs(d) % 20 === 0 ? 22 : 14;
    ctx.strokeStyle = 'rgba(0,210,210,0.4)'; ctx.lineWidth = 0.7;
    ctx.beginPath(); ctx.moveTo(-lw, y); ctx.lineTo(lw, y); ctx.stroke();
    ctx.fillStyle = 'rgba(0,210,210,0.5)'; ctx.font = '7px monospace'; ctx.textAlign = 'left';
    ctx.fillText((-d).toString(), lw + 2, y + 3);
  }
  ctx.restore();

  ctx.strokeStyle = '#f97316'; ctx.lineWidth = 1.5;
  ctx.beginPath();
  ctx.moveTo(cx - 28, cy); ctx.lineTo(cx - 8, cy);
  ctx.moveTo(cx + 8,  cy); ctx.lineTo(cx + 28, cy);
  ctx.moveTo(cx, cy - 4);  ctx.lineTo(cx, cy + 4);
  ctx.stroke();
}

function drawCompass(yaw) {
  const cCanvas = $('compass-canvas');
  if (!cCanvas) return;
  const ctx = cCanvas.getContext('2d');
  const W = cCanvas.width, H = cCanvas.height, cx = W / 2;
  const degPerPx = W / 70;
  const cardinals = { 0:'N', 45:'NE', 90:'E', 135:'SE', 180:'S', 225:'SW', 270:'W', 315:'NW' };

  ctx.clearRect(0, 0, W, H);
  ctx.fillStyle = '#050510'; ctx.fillRect(0, 0, W, H);

  for (let i = -35; i <= 35; i++) {
    const deg = ((Math.round(yaw) + i) % 360 + 360) % 360;
    const x   = cx + i * degPerPx;
    const isMaj = deg % 45 === 0, isMed = deg % 15 === 0;
    if (!isMed && !isMaj) continue;
    ctx.strokeStyle = isMaj ? '#00cccc' : 'rgba(0,200,200,0.3)';
    ctx.lineWidth   = isMaj ? 1.5 : 0.5;
    ctx.beginPath(); ctx.moveTo(x, 0); ctx.lineTo(x, isMaj ? 16 : 7); ctx.stroke();
    if (isMaj && cardinals[deg] !== undefined) {
      ctx.fillStyle = deg === 0 ? '#ff4444' : '#00cccc';
      ctx.font = 'bold 9px monospace'; ctx.textAlign = 'center';
      ctx.fillText(cardinals[deg], x, 28);
    }
  }
  const hdg = ((Math.round(yaw) % 360) + 360) % 360;
  ctx.fillStyle = '#00ffff'; ctx.font = 'bold 11px monospace'; ctx.textAlign = 'center';
  ctx.fillText(hdg + '°', cx, H - 4);
  ctx.strokeStyle = '#f97316'; ctx.lineWidth = 2;
  ctx.beginPath(); ctx.moveTo(cx, 0); ctx.lineTo(cx, 10); ctx.stroke();
}

// ── ROS connection ────────────────────────────────────────────────────────────

// ── Auto-reconnect state ──────────────────────────────────────────────────────

const RECONNECT_BASE_MS = 2000;
const RECONNECT_MAX_MS  = 30000;
const RECONNECT_FACTOR  = 1.5;

let reconnectDelay  = RECONNECT_BASE_MS;
let reconnectTimer  = null;
let reconnectTick   = null;
let reconnectTarget = null;

function cancelReconnect() {
  if (reconnectTimer) { clearTimeout(reconnectTimer);  reconnectTimer = null; }
  if (reconnectTick)  { clearInterval(reconnectTick);  reconnectTick  = null; }
}

function scheduleReconnect(url) {
  cancelReconnect();
  let secs = Math.round(reconnectDelay / 1000);
  $('conn-label').textContent = `Retry in ${secs}s…`;

  reconnectTick = setInterval(() => {
    secs = Math.max(0, secs - 1);
    if (secs > 0) $('conn-label').textContent = `Retry in ${secs}s…`;
  }, 1000);

  reconnectTimer = setTimeout(() => {
    reconnectTimer = null;
    connect(url, true);
  }, reconnectDelay);

  reconnectDelay = Math.min(reconnectDelay * RECONNECT_FACTOR, RECONNECT_MAX_MS);
}

function connect(url, isAutoRetry) {
  cancelReconnect();
  reconnectTarget = url;
  if (!isAutoRetry) reconnectDelay = RECONNECT_BASE_MS;

  if (ros) { try { ros.close(); } catch(_) {} }
  ros = new ROSLIB.Ros({ url });

  ros.on('connection', () => {
    reconnectDelay = RECONNECT_BASE_MS;
    cancelReconnect();
    $('conn-dot').className = 'connected';
    $('conn-label').style.color = '#22c55e';
    $('conn-label').textContent = 'Connected';
    $('ws-input').value = url;
    localStorage.setItem('can_monitor_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';
    scheduleReconnect(reconnectTarget || $('ws-input').value.trim());
  });
}

function setupTopics() {

  // /vesc/left/state
  new ROSLIB.Topic({ ros, name: '/vesc/left/state', messageType: 'std_msgs/String', throttle_rate: 200 })
    .subscribe(msg => {
      try {
        state.vescL = JSON.parse(msg.data);
        state.ts.vescL = Date.now(); state.msgCount++;
        flashDot('dot-vesc-l');
        setText('ts-vesc-l', fmtTs(state.ts.vescL));
        renderVesc('l', state.vescL);
        $('last-update').textContent = 'Last: ' + fmtTs(Date.now());
      } catch(_) {}
    });

  // /vesc/right/state
  new ROSLIB.Topic({ ros, name: '/vesc/right/state', messageType: 'std_msgs/String', throttle_rate: 200 })
    .subscribe(msg => {
      try {
        state.vescR = JSON.parse(msg.data);
        state.ts.vescR = Date.now(); state.msgCount++;
        flashDot('dot-vesc-r');
        setText('ts-vesc-r', fmtTs(state.ts.vescR));
        renderVesc('r', state.vescR);
      } catch(_) {}
    });

  // /saltybot/imu — sensor_msgs/Imu
  new ROSLIB.Topic({ ros, name: '/saltybot/imu', messageType: 'sensor_msgs/Imu', throttle_rate: 100 })
    .subscribe(msg => {
      state.ts.imu = Date.now(); state.msgCount++;
      flashDot('dot-imu');
      setText('ts-imu', fmtTs(state.ts.imu));

      const o  = msg.orientation          || {};
      const av = msg.angular_velocity     || {};
      const la = msg.linear_acceleration  || {};
      const cov = msg.orientation_covariance || [];

      const { x = 0, y = 0, z = 0, w = 1 } = o;
      state.imu = {
        pitch: Math.asin(2 * (w * y - z * x)) * 180 / Math.PI,
        roll:  Math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)) * 180 / Math.PI,
        yaw:   Math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)) * 180 / Math.PI,
        wp: (av.x || 0) * 180 / Math.PI,
        wr: (av.y || 0) * 180 / Math.PI,
        wy: (av.z || 0) * 180 / Math.PI,
        ax: la.x || 0, ay: la.y || 0, az: la.z || 0,
        cov0: cov.length > 0 ? cov[0] : null,
      };
      renderImu(state.imu);
      $('last-update').textContent = 'Last: ' + fmtTs(Date.now());
    });

  // /saltybot/balance_state — std_msgs/String JSON
  new ROSLIB.Topic({ ros, name: '/saltybot/balance_state', messageType: 'std_msgs/String', throttle_rate: 200 })
    .subscribe(msg => {
      try {
        state.bal = JSON.parse(msg.data);
        state.ts.balance = Date.now(); state.msgCount++;
        flashDot('dot-balance');
        setText('ts-balance', fmtTs(state.ts.balance));
        renderBalance(state.bal);
      } catch(_) {}
    });

  // /saltybot/barometer — std_msgs/String JSON
  new ROSLIB.Topic({ ros, name: '/saltybot/barometer', messageType: 'std_msgs/String', throttle_rate: 500 })
    .subscribe(msg => {
      try {
        const d = JSON.parse(msg.data);
        state.baro = {
          pressure_pa: d.pressure_pa ?? d.fluid_pressure ?? null,
          temp_c:      d.temp_c ?? null,
          altitude_m:  d.altitude_m ?? null,
        };
        state.ts.baro = Date.now(); state.msgCount++;
        flashDot('dot-baro');
        setText('ts-baro', fmtTs(state.ts.baro));
        renderBaro(state.baro);
      } catch(_) {}
    });
}

// ── UI wiring ─────────────────────────────────────────────────────────────────

$('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); });

// ── Auto-connect on load ──────────────────────────────────────────────────────

(function init() {
  const saved = localStorage.getItem('can_monitor_ws_url');
  const url   = saved || 'ws://localhost:9090';
  $('ws-input').value = url;
  drawHorizon(0, 0);
  drawCompass(0);
  connect(url);
  if (!saved && location.hostname && location.hostname !== 'localhost') {
    setTimeout(() => { if (!ros || !ros.isConnected) connect(`ws://${location.hostname}:9090`); }, 1500);
  }
})();