frontend/app.js

// RMScript Web Demo - Frontend Application
// Connects to:
// - Backend API for compilation/verification
// - Robot WebSocket (localhost:8000) for execution

// Auto-detect backend URL based on environment
// If opened as file:// (local development), use localhost:8001
// If served from web server (production/Space), use relative URLs
const BACKEND_URL = window.location.protocol === 'file:'
    ? 'http://localhost:8001'
    : '';  // Use relative URLs when served from web server
const ROBOT_WS_URL = 'ws://localhost:8000/api/move/ws/set_target';

// Global state
const state = {
    robotWs: null,
    robotConnected: false,
    currentIR: null,
    isExecuting: false
};

// DOM elements
const elements = {
    editor: document.getElementById('editor'),
    compileStatus: document.getElementById('compileStatus'),
    robotStatus: document.getElementById('robotStatus'),
    console: document.getElementById('console'),
    irDisplay: document.getElementById('irDisplay'),
    executionInfo: document.getElementById('executionInfo'),
    executeBtn: document.getElementById('executeBtn')
};

// Example scripts
const examples = {
    basic: `DESCRIPTION Simple head movement
look left
wait 1s
look right
wait 1s
look center`,

    complex: `DESCRIPTION Wave hello
look left
antenna both up
wait 1s
look right
antenna both down
wait 0.5s
look center`,

    repeat: `DESCRIPTION Repeat demo
REPEAT 3
  look left
  wait 0.5s
  look right
  wait 0.5s
END
look center`
};

// Initialize robot WebSocket connection (used for connection status only)
// Actual movement commands are sent via REST API /goto endpoint
function connectRobotWebSocket() {
    log('Connecting to robot...', 'info');

    state.robotWs = new WebSocket(ROBOT_WS_URL);

    state.robotWs.onopen = () => {
        state.robotConnected = true;
        updateRobotStatus(true);
        log('Robot connected!', 'success');
    };

    state.robotWs.onclose = () => {
        state.robotConnected = false;
        updateRobotStatus(false);
        log('Robot disconnected. Reconnecting...', 'warning');
        setTimeout(connectRobotWebSocket, 2000);
    };

    state.robotWs.onerror = (error) => {
        log(`Robot connection error: ${error}`, 'error');
    };

    state.robotWs.onmessage = (event) => {
        try {
            const message = JSON.parse(event.data);
            if (message.status === 'error') {
                log(`Robot error: ${message.detail}`, 'error');
            }
        } catch (e) {
            console.error('Failed to parse robot message:', e);
        }
    };
}

// Update robot connection status UI
function updateRobotStatus(connected) {
    if (connected) {
        elements.robotStatus.className = 'status connected';
        elements.robotStatus.innerHTML = '<span><span class="status-dot green"></span>Connected</span>';
        elements.executeBtn.disabled = false;
    } else {
        elements.robotStatus.className = 'status disconnected';
        elements.robotStatus.innerHTML = '<span><span class="status-dot red"></span>Disconnected</span>';
        elements.executeBtn.disabled = true;
    }
}

// Update compile status UI
function updateCompileStatus(status, message) {
    const dot = status === 'success' ? 'green' : status === 'error' ? 'red' : 'gray';
    elements.compileStatus.className = `status ${status}`;
    elements.compileStatus.innerHTML = `<span><span class="status-dot ${dot}"></span>${message}</span>`;
}

// Log to console
function log(message, type = 'info') {
    const line = document.createElement('div');
    line.className = `console-line ${type}`;
    line.textContent = `[${new Date().toLocaleTimeString()}] ${message}`;
    elements.console.appendChild(line);
    elements.console.scrollTop = elements.console.scrollHeight;
}

// Clear console
function clearConsole() {
    elements.console.innerHTML = '';
}

// Load example script
function loadExample(exampleName) {
    if (examples[exampleName]) {
        elements.editor.value = examples[exampleName];
        log(`Loaded ${exampleName} example`, 'info');
    }
}

// Clear editor
function clearEditor() {
    elements.editor.value = '';
    state.currentIR = null;
    elements.irDisplay.innerHTML = '<div style="color: #999; text-align: center; padding: 20px;">No IR yet. Verify a script first!</div>';
    updateCompileStatus('idle', 'Ready');
}

