src/lib/components/3d/Robot.svelte

<script lang="ts">
	import * as THREE from "three";
	import { T, useThrelte } from "@threlte/core";
	import { getRootLinks } from "@/components/3d/robot/URDF/utils/UrdfParser";
	import UrdfLink from "@/components/3d/robot/URDF/primitives/UrdfLink.svelte";
	import { robotManager } from "$lib/robot/RobotManager.svelte";
	import { Billboard, HTML } from "@threlte/extras";
	import { scale } from "svelte/transition";
	import { onMount } from "svelte";
	import { robotUrdfConfigMap } from "@/configs/robotUrdfConfig";
	import MasterConnectionModal from "@/components/interface/overlay/MasterConnectionModal.svelte";
	import SlaveConnectionModal from "@/components/interface/overlay/SlaveConnectionModal.svelte";
	import ManualControlModal from "@/components/interface/overlay/ManualControlSheet.svelte";
	import type { Robot } from "$lib/robot/Robot.svelte";
	import Hoverable from "@/components/utils/Hoverable.svelte";
	import { generateName } from "$lib/utils/generateName";

	interface Props {}

	let {}: Props = $props();

	// Get camera controls
	const { camera } = useThrelte();

	// Modal state using runes
	let isMasterModalOpen = $state(false);
	let isSlaveModalOpen = $state(false);
	let isManualControlModalOpen = $state(false);
	let selectedRobot = $state<Robot | null>(null);

	// Get all robots from the manager
	const robots = $derived(robotManager.robots);

	// Helper function to get connection status
	function getConnectionStatus(robot: any) {
		const status = robotManager.getRobotStatus(robot.id);
		if (!status) return "offline";

		if (status.hasActiveMaster && status.connectedSlaves > 0) {
			return "active";
		} else if (status.hasActiveMaster) {
			return "Master Only";
		} else if (status.connectedSlaves > 0) {
			return "Slave Only";
		}
		return "idle";
	}

	// Helper function to get status color
	function getStatusVariant(status: string) {
		switch (status) {
			case "Master + Slaves":
				return "default"; // Green
			case "Master Only":
				return "secondary"; // Yellow
			case "Slave Only":
				return "outline"; // Blue
			default:
				return "destructive"; // Red/Gray
		}
	}

	// Camera movement function
	function moveCameraToRobot(robot: Robot, index: number) {
		if (!camera.current) return;

		// Calculate robot position (same logic as used in the template)
		const gridWidth = 3;
		const spacing = 6;
		const totalRows = Math.ceil(robots.length / gridWidth);
		const row = Math.floor(index / gridWidth);
		const col = 1 + (index % gridWidth);
		const xPosition = (col - Math.floor(gridWidth / 2)) * spacing;
		const zPosition = (row - Math.floor(totalRows / 2)) * spacing;

		// Camera positioning parameters - adjust these to change the rotation angle
		const cameraDistance = 12; // Distance from robot
		const cameraHeight = 8; // Height above ground
		const angleOffset = Math.PI / 4; // 45 degrees - change this for different angles

		// Calculate camera position using polar coordinates for easy angle control
		const cameraX = xPosition + Math.cos(angleOffset) * cameraDistance;
		const cameraZ = zPosition + Math.sin(angleOffset) * cameraDistance;

		// Create target positions
		const targetPosition = new THREE.Vector3(cameraX, cameraHeight, cameraZ);
		const lookAtPosition = new THREE.Vector3(xPosition, 1, zPosition); // Look at robot center

		// Animate camera movement
		const startPosition = camera.current.position.clone();
		const startTime = Date.now();
		const duration = 1000; // 1 second animation

		function animateCamera() {
			const elapsed = Date.now() - startTime;
			const progress = Math.min(elapsed / duration, 1);

			// Use easing function for smooth animation
			const easeProgress = 1 - Math.pow(1 - progress, 3); // easeOutCubic

			// Interpolate position
			camera.current.position.lerpVectors(startPosition, targetPosition, easeProgress);
			camera.current.lookAt(lookAtPosition);

			if (progress < 1) {
				requestAnimationFrame(animateCamera);
			}
		}

		animateCamera();
	}

	// Modal management functions
	function openMasterModal(robot: Robot) {
		console.log("Opening master modal for robot:", robot.id);
		selectedRobot = robot;
		isMasterModalOpen = true;
	}

	function openSlaveModal(robot: Robot) {
		console.log("Opening slave modal for robot:", robot.id);
		selectedRobot = robot;
		isSlaveModalOpen = true;
	}

	function openManualControlModal(robot: Robot) {
		console.log("Opening manual control modal for robot:", robot.id);
		selectedRobot = robot;
		isManualControlModalOpen = true;
	}

	// Handle stop propagation for nested buttons
	function handleAddButtonClick(event: Event, robot: Robot, tab: "master" | "slaves") {
		console.log("Handling add button click for robot:", robot.id, "tab:", tab);
		event.stopPropagation();
		if (tab === "master") {
			openMasterModal(robot);
		} else {
			openSlaveModal(robot);
		}
	}

	// Handle box clicks (using mousedown since it works reliably in 3D context)
	function handleBoxClick(robot: Robot, type: "master" | "slaves" | "manual") {
		console.log("Box clicked:", type, "for robot:", robot.id);
		if (type === "master") {
			openMasterModal(robot);
		} else if (type === "slaves") {
			openSlaveModal(robot);
		} else if (type === "manual") {
			openManualControlModal(robot);
		}
	}

	onMount(() => {
		function createRobot() {
			const urdfConfig = robotUrdfConfigMap["so-arm100"];

			if (!urdfConfig) {
				return;
			}

			const robotId = generateName();
			console.log("Creating robot with ID:", robotId, "and config:", urdfConfig);

			robotManager.createRobot(robotId, urdfConfig);
		}

		// If no robot then create one
		if (robots.length === 0) {
			createRobot();
		}
	});

	function generateRobotName() {
		throw new Error("Function not implemented.");
	}
