/**
 * MapViewer.jsx — SLAM occupancy grid visualization with robot position overlay
 *
 * Features:
 *   - Subscribes to /map (nav_msgs/OccupancyGrid) for SLAM occupancy data
 *   - Real-time robot position from /odom (nav_msgs/Odometry)
 *   - Canvas-based rendering with efficient grid visualization
 *   - Pan/zoom controls with mouse/touch support
 *   - Costmap color coding: white=free, gray=unknown, black=occupied
 *   - Robot position overlay with heading indicator
 *   - Trail visualization of recent robot positions
 *   - Grid statistics (resolution, size, robot location)
 */

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

// Zoom and pan constants
const MIN_ZOOM = 0.5;
const MAX_ZOOM = 10;
const ZOOM_SPEED = 0.1;
const MAX_TRAIL_LENGTH = 300; // ~30 seconds at 10Hz

function MapViewer({ subscribe }) {
  const canvasRef = useRef(null);
  const containerRef = useRef(null);

  // Map data
  const [mapData, setMapData] = useState(null);
  const [robotPose, setRobotPose] = useState({ x: 0, y: 0, theta: 0 });
  const [robotTrail, setRobotTrail] = useState([]);

  // View controls
  const [zoom, setZoom] = useState(1);
  const [panX, setPanX] = useState(0);
  const [panY, setPanY] = useState(0);
  const [isDragging, setIsDragging] = useState(false);
  const dragStartRef = useRef({ x: 0, y: 0 });

  // Refs for tracking state
  const mapDataRef = useRef(null);
  const robotPoseRef = useRef({ x: 0, y: 0, theta: 0 });
  const robotTrailRef = useRef([]);

  // Subscribe to occupancy grid (SLAM map)
  useEffect(() => {
    const unsubMap = subscribe(
      '/map',
      'nav_msgs/OccupancyGrid',
      (msg) => {
        try {
          const mapInfo = {
            width: msg.info.width,
            height: msg.info.height,
            resolution: msg.info.resolution,
            origin: msg.info.origin,
            data: msg.data || new Uint8Array(msg.info.width * msg.info.height),
          };
          setMapData(mapInfo);
          mapDataRef.current = mapInfo;
        } catch (e) {
          console.error('Error parsing map data:', e);
        }
      }
    );
    return unsubMap;
  }, [subscribe]);

  // Subscribe to robot odometry (pose)
  useEffect(() => {
    const unsubOdom = subscribe(
      '/odom',
      'nav_msgs/Odometry',
      (msg) => {
        try {
          const pos = msg.pose.pose.position;
          const ori = msg.pose.pose.orientation;

          // Convert quaternion to yaw angle
          const siny_cosp = 2 * (ori.w * ori.z + ori.x * ori.y);
          const cosy_cosp = 1 - 2 * (ori.y * ori.y + ori.z * ori.z);
          const theta = Math.atan2(siny_cosp, cosy_cosp);

          const newPose = { x: pos.x, y: pos.y, theta };
          setRobotPose(newPose);
          robotPoseRef.current = newPose;

          // Add to trail
          setRobotTrail((prev) => {
            const updated = [...prev, { x: pos.x, y: pos.y }];
            if (updated.length > MAX_TRAIL_LENGTH) {
              updated.shift();
            }
            robotTrailRef.current = updated;
            return updated;
          });
        } catch (e) {
          console.error('Error parsing odometry data:', e);
        }
      }
    );
    return unsubOdom;
  }, [subscribe]);

  // Canvas rendering
  useEffect(() => {
    const canvas = canvasRef.current;
    if (!canvas || !mapDataRef.current) return;

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

    // Clear canvas
    ctx.fillStyle = '#1f2937';
    ctx.fillRect(0, 0, width, height);

    const map = mapDataRef.current;
    const cellSize = Math.max(1, map.resolution * zoom);

    // Calculate offset for centering and panning
    const originX = map.origin.position.x;
    const originY = map.origin.position.y;

    const centerX = width / 2 + panX;
    const centerY = height / 2 + panY;

    // Draw occupancy grid
    for (let y = 0; y < map.height; y++) {
      for (let x = 0; x < map.width; x++) {
        const idx = y * map.width + x;
        const cell = map.data[idx] ?? -1;

        // Convert grid coordinates to world coordinates
        const worldX = originX + x * map.resolution;
        const worldY = originY + y * map.resolution;

        // Project to canvas
        const canvasX = centerX + (worldX - robotPoseRef.current.x) * zoom;
        const canvasY = centerY - (worldY - robotPoseRef.current.y) * zoom;

        // Only draw cells on screen
        if (canvasX >= -cellSize && canvasX <= width &&
            canvasY >= -cellSize && canvasY <= height) {

          // Color based on occupancy
          if (cell >= 0 && cell < 25) {
            ctx.fillStyle = '#f5f5f5'; // Free space (white)
          } else if (cell >= 25 && cell < 65) {
            ctx.fillStyle = '#a0a0a0'; // Unknown/gray
          } else if (cell >= 65) {
            ctx.fillStyle = '#000000'; // Occupied (black)
          } else {
            ctx.fillStyle = '#4b5563'; // Unknown (-1)
          }

          ctx.fillRect(canvasX, canvasY, cellSize, cellSize);
        }
      }
    }

    // Draw robot trail
    if (robotTrailRef.current.length > 1) {
      ctx.strokeStyle = '#10b981';
      ctx.lineWidth = 2;
      ctx.globalAlpha = 0.6;
      ctx.beginPath();

      const startPoint = robotTrailRef.current[0];
      const sx = centerX + (startPoint.x - robotPoseRef.current.x) * zoom;
      const sy = centerY - (startPoint.y - robotPoseRef.current.y) * zoom;
      ctx.moveTo(sx, sy);

      for (let i = 1; i < robotTrailRef.current.length; i++) {
        const point = robotTrailRef.current[i];
        const px = centerX + (point.x - robotPoseRef.current.x) * zoom;
        const py = centerY - (point.y - robotPoseRef.current.y) * zoom;
        ctx.lineTo(px, py);
      }
      ctx.stroke();
      ctx.globalAlpha = 1.0;
    }

    // Draw robot position (at center, always)
    const robotRadius = Math.max(8, 0.2 * zoom); // ~0.2m radius at zoom=1
    ctx.fillStyle = '#06b6d4';
    ctx.beginPath();
    ctx.arc(centerX, centerY, robotRadius, 0, Math.PI * 2);
    ctx.fill();

    // Draw robot heading indicator
    const headingLen = robotRadius * 2.5;
    const headX = centerX + Math.cos(robotPoseRef.current.theta) * headingLen;
    const headY = centerY - Math.sin(robotPoseRef.current.theta) * headingLen;

    ctx.strokeStyle = '#06b6d4';
    ctx.lineWidth = 2;
    ctx.beginPath();
    ctx.moveTo(centerX, centerY);
    ctx.lineTo(headX, headY);
    ctx.stroke();

    // Draw grid overlay (every 1 meter)
    ctx.strokeStyle = '#374151';
    ctx.lineWidth = 0.5;
    ctx.globalAlpha = 0.3;

    const gridSpacing = 1 * zoom; // 1 meter grid
    for (let i = -100; i <= 100; i++) {
      const x = centerX + i * gridSpacing;
      const y = centerY + i * gridSpacing;

      // Vertical lines
      if (x > 0 && x < width) {
        ctx.beginPath();
        ctx.moveTo(x, 0);
        ctx.lineTo(x, height);
        ctx.stroke();
      }

      // Horizontal lines
      if (y > 0 && y < height) {
        ctx.beginPath();
        ctx.moveTo(0, y);
        ctx.lineTo(width, y);
        ctx.stroke();
      }
    }
    ctx.globalAlpha = 1.0;
  }, [zoom, panX, panY]);

  // Mouse wheel zoom
  const handleWheel = (e) => {
    e.preventDefault();
    const delta = e.deltaY > 0 ? -ZOOM_SPEED : ZOOM_SPEED;
    setZoom((prev) => Math.max(MIN_ZOOM, Math.min(MAX_ZOOM, prev + delta)));
  };

  // Mouse dragging for pan
  const handleMouseDown = (e) => {
    setIsDragging(true);
    dragStartRef.current = { x: e.clientX, y: e.clientY };
  };

  const handleMouseMove = (e) => {
    if (!isDragging) return;

    const dx = e.clientX - dragStartRef.current.x;
    const dy = e.clientY - dragStartRef.current.y;

    setPanX((prev) => prev + dx);
    setPanY((prev) => prev + dy);

    dragStartRef.current = { x: e.clientX, y: e.clientY };
  };

  const handleMouseUp = () => {
    setIsDragging(false);
  };

  // Touch support for mobile
  const handleTouchStart = (e) => {
    if (e.touches.length === 1) {
      setIsDragging(true);
      dragStartRef.current = { x: e.touches[0].clientX, y: e.touches[0].clientY };
    }
  };

  const handleTouchMove = (e) => {
    if (!isDragging || e.touches.length !== 1) return;

    const dx = e.touches[0].clientX - dragStartRef.current.x;
    const dy = e.touches[0].clientY - dragStartRef.current.y;

    setPanX((prev) => prev + dx);
    setPanY((prev) => prev + dy);

    dragStartRef.current = { x: e.touches[0].clientX, y: e.touches[0].clientY };
  };

  const handleTouchEnd = () => {
    setIsDragging(false);
  };

  // Setup event listeners
  useEffect(() => {
    const canvas = canvasRef.current;
    if (!canvas) return;

    canvas.addEventListener('wheel', handleWheel, { passive: false });
    canvas.addEventListener('mousedown', handleMouseDown);
    canvas.addEventListener('touchstart', handleTouchStart);
    canvas.addEventListener('touchmove', handleTouchMove);
    canvas.addEventListener('touchend', handleTouchEnd);

    window.addEventListener('mousemove', handleMouseMove);
    window.addEventListener('mouseup', handleMouseUp);

    return () => {
      canvas.removeEventListener('wheel', handleWheel);
      canvas.removeEventListener('mousedown', handleMouseDown);
      canvas.removeEventListener('touchstart', handleTouchStart);
      canvas.removeEventListener('touchmove', handleTouchMove);
      canvas.removeEventListener('touchend', handleTouchEnd);
      window.removeEventListener('mousemove', handleMouseMove);
      window.removeEventListener('mouseup', handleMouseUp);
    };
  }, [isDragging]);

  const resetView = () => {
    setZoom(1);
    setPanX(0);
    setPanY(0);
  };

  const clearTrail = () => {
    setRobotTrail([]);
    robotTrailRef.current = [];
  };

  const mapInfo = mapData ? {
    width: mapData.width,
    height: mapData.height,
    area: (mapData.width * mapData.height * mapData.resolution * mapData.resolution).toFixed(1),
    resolution: mapData.resolution.toFixed(3),
  } : null;

  const robotInfo = robotPose ? {
    x: robotPose.x.toFixed(2),
    y: robotPose.y.toFixed(2),
    theta: (robotPose.theta * 180 / Math.PI).toFixed(1),
  } : null;

  return (
    <div className="flex flex-col h-full space-y-3">
      {/* Controls */}
      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-3">
        <div className="flex justify-between items-center flex-wrap gap-2">
          <div className="text-cyan-700 text-xs font-bold tracking-widest">
            MAP VIEWER (SLAM)
          </div>
          <div className="text-gray-600 text-xs">
            Zoom: {zoom.toFixed(2)}× | Trail: {robotTrail.length} points
          </div>
        </div>

        {/* Control buttons */}
        <div className="flex gap-2 flex-wrap">
          <button
            onClick={resetView}
            className="px-3 py-1.5 text-xs font-bold tracking-widest rounded border border-cyan-800 bg-cyan-950 text-cyan-400 hover:bg-cyan-900 transition-colors"
          >
            RESET VIEW
          </button>
          <button
            onClick={clearTrail}
            className="px-3 py-1.5 text-xs font-bold tracking-widest rounded border border-green-800 bg-green-950 text-green-400 hover:bg-green-900 transition-colors"
          >
            CLEAR TRAIL
          </button>
          <div className="text-gray-600 text-xs flex items-center">
            Scroll: Zoom | Drag: Pan
          </div>
        </div>
      </div>

      {/* Canvas */}
      <div className="flex-1 bg-gray-900 rounded-lg border border-cyan-950 overflow-hidden">
        <canvas
          ref={canvasRef}
          width={800}
          height={600}
          className="w-full h-full cursor-grab active:cursor-grabbing"
          onWheel={handleWheel}
          style={{ userSelect: 'none' }}
        />
      </div>

      {/* Info panels */}
      <div className="grid grid-cols-2 gap-2 text-xs text-gray-600">
        {/* Map info */}
        <div className="bg-gray-950 rounded border border-gray-800 p-2 space-y-1">
          <div className="text-cyan-700 font-bold">MAP</div>
          {mapInfo ? (
            <>
              <div className="flex justify-between">
                <span>Size:</span>
                <span className="text-gray-400">{mapInfo.width} × {mapInfo.height} cells</span>
              </div>
              <div className="flex justify-between">
                <span>Resolution:</span>
                <span className="text-gray-400">{mapInfo.resolution} m/cell</span>
              </div>
              <div className="flex justify-between">
                <span>Area:</span>
                <span className="text-gray-400">{mapInfo.area} m²</span>
              </div>
            </>
          ) : (
            <div className="text-gray-700">Waiting for map...</div>
          )}
        </div>

        {/* Robot info */}
        <div className="bg-gray-950 rounded border border-gray-800 p-2 space-y-1">
          <div className="text-cyan-700 font-bold">ROBOT POSE</div>
          {robotInfo ? (
            <>
              <div className="flex justify-between">
                <span>X:</span>
                <span className="text-cyan-400">{robotInfo.x} m</span>
              </div>
              <div className="flex justify-between">
                <span>Y:</span>
                <span className="text-cyan-400">{robotInfo.y} m</span>
              </div>
              <div className="flex justify-between">
                <span>Heading:</span>
                <span className="text-cyan-400">{robotInfo.theta}°</span>
              </div>
            </>
          ) : (
            <div className="text-gray-700">Waiting for odometry...</div>
          )}
        </div>
      </div>

      {/* Topic info */}
      <div className="bg-gray-950 rounded border border-gray-800 p-2 text-xs text-gray-600 space-y-1">
        <div className="flex justify-between">
          <span>Map Topic:</span>
          <span className="text-gray-500">/map (nav_msgs/OccupancyGrid)</span>
        </div>
        <div className="flex justify-between">
          <span>Odometry Topic:</span>
          <span className="text-gray-500">/odom (nav_msgs/Odometry)</span>
        </div>
        <div className="flex justify-between">
          <span>Legend:</span>
          <span className="text-gray-500">⬜ Free | 🟦 Unknown | ⬛ Occupied | 🔵 Robot</span>
        </div>
      </div>
    </div>
  );
}

export { MapViewer };