// Verify script (syntax and semantics only)
async function verifyScript() {
    const source = elements.editor.value.trim();

    if (!source) {
        log('Editor is empty', 'warning');
        return;
    }

    clearConsole();
    log('Verifying script...', 'info');
    updateCompileStatus('idle', 'Verifying...');

    try {
        const response = await fetch(`${BACKEND_URL}/api/verify`, {
            method: 'POST',
            headers: { 'Content-Type': 'application/json' },
            body: JSON.stringify({ source })
        });

        const result = await response.json();

        if (result.success) {
            log('✓ Verification successful!', 'success');
            if (result.name) log(`  Name: ${result.name}`, 'info');
            if (result.description) log(`  Description: ${result.description}`, 'info');

            if (result.warnings.length > 0) {
                result.warnings.forEach(w => {
                    log(`  Warning (line ${w.line}): ${w.message}`, 'warning');
                });
            }

            updateCompileStatus('success', 'Valid script');
        } else {
            log('✗ Verification failed', 'error');
            result.errors.forEach(e => {
                log(`  Error (line ${e.line}): ${e.message}`, 'error');
            });
            updateCompileStatus('error', 'Invalid script');
        }
    } catch (error) {
        log(`Backend error: ${error.message}`, 'error');
        updateCompileStatus('error', 'Backend error');
    }
}

// Compile script to IR
async function compileScript() {
    const source = elements.editor.value.trim();

    if (!source) {
        log('Editor is empty', 'warning');
        return null;
    }

    log('Compiling script to IR...', 'info');
    updateCompileStatus('idle', 'Compiling...');

    try {
        const response = await fetch(`${BACKEND_URL}/api/compile`, {
            method: 'POST',
            headers: { 'Content-Type': 'application/json' },
            body: JSON.stringify({ source })
        });

        const result = await response.json();

        if (result.success) {
            log(`✓ Compiled ${result.ir.length} actions`, 'success');

            if (result.warnings.length > 0) {
                result.warnings.forEach(w => {
                    log(`  Warning (line ${w.line}): ${w.message}`, 'warning');
                });
            }

            state.currentIR = result.ir;
            displayIR(result.ir);
            updateCompileStatus('success', `${result.ir.length} actions ready`);
            return result.ir;
        } else {
            log('✗ Compilation failed', 'error');
            result.errors.forEach(e => {
                log(`  Error (line ${e.line}): ${e.message}`, 'error');
            });
            updateCompileStatus('error', 'Compilation failed');
            return null;
        }
    } catch (error) {
        log(`Backend error: ${error.message}`, 'error');
        updateCompileStatus('error', 'Backend error');
        return null;
    }
}

// Display IR in the UI
function displayIR(ir) {
    if (!ir || ir.length === 0) {
        elements.irDisplay.innerHTML = '<div style="color: #999; text-align: center; padding: 20px;">No actions</div>';
        return;
    }

    const html = ir.map((action, idx) => {
        let details = '';

        if (action.type === 'action') {
            details = `Duration: ${action.duration}s`;
            if (action.head_pose) details += ', Head movement';
            if (action.antennas) details += `, Antennas: [${action.antennas.map(a => a.toFixed(2)).join(', ')}]`;
            if (action.body_yaw !== null) details += `, Body yaw: ${action.body_yaw.toFixed(2)}`;
        } else if (action.type === 'wait') {
            details = `Wait ${action.duration}s`;
        } else if (action.type === 'picture') {
            details = 'Take picture';
        } else if (action.type === 'sound') {
            details = `Play "${action.sound_name}"`;
            if (action.blocking) details += ' (blocking)';
            if (action.loop) details += ' (loop)';
        }

        return `
            <div class="ir-action">
                <div class="ir-action-type">${idx + 1}. ${action.type.toUpperCase()}</div>
                <div class="ir-action-details">${details}</div>
            </div>
        `;
    }).join('');

    elements.irDisplay.innerHTML = html;
}

// Execute compiled script on robot
async function executeScript() {
    if (!state.robotConnected) {
        log('Robot not connected!', 'error');
        return;
    }

    if (state.isExecuting) {
        log('Already executing a script', 'warning');
        return;
    }

    // Always compile the current editor content before executing
    const ir = await compileScript();
    if (!ir) {
        log('Cannot execute - compilation failed', 'error');
        return;
    }

    // Execute IR actions sequentially
    state.isExecuting = true;
    elements.executeBtn.disabled = true;
    clearExecutionInfo();
    logExecution('Starting execution...', 'info');

    try {
        for (let i = 0; i < ir.length; i++) {
            const action = ir[i];
            logExecution(`Action ${i + 1}/${ir.length}: ${action.type}`, 'info');

            if (action.type === 'action') {
                await executeIRAction(action);
            } else if (action.type === 'wait') {
                await sleep(action.duration * 1000);
            } else if (action.type === 'picture') {
                logExecution('Picture action skipped (not implemented)', 'warning');
            } else if (action.type === 'sound') {
                logExecution('Sound action skipped (not implemented)', 'warning');
            }
        }

        logExecution('✓ Execution complete!', 'success');
    } catch (error) {
        logExecution(`✗ Execution error: ${error.message}`, 'error');
    } finally {
        state.isExecuting = false;
        if (state.robotConnected) {
            elements.executeBtn.disabled = false;
        }
    }
}

