src/lib/components/panel/RobotControlPanel.svelte

<script lang="ts">
    import { robotManager } from "$lib/robot/RobotManager.svelte";
    import { robotUrdfConfigMap } from "$lib/configs/robotUrdfConfig";
    import type { Robot } from "$lib/robot/Robot.svelte";

    interface Props {}
    
    let {}: Props = $props();
    
    // Local state
    let selectedRobotType = $state("so-arm100");
    let isCreating = $state(false);
    let error = $state<string | undefined>(undefined);

    // Robot selection modal state
    let showRobotSelectionModal = $state(false);
    let availableServerRobots = $state<{ id: string; name: string; robot_type: string }[]>([]);
    let selectedServerRobotId = $state<string>("");
    let pendingLocalRobot: Robot | null = $state(null);

    // Reactive values
    const robots = $derived(robotManager.robots);
    const robotsWithSlaves = $derived(robotManager.robotsWithSlaves.length);
    const robotsWithMaster = $derived(robotManager.robotsWithMaster.length);

    async function createRobot() {
        if (isCreating) return;
        
        console.log('Creating robot...');
        isCreating = true;
        error = undefined;
        
        try {
            const urdfConfig = robotUrdfConfigMap[selectedRobotType];
            if (!urdfConfig) {
                throw new Error(`Unknown robot type: ${selectedRobotType}`);
            }
            
            const robotId = `robot-${Date.now()}`;
            console.log('Creating robot with ID:', robotId, 'and config:', urdfConfig);
            const robot = await robotManager.createRobot(robotId, urdfConfig);
            console.log('Robot created successfully:', robot);
            
        } catch (err) {
            error = `Failed to create robot: ${err}`;
            console.error('Robot creation failed:', err);
        } finally {
            isCreating = false;
        }
    }

    // Master connection functions
    async function connectMockSequenceMaster(robot: Robot) {
        try {
            await robotManager.connectDemoSequences(robot.id, true);
        } catch (err) {
            error = `Failed to connect demo sequences: ${err}`;
            console.error(err);
        }
    }

    async function connectRemoteServerMaster(robot: Robot) {
        try {
            const config: import('$lib/types/robotDriver').MasterDriverConfig = {
                type: "remote-server",
                url: "ws://localhost:8080",
                apiKey: undefined,
                pollInterval: 100
            };
            await robotManager.connectMaster(robot.id, config);
        } catch (err) {
            error = `Failed to connect remote server: ${err}`;
            console.error(err);
        }
    }

    async function disconnectMaster(robot: Robot) {
        try {
            await robotManager.disconnectMaster(robot.id);
        } catch (err) {
            error = `Failed to disconnect master: ${err}`;
            console.error(err);
        }
    }

    // Slave connection functions
    async function connectMockSlave(robot: Robot) {
        try {
            await robotManager.connectMockSlave(robot.id, 50);
        } catch (err) {
            error = `Failed to connect mock slave: ${err}`;
            console.error(err);
        }
    }

    async function connectUSBSlave(robot: Robot) {
        try {
            await robotManager.connectUSBSlave(robot.id);
        } catch (err) {
            error = `Failed to connect USB slave: ${err}`;
            console.error(err);
        }
    }

    async function connectRemoteServerSlave(robot: Robot) {
        try {
            // First, fetch available robots from the server
            const serverUrl = "http://localhost:8080";
            const response = await fetch(`${serverUrl}/api/robots`);
            
            if (!response.ok) {
                throw new Error(`Server responded with ${response.status}: ${response.statusText}`);
            }
            
            const robots = await response.json();
            
            if (robots.length === 0) {
                error = "No robots available on the server. Create a robot on the server first.";
                return;
            }
            
            // Show modal for robot selection
            availableServerRobots = robots;
            pendingLocalRobot = robot;
            selectedServerRobotId = robots[0]?.id || "";
            showRobotSelectionModal = true;
            
        } catch (err) {
            error = `Failed to fetch server robots: ${err}`;
            console.error(err);
        }
    }

    async function confirmRobotSelection() {
        if (!pendingLocalRobot || !selectedServerRobotId) return;
        
        try {
            await robotManager.connectRemoteServerSlave(
                pendingLocalRobot.id, 
                "ws://localhost:8080", 
                undefined, 
                selectedServerRobotId
            );
            
            // Close modal
            showRobotSelectionModal = false;
            pendingLocalRobot = null;
            
        } catch (err) {
            error = `Failed to connect remote server slave: ${err}`;
            console.error(err);
        }
    }

    function cancelRobotSelection() {
        showRobotSelectionModal = false;
        pendingLocalRobot = null;
        selectedServerRobotId = "";
    }

    async function disconnectSlave(robot: Robot, slaveId: string) {
        try {
            await robotManager.disconnectSlave(robot.id, slaveId);
        } catch (err) {
            error = `Failed to disconnect slave: ${err}`;
            console.error(err);
        }
    }

    // Robot management
    async function calibrateRobot(robot: Robot) {
        try {
            await robot.calibrateRobot();
        } catch (err) {
            error = `Failed to calibrate: ${err}`;
            console.error(err);
        }
    }

    async function moveToRest(robot: Robot) {
        try {
            await robot.moveToRestPosition();
        } catch (err) {
            error = `Failed to move to rest: ${err}`;
            console.error(err);
        }
    }

    function clearCalibration(robot: Robot) {
        robot.clearCalibration();
    }

    async function removeRobot(robot: Robot) {
        try {
            await robotManager.removeRobot(robot.id);
        } catch (err) {
            error = `Failed to remove robot: ${err}`;
            console.error(err);
        }
    }

    function clearError() {
        error = undefined;
    }

    // Helper functions
    function getConnectionStatusText(robot: Robot): string {
        const hasActiveMaster = robot.controlState.hasActiveMaster;
        const connectedSlaves = robot.connectedSlaves.length;
        const totalSlaves = robot.slaves.length;
        
        if (hasActiveMaster && connectedSlaves > 0) {
            return `Master + ${connectedSlaves}/${totalSlaves} Slaves`;
        } else if (hasActiveMaster) {
            return `Master Only`;
        } else if (connectedSlaves > 0) {
            return `${connectedSlaves}/${totalSlaves} Slaves`;
        } else {
            return "Manual Control";
        }
    }

    function getConnectionStatusColor(robot: Robot): string {
        const hasActiveMaster = robot.controlState.hasActiveMaster;
        const connectedSlaves = robot.connectedSlaves.length;
        
        if (hasActiveMaster && connectedSlaves > 0) {
            return "green";  // Full master-slave setup
        } else if (hasActiveMaster || connectedSlaves > 0) {
            return "yellow"; // Partial connection
        } else {
            return "red";    // No connections
        }
    }

    async function connectUSBMaster(robot: Robot) {
        try {
            await robotManager.connectUSBMaster(robot.id, {
                pollInterval: 200,
                smoothing: true
            });
        } catch (err) {
            error = `Failed to connect USB master: ${err}`;
            console.error(err);
        }
    }
