/**
 * Teleop.jsx — Gamepad and keyboard teleoperation controller
 *
 * Features:
 *   - Virtual dual-stick gamepad (left=linear, right=angular velocity)
 *   - WASD keyboard fallback for manual driving
 *   - Speed limiter slider for safe operation
 *   - E-stop button for emergency stop
 *   - Real-time velocity display (m/s and rad/s)
 *   - Publishes geometry_msgs/Twist to /cmd_vel
 *   - Visual feedback with stick position and velocity vectors
 *   - 10% deadzone on both axes
 */

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

const MAX_LINEAR_VELOCITY = 0.5; // m/s
const MAX_ANGULAR_VELOCITY = 1.0; // rad/s
const DEADZONE = 0.1; // 10% deadzone
const STICK_UPDATE_RATE = 50; // ms

function VirtualStick({
  position,
  onMove,
  label,
  color,
  maxValue = 1.0,
}) {
  const canvasRef = useRef(null);
  const containerRef = useRef(null);
  const isDraggingRef = useRef(false);

  // Draw stick
  useEffect(() => {
    const canvas = canvasRef.current;
    if (!canvas) return;

    const ctx = canvas.getContext('2d');
    const width = canvas.width;
    const height = canvas.height;
    const centerX = width / 2;
    const centerY = height / 2;
    const baseRadius = Math.min(width, height) * 0.35;
    const knobRadius = Math.min(width, height) * 0.15;

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

    // Draw base circle
    ctx.strokeStyle = '#374151';
    ctx.lineWidth = 2;
    ctx.beginPath();
    ctx.arc(centerX, centerY, baseRadius, 0, Math.PI * 2);
    ctx.stroke();

    // Draw center crosshair
    ctx.strokeStyle = '#4b5563';
    ctx.lineWidth = 1;
    ctx.beginPath();
    ctx.moveTo(centerX - baseRadius * 0.3, centerY);
    ctx.lineTo(centerX + baseRadius * 0.3, centerY);
    ctx.moveTo(centerX, centerY - baseRadius * 0.3);
    ctx.lineTo(centerX, centerY + baseRadius * 0.3);
    ctx.stroke();

    // Draw deadzone circle
    ctx.strokeStyle = '#4b5563';
    ctx.lineWidth = 1;
    ctx.globalAlpha = 0.5;
    ctx.beginPath();
    ctx.arc(centerX, centerY, baseRadius * DEADZONE, 0, Math.PI * 2);
    ctx.stroke();
    ctx.globalAlpha = 1.0;

    // Draw knob at current position
    const knobX = centerX + (position.x / maxValue) * baseRadius;
    const knobY = centerY - (position.y / maxValue) * baseRadius;

    // Knob shadow
    ctx.fillStyle = 'rgba(0, 0, 0, 0.3)';
    ctx.beginPath();
    ctx.arc(knobX + 2, knobY + 2, knobRadius, 0, Math.PI * 2);
    ctx.fill();

    // Knob
    ctx.fillStyle = color;
    ctx.beginPath();
    ctx.arc(knobX, knobY, knobRadius, 0, Math.PI * 2);
    ctx.fill();

    // Knob border
    ctx.strokeStyle = '#fff';
    ctx.lineWidth = 2;
    ctx.beginPath();
    ctx.arc(knobX, knobY, knobRadius, 0, Math.PI * 2);
    ctx.stroke();

    // Draw velocity vector
    if (Math.abs(position.x) > DEADZONE || Math.abs(position.y) > DEADZONE) {
      ctx.strokeStyle = color;
      ctx.lineWidth = 2;
      ctx.globalAlpha = 0.7;
      ctx.beginPath();
      ctx.moveTo(centerX, centerY);
      ctx.lineTo(knobX, knobY);
      ctx.stroke();
      ctx.globalAlpha = 1.0;
    }
  }, [position, color, maxValue]);

  const handlePointerDown = (e) => {
    isDraggingRef.current = true;
    updateStickPosition(e);
  };

  const handlePointerMove = (e) => {
    if (!isDraggingRef.current) return;
    updateStickPosition(e);
  };

  const handlePointerUp = () => {
    isDraggingRef.current = false;
    onMove({ x: 0, y: 0 });
  };

  const updateStickPosition = (e) => {
    const canvas = canvasRef.current;
    const rect = canvas.getBoundingClientRect();
    const centerX = rect.width / 2;
    const centerY = rect.height / 2;
    const baseRadius = Math.min(rect.width, rect.height) * 0.35;

    const x = e.clientX - rect.left - centerX;
    const y = -(e.clientY - rect.top - centerY);

    const magnitude = Math.sqrt(x * x + y * y);
    const angle = Math.atan2(y, x);

    let clampedMagnitude = Math.min(magnitude, baseRadius) / baseRadius;

    // Apply deadzone
    if (clampedMagnitude < DEADZONE) {
      clampedMagnitude = 0;
    }

    onMove({
      x: Math.cos(angle) * clampedMagnitude,
      y: Math.sin(angle) * clampedMagnitude,
    });
  };

  return (
    <div ref={containerRef} className="flex flex-col items-center gap-2">
      <div className="text-xs font-bold text-gray-400 tracking-widest">{label}</div>
      <canvas
        ref={canvasRef}
        width={160}
        height={160}
        className="border-2 border-gray-800 rounded-lg bg-gray-900 cursor-grab active:cursor-grabbing"
        onPointerDown={handlePointerDown}
        onPointerMove={handlePointerMove}
        onPointerUp={handlePointerUp}
        onPointerLeave={handlePointerUp}
        style={{ touchAction: 'none' }}
      />
      <div className="text-xs text-gray-500 font-mono">
        X: {position.x.toFixed(2)} Y: {position.y.toFixed(2)}
      </div>
    </div>
  );
}