// Extract euler angles (roll, pitch, yaw) from 3x3 rotation matrix
// Uses XYZ euler convention to match scipy's R.from_matrix().as_euler("xyz")
function matrixToEulerXYZ(rotMatrix) {
    // Extract 3x3 rotation matrix from 4x4 pose matrix if needed
    const R = rotMatrix;

    // For XYZ euler order: R = Rz(yaw) * Ry(pitch) * Rx(roll)
    // pitch = asin(-R[2][0])
    // roll = atan2(R[2][1], R[2][2])
    // yaw = atan2(R[1][0], R[0][0])

    let pitch = Math.asin(-R[2][0]);

    // Check for gimbal lock
    if (Math.abs(Math.cos(pitch)) > 1e-6) {
        // Normal case
        const roll = Math.atan2(R[2][1], R[2][2]);
        const yaw = Math.atan2(R[1][0], R[0][0]);
        return { roll, pitch, yaw };
    } else {
        // Gimbal lock case
        const roll = Math.atan2(-R[0][1], R[1][1]);
        const yaw = 0;
        return { roll, pitch, yaw };
    }
}

// Execute a single IR action (send to robot)
async function executeIRAction(action) {
    if (!state.robotConnected) {
        throw new Error('Robot not connected');
    }

    // Extract pose data
    let head_pose = null;
    if (action.head_pose) {
        const x = action.head_pose[0][3];
        const y = action.head_pose[1][3];
        const z = action.head_pose[2][3];
        const euler = matrixToEulerXYZ(action.head_pose);

        head_pose = {
            x: x,
            y: y,
            z: z,
            roll: euler.roll,
            pitch: euler.pitch,
            yaw: euler.yaw
        };
    }

    // Try using /goto endpoint (works locally, respects duration)
    // Falls back to WebSocket if fetch fails (e.g., on HuggingFace Spaces HTTPS -> HTTP localhost)
    try {
        const gotoRequest = {
            duration: action.duration,
            interpolation: action.interpolation || 'minjerk'
        };

        if (head_pose) gotoRequest.head_pose = head_pose;
        if (action.antennas) gotoRequest.antennas = action.antennas;
        if (action.body_yaw !== null) gotoRequest.body_yaw = action.body_yaw;

        const response = await fetch('http://localhost:8000/api/move/goto', {
            method: 'POST',
            headers: { 'Content-Type': 'application/json' },
            body: JSON.stringify(gotoRequest)
        });

        if (!response.ok) {
            throw new Error('goto endpoint failed');
        }

        // Wait for movement to complete
        await sleep(action.duration * 1000 + 100);

    } catch (fetchError) {
        // Fallback to WebSocket (for Spaces deployment - HTTPS can't fetch localhost)
        // Note: This won't respect durations - robot moves as fast as possible
        console.warn('Using WebSocket fallback - durations not supported:', fetchError.message);

        if (!state.robotWs || state.robotWs.readyState !== WebSocket.OPEN) {
            throw new Error('Robot WebSocket not connected');
        }

        const wsMessage = {};
        if (head_pose) wsMessage.target_head_pose = head_pose;
        if (action.antennas) wsMessage.target_antennas = action.antennas;
        if (action.body_yaw !== null) wsMessage.target_body_yaw = action.body_yaw;

        state.robotWs.send(JSON.stringify(wsMessage));

        // Wait for specified duration to maintain script timing
        // (robot moves fast, but at least script execution pauses correctly)
        await sleep(action.duration * 1000);
    }
}

// Log execution info
function logExecution(message, type = 'info') {
    const line = document.createElement('div');
    line.className = `console-line ${type}`;
    line.textContent = `[${new Date().toLocaleTimeString()}] ${message}`;
    elements.executionInfo.appendChild(line);
    elements.executionInfo.scrollTop = elements.executionInfo.scrollHeight;
}

// Clear execution info
function clearExecutionInfo() {
    elements.executionInfo.innerHTML = '';
}

// Sleep helper
function sleep(ms) {
    return new Promise(resolve => setTimeout(resolve, ms));
}

// Initialize application
function init() {
    console.log('Initializing RMScript Web Demo');

    // Connect to robot
    connectRobotWebSocket();

    // Load default example
    loadExample('basic');

    console.log('RMScript Web Demo ready');
}

// Start when DOM is loaded
if (document.readyState === 'loading') {
    document.addEventListener('DOMContentLoaded', init);
} else {
    init();
}