/**
 * MapViewer.jsx — 2D occupancy grid + robot pose + Nav2 path overlay.
 *
 * Topics:
 *   /map           (nav_msgs/OccupancyGrid)  — SLAM/static map
 *   /odom          (nav_msgs/Odometry)       — robot position & heading
 *   /outdoor/route (nav_msgs/Path)           — Nav2 / OSM route path
 *
 * NOTE: OccupancyGrid data can be large (384×384 = 150K cells).
 *       We decode on a worker-free canvas; map refreshes at topic rate
 *       (typically 0.1–1 Hz from SLAM), odom at ~10 Hz.
 */

import { useEffect, useRef, useState, useCallback } from 'react';

const CELL_COLORS = {
  unknown: '#1a1a2e',
  free:    '#0a1020',
  occ:     '#00ffff33',
  occFull: '#00b8d9',
};

function quatToYaw(o) {
  return Math.atan2(2 * (o.w * o.z + o.x * o.y), 1 - 2 * (o.y * o.y + o.z * o.z));
}

export function MapViewer({ subscribe }) {
  const canvasRef = useRef(null);
  const mapRef    = useRef(null);  // last OccupancyGrid info
  const odomRef   = useRef(null);  // last robot pose {x,y,yaw}
  const pathRef   = useRef([]);    // [{x,y}] path points in map coords
  const [mapInfo, setMapInfo]   = useState(null);
  const [odomPose, setOdomPose] = useState(null);
  const [zoom, setZoom]         = useState(1);
  const [pan, setPan]           = useState({ x: 0, y: 0 });
  const dragging = useRef(null);

  // ── Render ────────────────────────────────────────────────────────────────
  const render = useCallback(() => {
    const canvas = canvasRef.current;
    const map    = mapRef.current;
    if (!canvas || !map) return;

    const ctx = canvas.getContext('2d');
    const W = canvas.width, H = canvas.height;

    ctx.clearRect(0, 0, W, H);
    ctx.fillStyle = CELL_COLORS.unknown;
    ctx.fillRect(0, 0, W, H);

    const { data, info } = map;
    const mW = info.width, mH = info.height;
    const res = info.resolution; // m/cell
    const cellPx = zoom;         // 1 map cell = zoom pixels

    // Canvas centre
    const cx = W / 2 + pan.x;
    const cy = H / 2 + pan.y;

    // Map origin (bottom-left in world) → we flip y for canvas
    const ox = info.origin.position.x;
    const oy = info.origin.position.y;

    // Draw map cells in chunks
    const img = ctx.createImageData(W, H);
    const imgData = img.data;

    for (let r = 0; r < mH; r++) {
      for (let c = 0; c < mW; c++) {
        const val = data[r * mW + c];
        // World coords of this cell centre
        const wx = ox + (c + 0.5) * res;
        const wy = oy + (r + 0.5) * res;
        // Canvas coords (flip y)
        const px = Math.round(cx + wx * cellPx / res);
        const py = Math.round(cy - wy * cellPx / res);

        if (px < 0 || px >= W || py < 0 || py >= H) continue;

        let ro, go, bo, ao;
        if (val < 0) { ro=26; go=26; bo=46; ao=255; }         // unknown
        else if (val === 0) { ro=10; go=16; bo=32; ao=255; }  // free
        else if (val < 60)  { ro=0;  go=100; bo=120; ao=120; } // low occ
        else                { ro=0;  go=184; bo=217; ao=220; }  // occupied

        const i = (py * W + px) * 4;
        imgData[i]   = ro; imgData[i+1] = go;
        imgData[i+2] = bo; imgData[i+3] = ao;
      }
    }
    ctx.putImageData(img, 0, 0);

    // ── Nav2 path ─────────────────────────────────────────────────────────
    const path = pathRef.current;
    if (path.length >= 2) {
      ctx.strokeStyle = '#f59e0b';
      ctx.lineWidth = 2;
      ctx.setLineDash([4, 4]);
      ctx.beginPath();
      path.forEach(({ x, y }, i) => {
        const px = cx + x * cellPx / res;
        const py = cy - y * cellPx / res;
        i === 0 ? ctx.moveTo(px, py) : ctx.lineTo(px, py);
      });
      ctx.stroke();
      ctx.setLineDash([]);
    }

    // ── Robot ────────────────────────────────────────────────────────────
    const odom = odomRef.current;
    if (odom) {
      const rx = cx + odom.x * cellPx / res;
      const ry = cy - odom.y * cellPx / res;

      // Heading arrow
      const arrowLen = Math.max(12, cellPx * 1.5);
      ctx.strokeStyle = '#f97316';
      ctx.lineWidth = 2;
      ctx.beginPath();
      ctx.moveTo(rx, ry);
      ctx.lineTo(rx + Math.cos(odom.yaw) * arrowLen, ry - Math.sin(odom.yaw) * arrowLen);
      ctx.stroke();

      // Robot circle
      ctx.fillStyle = '#f97316';
      ctx.shadowBlur = 8;
      ctx.shadowColor = '#f97316';
      ctx.beginPath();
      ctx.arc(rx, ry, 6, 0, Math.PI * 2);
      ctx.fill();
      ctx.shadowBlur = 0;

      // Heading dot
      ctx.fillStyle = '#ffffff';
      ctx.beginPath();
      ctx.arc(rx + Math.cos(odom.yaw) * arrowLen, ry - Math.sin(odom.yaw) * arrowLen, 3, 0, Math.PI * 2);
      ctx.fill();
    }

    // ── Scale bar ─────────────────────────────────────────────────────────
    if (mapInfo) {
      const scaleM = 2; // 2-metre scale bar
      const scalePx = scaleM * cellPx / res;
      const bx = 12, by = H - 12;
      ctx.strokeStyle = '#06b6d4';
      ctx.lineWidth = 2;
      ctx.beginPath();
      ctx.moveTo(bx, by); ctx.lineTo(bx + scalePx, by);
      ctx.moveTo(bx, by - 4); ctx.lineTo(bx, by + 4);
      ctx.moveTo(bx + scalePx, by - 4); ctx.lineTo(bx + scalePx, by + 4);
      ctx.stroke();
      ctx.fillStyle = '#06b6d4';
      ctx.font = '9px monospace';
      ctx.textAlign = 'left';
      ctx.fillText(`${scaleM}m`, bx, by - 6);
    }
  }, [zoom, pan, mapInfo]);

  // ── Subscribe /map ────────────────────────────────────────────────────────
  useEffect(() => {
    const unsub = subscribe('/map', 'nav_msgs/OccupancyGrid', (msg) => {
      mapRef.current = msg;
      setMapInfo(msg.info);
      render();
    });
    return unsub;
  }, [subscribe, render]);

  // ── Subscribe /odom ───────────────────────────────────────────────────────
  useEffect(() => {
    const unsub = subscribe('/odom', 'nav_msgs/Odometry', (msg) => {
      const p = msg.pose.pose.position;
      const o = msg.pose.pose.orientation;
      const pose = { x: p.x, y: p.y, yaw: quatToYaw(o) };
      odomRef.current = pose;
      setOdomPose(pose);
      render();
    });
    return unsub;
  }, [subscribe, render]);

  // ── Subscribe /outdoor/route (Nav2 / OSM path) ────────────────────────────
  useEffect(() => {
    const unsub = subscribe('/outdoor/route', 'nav_msgs/Path', (msg) => {
      pathRef.current = (msg.poses ?? []).map(p => ({
        x: p.pose.position.x,
        y: p.pose.position.y,
      }));
      render();
    });
    return unsub;
  }, [subscribe, render]);

  // Re-render when zoom/pan changes
  useEffect(() => { render(); }, [zoom, pan, render]);

  // ── Mouse pan ─────────────────────────────────────────────────────────────
  const onMouseDown = (e) => { dragging.current = { x: e.clientX - pan.x, y: e.clientY - pan.y }; };
  const onMouseMove = (e) => {
    if (!dragging.current) return;
    setPan({ x: e.clientX - dragging.current.x, y: e.clientY - dragging.current.y });
  };
  const onMouseUp = () => { dragging.current = null; };

  // Touch pan
  const touchRef = useRef(null);
  const onTouchStart = (e) => {
    const t = e.touches[0];
    touchRef.current = { x: t.clientX - pan.x, y: t.clientY - pan.y };
  };
  const onTouchMove = (e) => {
    if (!touchRef.current) return;
    const t = e.touches[0];
    setPan({ x: t.clientX - touchRef.current.x, y: t.clientY - touchRef.current.y });
  };

  return (
    <div className="space-y-3">
      {/* Toolbar */}
      <div className="flex items-center gap-2 flex-wrap">
        <div className="text-cyan-700 text-xs font-bold tracking-widest">MAP VIEWER</div>
        <div className="flex items-center gap-1 ml-auto">
          <button onClick={() => setZoom(z => Math.min(z * 1.5, 20))}
            className="px-2 py-1 rounded border border-gray-700 text-gray-300 hover:border-cyan-700 text-sm">+</button>
          <span className="text-gray-500 text-xs w-10 text-center">{zoom.toFixed(1)}x</span>
          <button onClick={() => setZoom(z => Math.max(z / 1.5, 0.2))}
            className="px-2 py-1 rounded border border-gray-700 text-gray-300 hover:border-cyan-700 text-sm">−</button>
          <button onClick={() => { setZoom(1); setPan({ x: 0, y: 0 }); }}
            className="px-2 py-1 rounded border border-gray-700 text-gray-400 hover:text-gray-200 text-xs ml-2">Reset</button>
        </div>
      </div>

      {/* Canvas */}
      <div className="bg-gray-950 rounded-lg border border-cyan-950 overflow-hidden">
        <canvas
          ref={canvasRef}
          width={600}
          height={400}
          className="w-full cursor-grab active:cursor-grabbing block"
          onMouseDown={onMouseDown}
          onMouseMove={onMouseMove}
          onMouseUp={onMouseUp}
          onMouseLeave={onMouseUp}
          onTouchStart={onTouchStart}
          onTouchMove={onTouchMove}
          onTouchEnd={() => { touchRef.current = null; }}
        />
      </div>

      {/* Info bar */}
      <div className="grid grid-cols-2 sm:grid-cols-4 gap-2 text-xs">
        <div className="bg-gray-950 rounded border border-gray-800 p-2">
          <div className="text-gray-600">MAP SIZE</div>
          <div className="text-cyan-400 font-bold">
            {mapInfo ? `${mapInfo.width}×${mapInfo.height}` : '—'}
          </div>
        </div>
        <div className="bg-gray-950 rounded border border-gray-800 p-2">
          <div className="text-gray-600">RESOLUTION</div>
          <div className="text-cyan-400 font-bold">
            {mapInfo ? `${mapInfo.resolution.toFixed(2)}m/cell` : '—'}
          </div>
        </div>
        <div className="bg-gray-950 rounded border border-gray-800 p-2">
          <div className="text-gray-600">ROBOT POS</div>
          <div className="text-orange-400 font-bold">
            {odomPose ? `${odomPose.x.toFixed(2)}, ${odomPose.y.toFixed(2)}` : '—'}
          </div>
        </div>
        <div className="bg-gray-950 rounded border border-gray-800 p-2">
          <div className="text-gray-600">HEADING</div>
          <div className="text-orange-400 font-bold">
            {odomPose ? `${(odomPose.yaw * 180 / Math.PI).toFixed(1)}°` : '—'}
          </div>
        </div>
      </div>

      {/* Legend */}
      <div className="flex gap-4 text-xs text-gray-600">
        <div className="flex items-center gap-1">
          <div className="w-3 h-3 rounded-sm" style={{ background: '#1a1a2e' }} />Unknown
        </div>
        <div className="flex items-center gap-1">
          <div className="w-3 h-3 rounded-sm" style={{ background: '#0a1020' }} />Free
        </div>
        <div className="flex items-center gap-1">
          <div className="w-3 h-3 rounded-sm" style={{ background: '#00b8d9' }} />Occupied
        </div>
        <div className="flex items-center gap-1">
          <div className="w-3 h-3 rounded-sm" style={{ background: '#f59e0b' }} />Path
        </div>
        <div className="flex items-center gap-1">
          <div className="w-3 h-3 rounded-full" style={{ background: '#f97316' }} />Robot
        </div>
      </div>
    </div>
  );
}