</script>

{#each robots as robot, index (robot.id)}
	{@const gridWidth = 3}
	<!-- Number of robots per row -->
	{@const spacing = 6}
	<!-- Space between robots -->
	{@const totalRows = Math.ceil(robots.length / gridWidth)}
	{@const row = Math.floor(index / gridWidth)}
	{@const col = 1 + (index % gridWidth)}
	{@const xPosition = (col - Math.floor(gridWidth / 2)) * spacing}
	<!-- Center the grid on x-axis -->
	{@const zPosition = (row - Math.floor(totalRows / 2)) * spacing}
	<!-- Center the grid on z-axis -->
	{@const robotStatus = robotManager.getRobotStatus(robot.id)}
	{@const connectionStatus = getConnectionStatus(robot)}
	{@const statusVariant = getStatusVariant(connectionStatus)}
	<T.Group
		position.x={xPosition}
		position.y={0}
		position.z={zPosition}
		quaternion={[0, 0, 0, 1]}
		scale={[10, 10, 10]}
		rotation={[-Math.PI / 2, 0, 0]}
	>
		<Hoverable
			onClick={() => {
				moveCameraToRobot(robot, index);
				handleBoxClick(robot, "manual");
			}}
		>
			{#snippet content({ isHovered, isSelected })}
				{#each getRootLinks(robot.robotState.robot) as link}
					<UrdfLink
						robot={robot.robotState.robot}
						{link}
						textScale={0.2}
						showName={isHovered || isSelected}
						showVisual={true}
						showCollision={false}
						visualColor="#333333"
						visualOpacity={isHovered || isSelected ? 0.4 : 1.0}
						collisionOpacity={1.0}
						collisionColor="#813d9c"
						jointNames={isHovered || isSelected}
						joints={isHovered || isSelected}
						jointColor="#62a0ea"
						jointIndicatorColor="#f66151"
						nameHeight={0.1}
						selectedLink={robot.robotState.selection.selectedLink}
						selectedJoint={robot.robotState.selection.selectedJoint}
						highlightColor="#ffa348"
						showLine={isHovered || isSelected}
						opacity={1}
						isInteractive={false}
					/>
				{/each}

				<T.Group position.z={0.25} rotation={[Math.PI / 2, 0, 0]} scale={[0.12, 0.12, 0.12]}>
					<Billboard>
						<HTML
							transform
							autoRender={true}
							center={true}
							distanceFactor={3}
							pointerEvents="auto"
							style="
								pointer-events: auto !important;
								image-rendering: auto;
								image-rendering: smooth;
								text-rendering: optimizeLegibility;
								-webkit-font-smoothing: subpixel-antialiased;
								-moz-osx-font-smoothing: auto;
								backface-visibility: hidden;
								transform-style: preserve-3d;
								will-change: transform;
							"
						>
							{#if isHovered || isSelected}
								<div
									class="pointer-events-auto select-none"
									style="pointer-events: auto !important;"
									in:scale={{ duration: 200, start: 0.5 }}
								>
									<div class="flex items-center gap-3">
										<!-- Manual Control Box -->
										<button
											class={[
												"relative min-h-[80px] min-w-[90px] cursor-pointer rounded-lg border border-purple-500/40 bg-purple-950/20 bg-slate-900/80 px-3 py-3 backdrop-blur-sm transition-colors hover:border-purple-400/60",
												robotStatus?.hasActiveMaster
													? "cursor-not-allowed border-dashed opacity-40"
													: robot.manualControlEnabled
														? ""
														: "border-dashed opacity-60"
											]}
											style="pointer-events: auto !important;"
											onmousedown={() =>
												!robotStatus?.hasActiveMaster && handleBoxClick(robot, "manual")}
											onclick={() =>
												!robotStatus?.hasActiveMaster && handleBoxClick(robot, "manual")}
											disabled={robotStatus?.hasActiveMaster}
										>
											{#if robotStatus?.hasActiveMaster}
												<div class="flex flex-col items-center">
													<div class="flex items-center gap-1">
														<span class="icon-[mdi--lock] size-3 text-purple-400/60"></span>
														<span
															class="text-xs leading-tight font-medium text-purple-400/60 uppercase"
															>CONTROL DISABLED</span
														>
													</div>
													<span class="mt-0.5 text-center text-xs leading-tight text-purple-300/60">
														Control managed by
													</span>
													<span
														class="text-center text-xs leading-tight font-semibold text-purple-300/60"
													>
														{robot.master?.name?.slice(0, 20) || "Master"}
													</span>
												</div>
											{:else if robot.manualControlEnabled}
												<div class="flex flex-col items-center">
													<div class="flex items-center gap-1">
														<span class="icon-[mdi--tune] size-3 text-purple-400"></span>
														<span
															class="text-xs leading-tight font-semibold text-purple-400 uppercase"
															>MANUAL</span
														>
													</div>
													<span class="mt-0.5 text-xs leading-tight text-purple-200">
														{robot.activeJoints.length} Joints Active
													</span>
													<span class="text-xs leading-tight text-purple-300/80">
														Click to take manual control
													</span>
												</div>
											{:else}
												<div class="flex flex-col items-center">
													<div class="flex items-center gap-1">
														<span class="icon-[mdi--tune] size-3 text-purple-400/60"></span>
														<span class="text-xs leading-tight font-medium text-purple-400/60"
															>MANUAL OFF</span
														>
													</div>
													<span class="mt-0.5 text-xs leading-tight text-purple-300/40"
														>Click to Configure</span
													>
													<div class="mt-2 text-purple-400/60">
														<span class="icon-[mdi--cog-outline] size-4"></span>
													</div>
												</div>
											{/if}
										</button>
									</div>
								</div>
							{:else}
								<div
									class="pointer-events-auto select-none"
									style="pointer-events: auto !important;"
									in:scale={{ duration: 200, start: 0.5 }}
								>
									<div class="flex items-center gap-3">
										<!-- Master Box -->
										<button
											class={[
												"relative min-h-[80px] min-w-[90px] cursor-pointer rounded-lg border border-green-500/40 bg-green-950/20 bg-slate-900/80 px-3 py-3 backdrop-blur-sm transition-colors hover:border-green-400/60",
												robotStatus?.hasActiveMaster ? "" : "border-dashed opacity-60"
											]}
											style="pointer-events: auto !important;"
											onmousedown={() => handleBoxClick(robot, "master")}
											onclick={() => handleBoxClick(robot, "master")}
										>
											{#if robotStatus?.hasActiveMaster}
												<div class="flex flex-col items-center">
													<div class="flex items-center gap-1">
														<span class="icon-[mdi--speak] size-3 text-green-400"></span>
														<span
															class="text-xs leading-tight font-semibold text-green-400 uppercase"
															>MASTER</span
														>
													</div>
													<span class="mt-0.5 text-xs leading-tight text-green-200">
														{robot.master?.name.slice(0, 30) || "Unknown"}
													</span>
													{#if robot.master?.constructor.name}
														<span class="text-xs leading-tight text-green-300/80">
															{robot.master?.constructor.name.replace("Driver", "").slice(0, 30)}
														</span>
													{:else}
														<span class="text-xs leading-tight text-red-300/80"> N/A </span>
													{/if}
													<div
														class="mt-1 h-1.5 w-1.5 animate-pulse rounded-full bg-green-400"
													></div>
												</div>
											{:else}
												<div class="flex flex-col items-center">
													<div class="flex items-center gap-1">
														<span class="icon-[mdi--speak] size-3 text-green-400/60"></span>
														<span class="text-xs leading-tight font-medium text-green-400/60"
															>NO MASTER</span
														>
													</div>
													<span class="mt-0.5 text-xs leading-tight text-green-300/40"
														>Click to Connect</span
													>
													<div class="mt-2 text-green-400/60">
														<span class="icon-[mdi--plus-circle] size-4"></span>
													</div>
												</div>
											{/if}
										</button>

										<!-- Arrow 1: Master to Robot -->
										<div class="font-mono text-sm text-slate-400">
											{#if robotStatus?.hasActiveMaster}
												<span class="text-green-400">→</span>
											{:else}
												<span class="text-green-400/50">⇢</span>
											{/if}
										</div>

										<!-- Robot Box (Simplified) -->
										<div
											class={[
												"min-h-[80px] min-w-[90px] rounded-lg border border-amber-500/40 bg-slate-900/80 px-3 py-3 backdrop-blur-sm"
											]}
										>
											<div class="flex flex-col items-center">
												<div class="flex items-center gap-1">
													<span class="icon-[mdi--connection] size-3 text-amber-400"></span>
													<span
														class="text-xs leading-tight font-semibold text-amber-400 uppercase"
													>
														Robot
													</span>
												</div>
												<span class="mt-0.5 text-xs leading-tight text-amber-200">
													{robot.id}
												</span>
											</div>
										</div>

										<!-- Arrow 2: Robot to Slaves -->
										<div class="font-mono text-sm text-slate-400">
											{#if robotStatus?.connectedSlaves && robotStatus.connectedSlaves > 0}
												<span class="text-blue-400">→</span>
											{:else}
												<span class="text-blue-400/50">⇢</span>
											{/if}
										</div>

										<!-- Slaves Box -->
										<button
											class={[
												"relative min-h-[80px] min-w-[90px] cursor-pointer rounded-lg border border-blue-500/40 bg-blue-950/20 bg-slate-900/80 px-3 py-3 backdrop-blur-sm transition-colors hover:border-blue-400/60",
												robotStatus?.connectedSlaves && robotStatus.connectedSlaves > 0
													? ""
													: "border-dashed opacity-60"
											]}
											style="pointer-events: auto !important;"
											onmousedown={() => handleBoxClick(robot, "slaves")}
											onclick={() => handleBoxClick(robot, "slaves")}
										>
											{#if robotStatus?.connectedSlaves && robotStatus.connectedSlaves > 0}
												<div class="flex flex-col items-center">
													<div class="flex items-center gap-1">
														<span class="icon-[fa6-solid--ear-listen] size-3 text-blue-400"></span>
														<span
															class="text-xs leading-tight font-semibold text-blue-400 uppercase"
															>SLAVES</span
														>
													</div>
													{#if robot.slaves.length > 0}
														<div class="mt-1 flex flex-col items-center gap-0.5">
															{#each robot.slaves.slice(0, 2) as slave}
																<div class="mt-0.5 text-xs leading-tight text-blue-300/80">
																	{slave.name.slice(0, 30) || "Slave"}
																</div>
																<div class="text-xs leading-tight text-blue-200">
																	{slave.constructor.name.replace("Driver", "").slice(0, 30) ||
																		"N/A"}
																</div>
															{/each}
															{#if robot.slaves.length > 2}
																<span class="text-xs text-blue-400/60"
																	>+{robot.slaves.length - 2} more</span
																>
															{/if}
														</div>
													{/if}
												</div>
											{:else}
												<div class="flex flex-col items-center">
													<div class="flex items-center gap-1">
														<span class="icon-[fa6-solid--ear-listen] size-3 text-blue-400/60"
														></span>
														<span class="text-xs leading-tight font-medium text-blue-400/60"
															>NO SLAVES</span
														>
													</div>
													<span class="mt-0.5 text-xs leading-tight text-blue-300/40"
														>Click to Connect</span
													>
													<div class="mt-2 text-blue-400/60">
														<span class="icon-[mdi--plus-circle] size-4"></span>
													</div>
												</div>
											{/if}
										</button>
									</div>
								</div>
							{/if}
						</HTML>
					</Billboard>
				</T.Group>
			{/snippet}
		</Hoverable>
	</T.Group>
{/each}

<!-- Master Connection Modal -->
<MasterConnectionModal bind:open={isMasterModalOpen} robot={selectedRobot} />

<!-- Slave Connection Modal -->
<SlaveConnectionModal bind:open={isSlaveModalOpen} robot={selectedRobot} />

<!-- Manual Control Modal -->
<ManualControlModal bind:open={isManualControlModalOpen} robot={selectedRobot} />