// Minimal rosbridge v2 stand-in: hand-rolled RFC6455 (no deps) + synthetic
// FAST-LIVO2-shaped topics, so the viewer's live path can be exercised for real.
import http from 'node:http';
import crypto from 'node:crypto';
import zlib from 'node:zlib';

const PORT = 9090;

// ── websocket framing ────────────────────────────────────────────────────
function accept(key) {
  return crypto.createHash('sha1').update(key + '258EAFA5-E914-47DA-95CA-C5AB0DC85B11').digest('base64');
}
function encodeFrame(str) {
  const payload = Buffer.from(str, 'utf8');
  const len = payload.length;
  let header;
  if (len < 126) { header = Buffer.alloc(2); header[1] = len; }
  else if (len < 65536) { header = Buffer.alloc(4); header[1] = 126; header.writeUInt16BE(len, 2); }
  else { header = Buffer.alloc(10); header[1] = 127; header.writeBigUInt64BE(BigInt(len), 2); }
  header[0] = 0x81;                   // FIN + text
  return Buffer.concat([header, payload]);
}
function* decodeFrames(buf) {         // client → server frames are masked
  let off = 0;
  while (off + 2 <= buf.length) {
    const b1 = buf[off + 1];
    let len = b1 & 127, p = off + 2;
    if (len === 126) { len = buf.readUInt16BE(p); p += 2; }
    else if (len === 127) { len = Number(buf.readBigUInt64BE(p)); p += 8; }
    const masked = b1 & 128;
    const mask = masked ? buf.subarray(p, p + 4) : null;
    if (masked) p += 4;
    if (p + len > buf.length) return;  // partial frame: wait for more
    const data = Buffer.from(buf.subarray(p, p + len));
    if (mask) for (let i = 0; i < data.length; i++) data[i] ^= mask[i % 4];
    yield { opcode: buf[off] & 15, text: data.toString('utf8') };
    off = p + len;
  }
}

// ── a tiny PNG encoder, to stand in for the camera's compressed frames ───
function crc32(buf) {
  let c, crc = 0xffffffff;
  for (let n = 0; n < buf.length; n++) {
    c = (crc ^ buf[n]) & 0xff;
    for (let k = 0; k < 8; k++) c = c & 1 ? 0xedb88320 ^ (c >>> 1) : c >>> 1;
    crc = (crc >>> 8) ^ c;
  }
  return (crc ^ 0xffffffff) >>> 0;
}
function chunk(type, data) {
  const len = Buffer.alloc(4); len.writeUInt32BE(data.length);
  const td = Buffer.concat([Buffer.from(type, 'ascii'), data]);
  const crc = Buffer.alloc(4); crc.writeUInt32BE(crc32(td));
  return Buffer.concat([len, td, crc]);
}
function png(w, h, paint) {
  const raw = Buffer.alloc(h * (w * 3 + 1));
  for (let y = 0; y < h; y++) {
    const row = y * (w * 3 + 1);
    raw[row] = 0;
    for (let x = 0; x < w; x++) {
      const [r, g, b] = paint(x, y);
      raw[row + 1 + x * 3] = r; raw[row + 2 + x * 3] = g; raw[row + 3 + x * 3] = b;
    }
  }
  const ihdr = Buffer.alloc(13);
  ihdr.writeUInt32BE(w, 0); ihdr.writeUInt32BE(h, 4);
  ihdr[8] = 8; ihdr[9] = 2;           // 8-bit RGB
  return Buffer.concat([
    Buffer.from([0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a]),
    chunk('IHDR', ihdr), chunk('IDAT', zlib.deflateSync(raw)), chunk('IEND', Buffer.alloc(0))
  ]);
}

// ── synthetic FAST-LIVO2 session ─────────────────────────────────────────
const TOPICS = [
  ['/cloud_registered', 'sensor_msgs/msg/PointCloud2'],
  ['/cloud_effected',   'sensor_msgs/msg/PointCloud2'],
  ['/aft_mapped_to_init', 'nav_msgs/msg/Odometry'],
  ['/path',             'nav_msgs/msg/Path'],
  ['/origin_img',       'sensor_msgs/msg/CompressedImage'],
  ['/rosout',           'rcl_interfaces/msg/Log']
];

let t0 = Date.now();
const trail = [];
const pose = () => {
  const t = (Date.now() - t0) / 1000;
  return { x: 6 * Math.cos(t * 0.25), y: 6 * Math.sin(t * 0.25), z: 0.4 + 0.1 * Math.sin(t) };
};
const stamp = () => ({ sec: Math.floor(Date.now() / 1000), nanosec: (Date.now() % 1000) * 1e6 });