export function Teleop({ publish }) {
  const [leftStick, setLeftStick] = useState({ x: 0, y: 0 });
  const [rightStick, setRightStick] = useState({ x: 0, y: 0 });
  const [speedLimit, setSpeedLimit] = useState(1.0);
  const [isEstopped, setIsEstopped] = useState(false);
  const [linearVel, setLinearVel] = useState(0);
  const [angularVel, setAngularVel] = useState(0);

  const keysPressed = useRef({});
  const publishIntervalRef = useRef(null);

  // Keyboard handling
  useEffect(() => {
    const handleKeyDown = (e) => {
      const key = e.key.toLowerCase();
      if (['w', 'a', 's', 'd', ' '].includes(key)) {
        keysPressed.current[key] = true;
        e.preventDefault();
      }
    };

    const handleKeyUp = (e) => {
      const key = e.key.toLowerCase();
      if (['w', 'a', 's', 'd', ' '].includes(key)) {
        keysPressed.current[key] = false;
        e.preventDefault();
      }
    };

    window.addEventListener('keydown', handleKeyDown);
    window.addEventListener('keyup', handleKeyUp);

    return () => {
      window.removeEventListener('keydown', handleKeyDown);
      window.removeEventListener('keyup', handleKeyUp);
    };
  }, []);

  // Calculate velocities from input
  useEffect(() => {
    const interval = setInterval(() => {
      let linear = leftStick.y;
      let angular = rightStick.x;

      // WASD fallback
      if (keysPressed.current['w']) linear = Math.min(1, linear + 0.5);
      if (keysPressed.current['s']) linear = Math.max(-1, linear - 0.5);
      if (keysPressed.current['d']) angular = Math.min(1, angular + 0.5);
      if (keysPressed.current['a']) angular = Math.max(-1, angular - 0.5);

      // Clamp to [-1, 1]
      linear = Math.max(-1, Math.min(1, linear));
      angular = Math.max(-1, Math.min(1, angular));

      // Apply speed limit
      const speedFactor = isEstopped ? 0 : speedLimit;
      const finalLinear = linear * MAX_LINEAR_VELOCITY * speedFactor;
      const finalAngular = angular * MAX_ANGULAR_VELOCITY * speedFactor;

      setLinearVel(finalLinear);
      setAngularVel(finalAngular);

      // Publish Twist
      if (publish) {
        publish('/cmd_vel', 'geometry_msgs/Twist', {
          linear: { x: finalLinear, y: 0, z: 0 },
          angular: { x: 0, y: 0, z: finalAngular },
        });
      }
    }, STICK_UPDATE_RATE);

    return () => clearInterval(interval);
  }, [leftStick, rightStick, speedLimit, isEstopped, publish]);

  const handleEstop = () => {
    setIsEstopped(!isEstopped);
    // Publish stop immediately
    if (publish) {
      publish('/cmd_vel', 'geometry_msgs/Twist', {
        linear: { x: 0, y: 0, z: 0 },
        angular: { x: 0, y: 0, z: 0 },
      });
    }
  };

  return (
    <div className="flex flex-col h-full space-y-3">
      {/* Status bar */}
      <div className={`rounded-lg border p-3 space-y-2 ${
        isEstopped
          ? 'bg-red-950 border-red-900'
          : 'bg-gray-950 border-cyan-950'
      }`}>
        <div className="flex justify-between items-center">
          <div className={`text-xs font-bold tracking-widest ${
            isEstopped ? 'text-red-700' : 'text-cyan-700'
          }`}>
            {isEstopped ? '🛑 E-STOP ACTIVE' : '⚡ TELEOP READY'}
          </div>
          <button
            onClick={handleEstop}
            className={`px-3 py-1 text-xs font-bold rounded border transition-colors ${
              isEstopped
                ? 'bg-green-950 border-green-800 text-green-400 hover:bg-green-900'
                : 'bg-red-950 border-red-800 text-red-400 hover:bg-red-900'
            }`}
          >
            {isEstopped ? 'RESUME' : 'E-STOP'}
          </button>
        </div>

        {/* Velocity display */}
        <div className="grid grid-cols-2 gap-2">
          <div className="bg-gray-900 rounded p-2">
            <div className="text-gray-600 text-xs">LINEAR</div>
            <div className="text-lg font-mono text-cyan-300">
              {linearVel.toFixed(2)} m/s
            </div>
          </div>
          <div className="bg-gray-900 rounded p-2">
            <div className="text-gray-600 text-xs">ANGULAR</div>
            <div className="text-lg font-mono text-amber-300">
              {angularVel.toFixed(2)} rad/s
            </div>
          </div>
        </div>
      </div>

      {/* Gamepad area */}
      <div className="flex-1 bg-gray-950 rounded-lg border border-cyan-950 p-4 flex justify-center items-center gap-8">
        <VirtualStick
          position={leftStick}
          onMove={setLeftStick}
          label="LEFT — LINEAR"
          color="#10b981"
          maxValue={1.0}
        />
        <VirtualStick
          position={rightStick}
          onMove={setRightStick}
          label="RIGHT — ANGULAR"
          color="#f59e0b"
          maxValue={1.0}
        />
      </div>

      {/* Speed limiter */}
      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-3 space-y-2">
        <div className="flex justify-between items-center">
          <div className="text-cyan-700 text-xs font-bold tracking-widest">
            SPEED LIMITER
          </div>
          <div className="text-gray-400 text-xs font-mono">
            {(speedLimit * 100).toFixed(0)}%
          </div>
        </div>
        <input
          type="range"
          min="0"
          max="1"
          step="0.05"
          value={speedLimit}
          onChange={(e) => setSpeedLimit(parseFloat(e.target.value))}
          disabled={isEstopped}
          className="w-full cursor-pointer"
          style={{
            accentColor: isEstopped ? '#6b7280' : '#06b6d4',
          }}
        />
        <div className="grid grid-cols-3 gap-2 text-xs text-gray-600">
          <div className="text-center">0%</div>
          <div className="text-center">50%</div>
          <div className="text-center">100%</div>
        </div>
      </div>

      {/* Control info */}
      <div className="bg-gray-950 rounded border border-gray-800 p-2 text-xs text-gray-600 space-y-1">
        <div className="font-bold text-cyan-700 mb-2">KEYBOARD FALLBACK</div>
        <div className="grid grid-cols-2 gap-2">
          <div>
            <span className="text-gray-500">W/S:</span> <span className="text-gray-400 font-mono">Forward/Back</span>
          </div>
          <div>
            <span className="text-gray-500">A/D:</span> <span className="text-gray-400 font-mono">Turn Left/Right</span>
          </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>Topic:</span>
          <span className="text-gray-500">/cmd_vel (geometry_msgs/Twist)</span>
        </div>
        <div className="flex justify-between">
          <span>Max Linear:</span>
          <span className="text-gray-500">{MAX_LINEAR_VELOCITY} m/s</span>
        </div>
        <div className="flex justify-between">
          <span>Max Angular:</span>
          <span className="text-gray-500">{MAX_ANGULAR_VELOCITY} rad/s</span>
        </div>
        <div className="flex justify-between">
          <span>Deadzone:</span>
          <span className="text-gray-500">{(DEADZONE * 100).toFixed(0)}%</span>
        </div>
      </div>
    </div>
  );
}