</script>

<div class="space-y-6 p-4">
    <div class="flex items-center justify-between">
        <h2 class="text-xl font-bold text-slate-100">Robot Control - Master/Slave Architecture</h2>
        <div class="text-sm text-slate-400">
            {robots.length} robots, {robotsWithMaster} with masters, {robotsWithSlaves} with slaves
        </div>
    </div>

    <!-- Error Display -->
    {#if error}
        <div class="bg-red-500/20 border border-red-500 rounded-lg p-3 flex items-center justify-between">
            <span class="text-red-200 text-sm">{error}</span>
            <button 
                onclick={clearError}
                class="text-red-200 hover:text-white"
            >×</button>
        </div>
    {/if}

    <!-- Create Robot Section -->
    <div class="bg-slate-800 rounded-lg p-4 space-y-4">
        <h3 class="text-lg font-semibold text-slate-100">Create Robot</h3>
        
        <div class="flex gap-3">
            <select 
                bind:value={selectedRobotType}
                class="flex-1 px-3 py-2 bg-slate-700 border border-slate-600 rounded-md text-slate-100"
            >
                {#each Object.keys(robotUrdfConfigMap) as robotType}
                    <option value={robotType}>{robotType}</option>
                {/each}
            </select>
            
            <button 
                onclick={createRobot}
                disabled={isCreating}
                class="px-4 py-2 bg-blue-600 hover:bg-blue-700 disabled:bg-slate-600 disabled:cursor-not-allowed text-white rounded-md transition-colors"
            >
                {isCreating ? "Creating..." : "Create Robot"}
            </button>
        </div>
    </div>

    <!-- Robots List -->
    <div class="space-y-3">
        {#each robots as robot (robot.id)}
            <div class="bg-slate-800 rounded-lg p-4">
                <div class="flex items-center justify-between mb-3">
                    <div>
                        <h4 class="font-semibold text-slate-100">{robot.id}</h4>
                        <div class="text-sm text-slate-400">
                            Status: 
                            <span class="text-{getConnectionStatusColor(robot)}-400">
                                {getConnectionStatusText(robot)}
                            </span>
                            {#if robot.controlState.lastCommandSource !== "none"}
                                <span class="text-blue-400 ml-2">• Last: {robot.controlState.lastCommandSource}</span>
                            {/if}
                            {#if robot.isCalibrated}
                                <span class="text-green-400 ml-2">• Calibrated</span>
                            {/if}
                        </div>
                    </div>
                    
                    <button 
                        onclick={() => removeRobot(robot)}
                        class="px-3 py-1 bg-red-600 hover:bg-red-700 text-white text-sm rounded transition-colors"
                    >
                        Remove
                    </button>
                </div>

                <!-- Master Controls -->
                <div class="mb-4 p-3 bg-orange-500/10 border border-orange-500/30 rounded-lg">
                    <div class="text-sm text-orange-200 mb-2">
                        <strong>Masters (Control Sources)</strong>
                        {#if robot.controlState.hasActiveMaster}
                            - <span class="text-green-400">Connected: {robot.controlState.masterName}</span>
                        {:else}
                            - <span class="text-slate-400">None Connected</span>
                        {/if}
                    </div>
                    
                    <div class="flex flex-wrap gap-2">
                        <button
                            class="px-3 py-1.5 bg-orange-500 text-white rounded text-sm hover:bg-orange-600 transition-colors disabled:bg-slate-600 disabled:cursor-not-allowed"
                            onclick={() => connectMockSequenceMaster(robot)}
                            disabled={robot.controlState.hasActiveMaster}
                        >
                            Demo Sequences
                        </button>
                        
                        <button
                            class="px-3 py-1.5 bg-purple-500 text-white rounded text-sm hover:bg-purple-600 transition-colors disabled:bg-slate-600 disabled:cursor-not-allowed"
                            onclick={() => connectRemoteServerMaster(robot)}
                            disabled={robot.controlState.hasActiveMaster}
                        >
                            Connect Remote Server
                        </button>

                        <button
                            class="px-3 py-1.5 bg-blue-500 text-white rounded text-sm hover:bg-blue-600 transition-colors disabled:bg-slate-600 disabled:cursor-not-allowed"
                            onclick={() => connectUSBMaster(robot)}
                            disabled={robot.controlState.hasActiveMaster}
                        >
                            Connect USB Master
                        </button>
                        
                        {#if robot.controlState.hasActiveMaster}
                            <button
                                class="px-3 py-1.5 bg-red-500 text-white rounded text-sm hover:bg-red-600 transition-colors"
                                onclick={() => disconnectMaster(robot)}
                            >
                                Disconnect Master
                            </button>
                        {/if}
                    </div>
                </div>

                <!-- Slave Controls -->
                <div class="mb-4 p-3 bg-blue-500/10 border border-blue-500/30 rounded-lg">
                    <div class="text-sm text-blue-200 mb-2">
                        <strong>Slaves (Execution Targets)</strong>
                        - <span class="text-green-400">{robot.connectedSlaves.length} Connected</span>
                        / <span class="text-slate-400">{robot.slaves.length} Total</span>
                    </div>
                    
                    <div class="flex gap-2 mb-2">
                        <button 
                            onclick={() => connectMockSlave(robot)}
                            class="px-3 py-1 bg-yellow-600 hover:bg-yellow-700 text-white text-sm rounded transition-colors"
                        >
                            Add Mock Slave
                        </button>
                        <button 
                            onclick={() => connectUSBSlave(robot)}
                            class="px-3 py-1 bg-green-600 hover:bg-green-700 text-white text-sm rounded transition-colors"
                        >
                            Add USB Slave
                        </button>
                        <button 
                            onclick={() => connectRemoteServerSlave(robot)}
                            class="px-3 py-1 bg-purple-600 hover:bg-purple-700 text-white text-sm rounded transition-colors"
                        >
                            Add Remote Server Slave
                        </button>
                    </div>
                    
                    {#if robot.slaves.length > 0}
                        <div class="text-xs text-blue-300">
                            <strong>Connected Slaves:</strong>
                            {#each robot.slaves as slave}
                                <div class="flex items-center justify-between mt-1">
                                    <span>{slave.name} ({slave.id})</span>
                                    <button 
                                        onclick={() => disconnectSlave(robot, slave.id)}
                                        class="px-2 py-1 bg-red-500 hover:bg-red-600 text-white text-xs rounded"
                                    >
                                        Remove
                                    </button>
                                </div>
                            {/each}
                        </div>
                    {/if}
                </div>

                <!-- Calibration Section (for USB slaves) -->
                {#if robot.connectedSlaves.some(slave => slave.name.includes("USB"))}
                    <div class="mb-3 p-3 bg-orange-500/10 border border-orange-500/30 rounded-lg">
                        <div class="text-sm text-orange-200 mb-2">
                            <strong>USB Robot Calibration</strong>
                        </div>
                        {#if !robot.isCalibrated}
                            <div class="text-xs text-orange-300 mb-2">
                                1. Manually position your robot to match the digital twin's rest pose<br>
                                2. Click "Calibrate" when positioned correctly
                            </div>
                            <div class="flex gap-2">
                                <button 
                                    onclick={() => moveToRest(robot)}
                                    class="px-3 py-1 bg-blue-600 hover:bg-blue-700 text-white text-sm rounded transition-colors"
                                >
                                    Show Rest Pose
                                </button>
                                <button 
                                    onclick={() => calibrateRobot(robot)}
                                    class="px-3 py-1 bg-green-600 hover:bg-green-700 text-white text-sm rounded transition-colors"
                                >
                                    Calibrate
                                </button>
                            </div>
                        {:else}
                            <div class="text-xs text-green-300 mb-2">
                                ✓ Robot calibrated at {robot.calibrationState.calibrationTime?.toLocaleTimeString()}
                            </div>
                            <button 
                                onclick={() => clearCalibration(robot)}
                                class="px-3 py-1 bg-gray-600 hover:bg-gray-700 text-white text-sm rounded transition-colors"
                            >
                                Clear Calibration
                            </button>
                        {/if}
                    </div>
                {/if}

                <!-- Manual Joint Controls (only when no master) -->
                {#if robot.manualControlEnabled}
                    <div class="space-y-3">
                        <h5 class="text-sm font-medium text-slate-300">Manual Control ({robot.activeJoints.length} active joints)</h5>
                        {#each robot.activeJoints as joint}
                            <div class="joint-control">
                                <div class="joint-header">
                                    <span class="joint-name">{joint.name}</span>
                                    <div class="joint-values">
                                        <span class="virtual-value">{joint.virtualValue.toFixed(0)}°</span>
                                        {#if joint.realValue !== undefined}
                                            <span class="real-value" title="Real robot position">
                                                Real: {joint.realValue.toFixed(0)}°
                                            </span>
                                        {:else}
                                            <span class="real-value disconnected">N/A</span>
                                        {/if}
                                    </div>
                                </div>
                                <input 
                                    type="range"
                                    min="-180"
                                    max="180"
                                    step="1"
                                    value={joint.virtualValue}
                                    oninput={(e) => {
                                        const target = e.target as HTMLInputElement;
                                        robot.updateJointValue(joint.name, parseFloat(target.value));
                                    }}
                                    class="joint-slider"
                                />
                            </div>
                        {/each}
                        
                        {#if robot.activeJoints.length === 0}
                            <div class="text-sm text-slate-500 italic">No active joints</div>
                        {/if}
                    </div>
                {:else}
                    <div class="p-3 bg-purple-500/10 border border-purple-500/30 rounded-lg">
                        <div class="text-sm text-purple-200">
                            🎮 <strong>Master Control Active</strong><br>
                            <span class="text-xs text-purple-300">
                                Robot is controlled by: {robot.controlState.masterName}<br>
                                Manual controls are disabled. Disconnect master to regain manual control.
                            </span>
                        </div>
                    </div>
                {/if}
            </div>
        {/each}
        
        {#if robots.length === 0}
            <div class="text-center text-slate-500 py-8">
                No robots created yet. Create one above to get started with the master-slave architecture.
            </div>
        {/if}
    </div>
</div>

<!-- Robot Selection Modal -->
{#if showRobotSelectionModal}
    <div class="fixed inset-0 bg-black bg-opacity-50 flex items-center justify-center z-50">
        <div class="bg-slate-800 rounded-lg p-6 w-96 max-w-full mx-4">
            <h3 class="text-lg font-semibold text-slate-100 mb-4">Select Server Robot</h3>
            
            <div class="mb-4">
                <label class="block text-sm font-medium text-slate-300 mb-2">
                    Available robots on server:
                </label>
                <select 
                    bind:value={selectedServerRobotId}
                    class="w-full px-3 py-2 bg-slate-700 border border-slate-600 rounded-md text-slate-100"
                >
                    {#each availableServerRobots as serverRobot}
                        <option value={serverRobot.id}>
                            {serverRobot.name} ({serverRobot.id}) - {serverRobot.robot_type}
                        </option>
                    {/each}
                </select>
            </div>
            
            <div class="text-sm text-slate-400 mb-4">
                This will connect your local robot "{pendingLocalRobot?.id}" as a slave to 
                receive commands from the selected server robot.
            </div>
            
            <div class="flex gap-3 justify-end">
                <button 
                    onclick={cancelRobotSelection}
                    class="px-4 py-2 bg-slate-600 hover:bg-slate-700 text-white rounded-md transition-colors"
                >
                    Cancel
                </button>
                <button 
                    onclick={confirmRobotSelection}
                    class="px-4 py-2 bg-purple-600 hover:bg-purple-700 text-white rounded-md transition-colors"
                    disabled={!selectedServerRobotId}
                >
                    Connect Slave
                </button>
            </div>
        </div>
    </div>
{/if}

<style>
    .joint-control {
        background: rgba(71, 85, 105, 0.3);
        border-radius: 8px;
        padding: 12px;
        border: 1px solid rgba(71, 85, 105, 0.5);
    }

    .joint-header {
        display: flex;
        justify-content: space-between;
        align-items: center;
        margin-bottom: 8px;
    }

    .joint-name {
        font-weight: 500;
        color: #e2e8f0;
        font-size: 14px;
    }

    .joint-values {
        display: flex;
        gap: 12px;
        font-size: 12px;
    }

    .virtual-value {
        color: #60a5fa;
        font-weight: 500;
    }

    .real-value {
        color: #34d399;
        font-weight: 500;
    }

    .real-value.disconnected {
        color: #f87171;
    }

    .joint-slider {
        width: 100%;
        height: 6px;
        background: #374151;
        border-radius: 3px;
        outline: none;
        cursor: pointer;
        appearance: none;
    }

    .joint-slider::-webkit-slider-thumb {
        appearance: none;
        width: 18px;
        height: 18px;
        border-radius: 50%;
        background: #3b82f6;
        cursor: pointer;
        border: 2px solid #1e293b;
        box-shadow: 0 2px 4px rgba(0, 0, 0, 0.2);
        transition: all 0.15s ease;
    }

    .joint-slider::-webkit-slider-thumb:hover {
        background: #2563eb;
        transform: scale(1.1);
    }

    .joint-slider::-moz-range-thumb {
        width: 18px;
        height: 18px;
        border-radius: 50%;
        background: #3b82f6;
        cursor: pointer;
        border: 2px solid #1e293b;
        box-shadow: 0 2px 4px rgba(0, 0, 0, 0.2);
        transition: all 0.15s ease;
    }

    .joint-slider::-moz-range-track {
        height: 6px;
        background: #374151;
        border-radius: 3px;
        border: none;
    }

    .joint-slider:focus {
        box-shadow: 0 0 0 2px rgba(59, 130, 246, 0.5);
    }
</style>