function cloudMsg(n = 2500) {
  const step = 16;                    // x,y,z float32 + rgb packed
  const buf = Buffer.alloc(n * step);
  const p = pose();
  for (let i = 0; i < n; i++) {
    const a = Math.random() * Math.PI * 2, r = 2 + Math.random() * 3;
    const x = p.x + r * Math.cos(a), y = p.y + r * Math.sin(a), z = Math.random() * 2.4;
    const o = i * step;
    buf.writeFloatLE(x, o); buf.writeFloatLE(y, o + 4); buf.writeFloatLE(z, o + 8);
    const t = Math.min(1, z / 2.4);
    const rgb = ((60 + 180 * t | 0) << 16) | ((90 + 100 * (1 - t) | 0) << 8) | (200 - 140 * t | 0);
    buf.writeUInt32LE(rgb >>> 0, o + 12);
  }
  return {
    header: { stamp: stamp(), frame_id: 'camera_init' },
    height: 1, width: n,
    fields: [
      { name: 'x', offset: 0, datatype: 7, count: 1 },
      { name: 'y', offset: 4, datatype: 7, count: 1 },
      { name: 'z', offset: 8, datatype: 7, count: 1 },
      { name: 'rgb', offset: 12, datatype: 7, count: 1 }
    ],
    is_bigendian: false, point_step: step, row_step: step * n,
    data: buf.toString('base64'), is_dense: true
  };
}
function odomMsg() {
  const p = pose();
  trail.push(p);
  if (trail.length > 600) trail.shift();
  return { header: { stamp: stamp(), frame_id: 'camera_init' }, child_frame_id: 'body',
    pose: { pose: { position: p, orientation: { x: 0, y: 0, z: 0, w: 1 } } } };
}
function pathMsg() {
  return { header: { stamp: stamp(), frame_id: 'camera_init' },
    poses: trail.map(p => ({ header: { stamp: stamp(), frame_id: 'camera_init' },
      pose: { position: p, orientation: { x: 0, y: 0, z: 0, w: 1 } } })) };
}
function imgMsg() {
  const t = (Date.now() - t0) / 400;
  const buf = png(320, 180, (x, y) => {
    const v = 40 + 90 * Math.sin(x / 22 + t) * Math.cos(y / 18 - t * 0.4);
    return [Math.max(0, v * 1.4) | 0, Math.max(0, v) | 0, Math.max(0, v * 0.7) | 0];
  });
  return { header: { stamp: stamp(), frame_id: 'camera' }, format: 'rgb8; png compressed bgr8',
    data: buf.toString('base64') };
}

// ── server ───────────────────────────────────────────────────────────────
const server = http.createServer((_, res) => res.end('mock rosbridge'));
server.on('upgrade', (req, socket) => {
  socket.write('HTTP/1.1 101 Switching Protocols\r\nUpgrade: websocket\r\nConnection: Upgrade\r\n' +
    `Sec-WebSocket-Accept: ${accept(req.headers['sec-websocket-key'])}\r\n\r\n`);
  const send = o => socket.write(encodeFrame(JSON.stringify(o)));
  const timers = [];
  const pub = (topic, make, hz) => timers.push(setInterval(() => send({ op: 'publish', topic, msg: make() }), 1000 / hz));

  let acc = Buffer.alloc(0);
  socket.on('data', d => {
    acc = Buffer.concat([acc, d]);
    let consumed = 0;
    for (const fr of decodeFrames(acc)) {
      consumed = acc.length;                      // our client only sends small whole frames
      if (fr.opcode === 8) { socket.end(); return; }
      let m; try { m = JSON.parse(fr.text); } catch { continue; }
      console.log('→', m.op, m.service || m.topic || '');
      if (m.op === 'call_service' && m.service === '/rosapi/topics') {
        send({ op: 'service_response', service: m.service, id: m.id, result: true,
          values: { topics: TOPICS.map(t => t[0]), types: TOPICS.map(t => t[1]) } });
      } else if (m.op === 'subscribe') {
        if (m.topic === '/cloud_registered' || m.topic === '/cloud_effected') pub(m.topic, () => cloudMsg(), 10);
        else if (m.topic === '/aft_mapped_to_init') pub(m.topic, odomMsg, 20);
        else if (m.topic === '/path') pub(m.topic, pathMsg, 5);
        else if (m.topic === '/origin_img') pub(m.topic, imgMsg, 5);
      }
    }
    acc = acc.subarray(consumed);
  });
  socket.on('close', () => timers.forEach(clearInterval));
  socket.on('error', () => timers.forEach(clearInterval));
});
server.listen(PORT, () => console.log('mock rosbridge on ws://localhost:' + PORT));