/**
 * WaypointEditor.jsx — Interactive waypoint navigation editor with click-to-place and drag-to-reorder
 *
 * Features:
 *   - Click on map canvas to place waypoints
 *   - Drag waypoints to reorder navigation sequence
 *   - Right-click to delete waypoints
 *   - Real-time waypoint list with labels and coordinates
 *   - Send Nav2 goal to /navigate_to_pose action
 *   - Execute waypoint sequence with automatic progression
 *   - Clear all waypoints button
 *   - Visual feedback for active waypoint (executing)
 *   - Imports map display from MapViewer for coordinate system
 */

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

function WaypointEditor({ subscribe, publish, callService }) {
  // Waypoint storage
  const [waypoints, setWaypoints] = useState([]);
  const [selectedWaypoint, setSelectedWaypoint] = useState(null);
  const [isDragging, setIsDragging] = useState(false);
  const [dragIndex, setDragIndex] = useState(null);
  const [activeWaypoint, setActiveWaypoint] = useState(null);
  const [executing, setExecuting] = useState(false);

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

  // Canvas reference
  const canvasRef = useRef(null);
  const containerRef = useRef(null);

  // Refs for ROS integration
  const mapDataRef = useRef(null);
  const robotPoseRef = useRef({ x: 0, y: 0, theta: 0 });
  const waypointsRef = useRef([]);

  // Subscribe to map data (for coordinate reference)
  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,
          };
          setMapData(mapInfo);
          mapDataRef.current = mapInfo;
        } catch (e) {
          console.error('Error parsing map data:', e);
        }
      }
    );
    return unsubMap;
  }, [subscribe]);

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

          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;
        } catch (e) {
          console.error('Error parsing odometry data:', e);
        }
      }
    );
    return unsubOdom;
  }, [subscribe]);

  // Canvas event handlers
  const handleCanvasClick = (e) => {
    if (!mapDataRef.current || !canvasRef.current) return;

    const canvas = canvasRef.current;
    const rect = canvas.getBoundingClientRect();
    const clickX = e.clientX - rect.left;
    const clickY = e.clientY - rect.top;

    // Convert canvas coordinates to world coordinates
    // This assumes the map is centered on the robot
    const map = mapDataRef.current;
    const robot = robotPoseRef.current;
    const zoom = 1; // Would need to track zoom if map has zoom controls

    // Inverse of map rendering calculation
    const centerX = canvas.width / 2;
    const centerY = canvas.height / 2;

    const worldX = robot.x + (clickX - centerX) / zoom;
    const worldY = robot.y - (clickY - centerY) / zoom;

    // Create new waypoint
    const newWaypoint = {
      id: Date.now(),
      x: parseFloat(worldX.toFixed(2)),
      y: parseFloat(worldY.toFixed(2)),
      label: `WP-${waypoints.length + 1}`,
    };

    setWaypoints((prev) => [...prev, newWaypoint]);
    waypointsRef.current = [...waypointsRef.current, newWaypoint];
  };

  const handleCanvasContextMenu = (e) => {
    e.preventDefault();
    // Right-click handled by waypoint list
  };

  // Waypoint list handlers
  const handleDeleteWaypoint = (id) => {
    setWaypoints((prev) => prev.filter((wp) => wp.id !== id));
    waypointsRef.current = waypointsRef.current.filter((wp) => wp.id !== id);
    if (selectedWaypoint === id) setSelectedWaypoint(null);
  };

  const handleWaypointSelect = (id) => {
    setSelectedWaypoint(selectedWaypoint === id ? null : id);
  };

  const handleWaypointDragStart = (e, index) => {
    setIsDragging(true);
    setDragIndex(index);
  };

  const handleWaypointDragOver = (e, targetIndex) => {
    if (!isDragging || dragIndex === null || dragIndex === targetIndex) return;

    const newWaypoints = [...waypoints];
    const draggedWaypoint = newWaypoints[dragIndex];
    newWaypoints.splice(dragIndex, 1);
    newWaypoints.splice(targetIndex, 0, draggedWaypoint);

    setWaypoints(newWaypoints);
    waypointsRef.current = newWaypoints;
    setDragIndex(targetIndex);
  };

  const handleWaypointDragEnd = () => {
    setIsDragging(false);
    setDragIndex(null);
  };

  // Execute waypoints
  const sendNavGoal = async (waypoint) => {
    if (!callService) return;

    try {
      // Create quaternion from heading (default to 0 if no heading)
      const heading = waypoint.theta || 0;
      const halfHeading = heading / 2;
      const qx = 0;
      const qy = 0;
      const qz = Math.sin(halfHeading);
      const qw = Math.cos(halfHeading);

      const goal = {
        pose: {
          position: {
            x: waypoint.x,
            y: waypoint.y,
            z: 0,
          },
          orientation: {
            x: qx,
            y: qy,
            z: qz,
            w: qw,
          },
        },
      };

      // Send to Nav2 navigate_to_pose action
      await callService(
        '/navigate_to_pose',
        'nav2_msgs/NavigateToPose',
        { pose: goal.pose }
      );

      setActiveWaypoint(waypoint.id);
      return true;
    } catch (e) {
      console.error('Error sending nav goal:', e);
      return false;
    }
  };

  const executeWaypoints = async () => {
    if (waypoints.length === 0) return;

    setExecuting(true);
    for (const waypoint of waypoints) {
      const success = await sendNavGoal(waypoint);
      if (!success) {
        console.error('Failed to send goal for waypoint:', waypoint);
        break;
      }
      // Wait a bit before sending next goal
      await new Promise((resolve) => setTimeout(resolve, 500));
    }
    setExecuting(false);
    setActiveWaypoint(null);
  };

  const clearWaypoints = () => {
    setWaypoints([]);
    waypointsRef.current = [];
    setSelectedWaypoint(null);
    setActiveWaypoint(null);
  };

  const sendSingleGoal = async () => {
    if (selectedWaypoint === null) return;

    const wp = waypoints.find((w) => w.id === selectedWaypoint);
    if (wp) {
      await sendNavGoal(wp);
    }
  };

  return (
    <div className="flex h-full gap-3">
      {/* Map area with click handlers */}
      <div className="flex-1 flex flex-col space-y-3">
        <div className="flex-1 bg-gray-900 rounded-lg border border-cyan-950 overflow-hidden relative cursor-crosshair">
          <div
            ref={containerRef}
            className="w-full h-full"
            onClick={handleCanvasClick}
            onContextMenu={handleCanvasContextMenu}
          >
            {/* Virtual map display - waypoints overlaid */}
            <svg
              className="absolute inset-0 w-full h-full pointer-events-none"
              id="waypoint-overlay"
            >
              {/* Waypoint markers */}
              {waypoints.map((wp, idx) => {
                if (!mapDataRef.current) return null;

                const robot = robotPoseRef.current;
                const zoom = 1;
                const centerX = containerRef.current?.clientWidth / 2 || 400;
                const centerY = containerRef.current?.clientHeight / 2 || 300;

                const canvasX = centerX + (wp.x - robot.x) * zoom;
                const canvasY = centerY - (wp.y - robot.y) * zoom;

                const isActive = wp.id === activeWaypoint;
                const isSelected = wp.id === selectedWaypoint;

                return (
                  <g key={wp.id}>
                    {/* Waypoint circle */}
                    <circle
                      cx={canvasX}
                      cy={canvasY}
                      r="10"
                      fill={isActive ? '#ef4444' : isSelected ? '#fbbf24' : '#06b6d4'}
                      opacity="0.8"
                    />
                    {/* Waypoint number */}
                    <text
                      x={canvasX}
                      y={canvasY}
                      textAnchor="middle"
                      dominantBaseline="middle"
                      fill="white"
                      fontSize="10"
                      fontWeight="bold"
                      pointerEvents="none"
                    >
                      {idx + 1}
                    </text>
                    {/* Line to next waypoint */}
                    {idx < waypoints.length - 1 && (
                      <line
                        x1={canvasX}
                        y1={canvasY}
                        x2={
                          centerX +
                          (waypoints[idx + 1].x - robot.x) * zoom
                        }
                        y2={
                          centerY -
                          (waypoints[idx + 1].y - robot.y) * zoom
                        }
                        stroke="#10b981"
                        strokeWidth="2"
                        opacity="0.6"
                      />
                    )}
                  </g>
                );
              })}

              {/* Robot position marker */}
              <circle
                cx={containerRef.current?.clientWidth / 2 || 400}
                cy={containerRef.current?.clientHeight / 2 || 300}
                r="8"
                fill="#8b5cf6"
                opacity="1"
              />
            </svg>

            <div className="absolute inset-0 flex items-center justify-center pointer-events-none text-gray-600 text-sm">
              {waypoints.length === 0 && (
                <div className="text-center">
                  <div>Click to place waypoints</div>
                  <div className="text-xs text-gray-700">Right-click to delete</div>
                </div>
              )}
            </div>
          </div>
        </div>

        {/* Info panel */}
        <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 text-xs text-gray-600 space-y-1">
          <div className="flex justify-between">
            <span>Waypoints:</span>
            <span className="text-cyan-400">{waypoints.length}</span>
          </div>
          <div className="flex justify-between">
            <span>Robot Position:</span>
            <span className="text-cyan-400">
              ({robotPose.x.toFixed(2)}, {robotPose.y.toFixed(2)})
            </span>
          </div>
        </div>
      </div>

      {/* Waypoint list sidebar */}
      <div className="w-64 flex flex-col bg-gray-950 rounded-lg border border-cyan-950 space-y-3 p-3">
        <div className="flex items-center justify-between">
          <div className="text-cyan-700 text-xs font-bold tracking-widest">WAYPOINTS</div>
          <div className="text-gray-600 text-xs">{waypoints.length}</div>
        </div>

        {/* Waypoint list */}
        <div className="flex-1 overflow-y-auto space-y-1">
          {waypoints.length === 0 ? (
            <div className="text-center text-gray-700 text-xs py-4">
              Click map to add waypoints
            </div>
          ) : (
            waypoints.map((wp, idx) => (
              <div
                key={wp.id}
                draggable
                onDragStart={(e) => handleWaypointDragStart(e, idx)}
                onDragOver={(e) => {
                  e.preventDefault();
                  handleWaypointDragOver(e, idx);
                }}
                onDragEnd={handleWaypointDragEnd}
                onClick={() => handleWaypointSelect(wp.id)}
                onContextMenu={(e) => {
                  e.preventDefault();
                  handleDeleteWaypoint(wp.id);
                }}
                className={`p-2 rounded border text-xs cursor-move transition-colors ${
                  wp.id === activeWaypoint
                    ? 'bg-red-950 border-red-700 text-red-300'
                    : wp.id === selectedWaypoint
                    ? 'bg-amber-950 border-amber-700 text-amber-300'
                    : 'bg-gray-900 border-gray-700 text-gray-400 hover:border-gray-600'
                }`}
              >
                <div className="flex justify-between items-start gap-2">
                  <div className="font-bold">#{idx + 1}</div>
                  <div className="text-right flex-1">
                    <div className="text-gray-500">{wp.label}</div>
                    <div className="text-gray-600">
                      {wp.x.toFixed(2)}, {wp.y.toFixed(2)}
                    </div>
                  </div>
                </div>
              </div>
            ))
          )}
        </div>

        {/* Control buttons */}
        <div className="space-y-2 border-t border-gray-800 pt-3">
          <button
            onClick={sendSingleGoal}
            disabled={selectedWaypoint === null || executing}
            className="w-full px-2 py-1.5 text-xs font-bold tracking-widest rounded border border-cyan-800 bg-cyan-950 text-cyan-400 hover:bg-cyan-900 disabled:opacity-50 disabled:cursor-not-allowed transition-colors"
          >
            SEND GOAL
          </button>

          <button
            onClick={executeWaypoints}
            disabled={waypoints.length === 0 || executing}
            className="w-full px-2 py-1.5 text-xs font-bold tracking-widest rounded border border-green-800 bg-green-950 text-green-400 hover:bg-green-900 disabled:opacity-50 disabled:cursor-not-allowed transition-colors"
          >
            {executing ? 'EXECUTING...' : 'EXECUTE ALL'}
          </button>

          <button
            onClick={clearWaypoints}
            disabled={waypoints.length === 0}
            className="w-full px-2 py-1.5 text-xs font-bold tracking-widest rounded border border-red-800 bg-red-950 text-red-400 hover:bg-red-900 disabled:opacity-50 disabled:cursor-not-allowed transition-colors"
          >
            CLEAR ALL
          </button>
        </div>

        {/* Instructions */}
        <div className="text-xs text-gray-600 space-y-1 border-t border-gray-800 pt-3">
          <div className="font-bold text-gray-500">CONTROLS:</div>
          <div>• Click: Place waypoint</div>
          <div>• Right-click: Delete waypoint</div>
          <div>• Drag: Reorder waypoints</div>
          <div>• Click list: Select waypoint</div>
        </div>

        {/* Topic info */}
        <div className="text-xs text-gray-600 border-t border-gray-800 pt-3">
          <div className="flex justify-between">
            <span>Service:</span>
            <span className="text-gray-500">/navigate_to_pose</span>
          </div>
        </div>
      </div>
    </div>
  );
}

export { WaypointEditor };