saltylab-firmware/ui/social-bot/src/components/JoystickTeleop.jsx

/**
 * JoystickTeleop.jsx — Virtual joystick for robot teleop
 *
 * Features:
 *   - Virtual thumbstick UI with visual feedback
 *   - Touch & mouse support
 *   - Publishes geometry_msgs/Twist to /cmd_vel
 *   - 10% deadzone for both axes
 *   - Max linear velocity: 0.5 m/s
 *   - Max angular velocity: 1.0 rad/s
 *   - Real-time velocity display
 */

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

const DEADZONE = 0.1; // 10%
const MAX_LINEAR_VEL = 0.5; // m/s
const MAX_ANGULAR_VEL = 1.0; // rad/s
const RADIUS = 80; // pixels
const STICK_RADIUS = 25; // pixels

function JoystickTeleop({ publish }) {
  const canvasRef = useRef(null);
  const containerRef = useRef(null);
  const [joystickPos, setJoystickPos] = useState({ x: 0, y: 0 });
  const [isDragging, setIsDragging] = useState(false);
  const [linearVel, setLinearVel] = useState(0);
  const [angularVel, setAngularVel] = useState(0);
  const velocityTimerRef = useRef(null);

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

    const ctx = canvas.getContext('2d');
    const rect = canvas.getBoundingClientRect();
    const centerX = canvas.width / 2;
    const centerY = canvas.height / 2;

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

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

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

    // Draw deadzone indicator
    ctx.strokeStyle = '#6b7280';
    ctx.lineWidth = 1;
    ctx.setLineDash([2, 2]);
    ctx.beginPath();
    ctx.arc(centerX, centerY, RADIUS * DEADZONE, 0, Math.PI * 2);
    ctx.stroke();
    ctx.setLineDash([]);

    // Draw stick
    const stickX = centerX + joystickPos.x * RADIUS;
    const stickY = centerY + joystickPos.y * RADIUS;

    ctx.fillStyle = isDragging ? '#06b6d4' : '#0891b2';
    ctx.beginPath();
    ctx.arc(stickX, stickY, STICK_RADIUS, 0, Math.PI * 2);
    ctx.fill();

    // Draw direction indicator (arrow)
    if (Math.abs(joystickPos.x) > 0.01 || Math.abs(joystickPos.y) > 0.01) {
      const angle = Math.atan2(-joystickPos.y, joystickPos.x);
      const arrowLen = 40;
      const arrowX = centerX + Math.cos(angle) * arrowLen;
      const arrowY = centerY - Math.sin(angle) * arrowLen;

      ctx.strokeStyle = '#06b6d4';
      ctx.lineWidth = 2;
      ctx.beginPath();
      ctx.moveTo(centerX, centerY);
      ctx.lineTo(arrowX, arrowY);
      ctx.stroke();
    }
  }, [joystickPos, isDragging]);

  // Handle joystick input
  const handleInput = (clientX, clientY) => {
    const canvas = canvasRef.current;
    if (!canvas) return;

    const rect = canvas.getBoundingClientRect();
    const centerX = canvas.width / 2;
    const centerY = canvas.height / 2;

    // Calculate relative position
    let x = (clientX - rect.left - centerX) / RADIUS;
    let y = (clientY - rect.top - centerY) / RADIUS;

    // Normalize to unit circle
    const distance = Math.sqrt(x * x + y * y);
    if (distance > 1) {
      x /= distance;
      y /= distance;
    }

    // Apply deadzone
    const deadzoneMagnitude = Math.sqrt(x * x + y * y);
    let adjustedX = x;
    let adjustedY = y;

    if (deadzoneMagnitude < DEADZONE) {
      adjustedX = 0;
      adjustedY = 0;
    } else {
      // Scale values above deadzone
      const scale = (deadzoneMagnitude - DEADZONE) / (1 - DEADZONE);
      adjustedX = (x / deadzoneMagnitude) * scale;
      adjustedY = (y / deadzoneMagnitude) * scale;
    }

    setJoystickPos({ x: adjustedX, y: adjustedY });

    // Calculate velocities
    // X-axis (left-right) = angular velocity
    // Y-axis (up-down) = linear velocity (negative because canvas Y is inverted)
    const linear = -adjustedY * MAX_LINEAR_VEL;
    const angular = adjustedX * MAX_ANGULAR_VEL;

    setLinearVel(linear);
    setAngularVel(angular);

    // Publish Twist message
    if (publish) {
      const twist = {
        linear: { x: linear, y: 0, z: 0 },
        angular: { x: 0, y: 0, z: angular }
      };
      publish('/cmd_vel', 'geometry_msgs/Twist', twist);
    }
  };

  const handleMouseDown = (e) => {
    setIsDragging(true);
    handleInput(e.clientX, e.clientY);
  };

  const handleMouseMove = (e) => {
    if (isDragging) {
      handleInput(e.clientX, e.clientY);
    }
  };

  const handleMouseUp = () => {
    setIsDragging(false);
    setJoystickPos({ x: 0, y: 0 });
    setLinearVel(0);
    setAngularVel(0);

    if (publish) {
      const twist = {
        linear: { x: 0, y: 0, z: 0 },
        angular: { x: 0, y: 0, z: 0 }
      };
      publish('/cmd_vel', 'geometry_msgs/Twist', twist);
    }
  };

  // Touch support
  const handleTouchStart = (e) => {
    setIsDragging(true);
    const touch = e.touches[0];
    handleInput(touch.clientX, touch.clientY);
  };

  const handleTouchMove = (e) => {
    if (isDragging) {
      e.preventDefault();
      const touch = e.touches[0];
      handleInput(touch.clientX, touch.clientY);
    }
  };

  const handleTouchEnd = () => {
    setIsDragging(false);
    setJoystickPos({ x: 0, y: 0 });
    setLinearVel(0);
    setAngularVel(0);

    if (publish) {
      const twist = {
        linear: { x: 0, y: 0, z: 0 },
        angular: { x: 0, y: 0, z: 0 }
      };
      publish('/cmd_vel', 'geometry_msgs/Twist', twist);
    }
  };

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

    canvas.addEventListener('mousedown', handleMouseDown);
    window.addEventListener('mousemove', handleMouseMove);
    window.addEventListener('mouseup', handleMouseUp);
    canvas.addEventListener('touchstart', handleTouchStart);
    canvas.addEventListener('touchmove', handleTouchMove);
    canvas.addEventListener('touchend', handleTouchEnd);

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

  // Cleanup on unmount
  useEffect(() => {
    return () => {
      if (velocityTimerRef.current) {
        clearTimeout(velocityTimerRef.current);
      }
      // Send stop command on unmount
      if (publish) {
        const twist = {
          linear: { x: 0, y: 0, z: 0 },
          angular: { x: 0, y: 0, z: 0 }
        };
        publish('/cmd_vel', 'geometry_msgs/Twist', twist);
      }
    };
  }, [publish]);

  return (
    <div className="flex flex-col h-full space-y-4">
      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4 space-y-3">
        <div className="text-cyan-700 text-xs font-bold tracking-widest">
          JOYSTICK TELEOP
        </div>

        <div className="flex gap-6">
          {/* Joystick canvas */}
          <div
            ref={containerRef}
            className="relative bg-gray-900 rounded border border-gray-800 p-2"
          >
            <canvas
              ref={canvasRef}
              width={320}
              height={320}
              className="rounded cursor-grab active:cursor-grabbing touch-none"
              style={{ userSelect: 'none' }}
            />
          </div>

          {/* Controls and info */}
          <div className="flex flex-col justify-center space-y-4 flex-1 min-w-64">
            {/* Velocity displays */}
            <div className="space-y-2">
              <div className="text-sm">
                <div className="text-gray-500 text-xs mb-1">LINEAR VELOCITY</div>
                <div className="flex items-end gap-2">
                  <span className="text-xl font-mono text-cyan-400">
                    {linearVel.toFixed(3)}
                  </span>
                  <span className="text-xs text-gray-600 mb-0.5">m/s</span>
                </div>
                <div className="text-xs text-gray-700 mt-1">
                  Range: -0.500 to +0.500 m/s
                </div>
              </div>
            </div>

            <div className="w-full h-px bg-gray-800" />

            <div className="space-y-2">
              <div className="text-sm">
                <div className="text-gray-500 text-xs mb-1">ANGULAR VELOCITY</div>
                <div className="flex items-end gap-2">
                  <span className="text-xl font-mono text-yellow-400">
                    {angularVel.toFixed(3)}
                  </span>
                  <span className="text-xs text-gray-600 mb-0.5">rad/s</span>
                </div>
                <div className="text-xs text-gray-700 mt-1">
                  Range: -1.000 to +1.000 rad/s
                </div>
              </div>
            </div>

            <div className="w-full h-px bg-gray-800" />

            {/* Instructions */}
            <div className="space-y-1 text-xs text-gray-600">
              <div className="font-bold text-gray-500">CONTROLS:</div>
              <div>▲ Forward / ▼ Backward</div>
              <div>◄ Rotate Left / ► Rotate Right</div>
              <div className="text-gray-700 mt-2">Deadzone: 10%</div>
            </div>

            {/* Status indicators */}
            <div className="flex gap-2">
              <div className={`flex items-center gap-1 px-2 py-1 rounded text-xs ${
                isDragging
                  ? 'bg-cyan-950 border border-cyan-700 text-cyan-400'
                  : 'bg-gray-900 border border-gray-800 text-gray-600'
              }`}>
                <div className={`w-2 h-2 rounded-full ${isDragging ? 'bg-cyan-400' : 'bg-gray-700'}`} />
                {isDragging ? 'ACTIVE' : 'READY'}
              </div>
            </div>
          </div>
        </div>
      </div>

      {/* Topic info */}
      <div className="bg-gray-950 rounded border border-gray-800 p-3 text-xs text-gray-600">
        <div className="flex justify-between">
          <span>Publishing to: <code className="text-gray-500">/cmd_vel</code></span>
          <span className="text-gray-700">geometry_msgs/Twist</span>
        </div>
      </div>
    </div>
  );
}

export default JoystickTeleop;