"""Dora node for controlling a UFactory Lite6 robot and publishing state."""

import json
import os
import threading
import time
from typing import Any, Dict, Optional

import numpy as np
import pyarrow as pa
import uvicorn
from dora import Node
from fastapi import FastAPI, HTTPException
from fastapi.responses import HTMLResponse
from pydantic import BaseModel
from scipy.spatial.transform import Rotation
from xarm.wrapper import XArmAPI


class ULite6Helper:
    """Minimal ULite6 helper based on ufactory-control."""

    def __init__(self, robot_ip: str):
        self.robot_ip = robot_ip
        self.arm = XArmAPI(self.robot_ip)
        self.arm.connect()
        self.arm.motion_enable(enable=True)

    def go_home(self) -> int:
        self.arm.set_mode(0)
        self.arm.set_state(state=0)
        code = self.arm.move_gohome(wait=True)
        # move_gohome may return tuple (code, result) in some versions
        if isinstance(code, tuple):
            code = code[0]
        return code

    def move_to_pose(
        self,
        x: float,
        y: float,
        z: float,
        roll: float,
        pitch: float,
        yaw: float,
        speed: float = 30.0,
        units: str = "mm",
    ) -> int:
        if units == "m":
            x, y, z = x * 1000.0, y * 1000.0, z * 1000.0
        self.arm.set_mode(0)
        self.arm.set_state(0)
        return self.arm.set_position(
            x, y, z, roll=roll, pitch=pitch, yaw=yaw, speed=speed, wait=True
        )

    def get_current_position(self) -> Dict[str, float]:
        code, pos = self.arm.get_position()
        if code != 0:
            raise RuntimeError(f"get_position failed with code {code}")
        return {
            "x": pos[0],
            "y": pos[1],
            "z": pos[2],
            "roll": pos[3],
            "pitch": pos[4],
            "yaw": pos[5],
        }

    def get_pose_matrix(self, tcp_offset_mm: float = 0.0) -> np.ndarray:
        pos = self.get_current_position()
        R = Rotation.from_euler(
            "xyz", [pos["roll"], pos["pitch"], pos["yaw"]], degrees=True
        ).as_matrix()

        position_m = np.array([pos["x"], pos["y"], pos["z"]]) / 1000.0
        if tcp_offset_mm != 0.0:
            tcp_offset_local = np.array([0, 0, tcp_offset_mm]) / 1000.0
            tcp_offset_global = R @ tcp_offset_local
            position_m = position_m + tcp_offset_global

        T = np.eye(4)
        T[:3, :3] = R
        T[:3, 3] = position_m
        return T

    def get_joint_angles_deg(self) -> np.ndarray:
        code, angles = self.arm.get_servo_angle(is_radian=False)
        if code != 0:
            raise RuntimeError(f"get_servo_angle failed with code {code}")
        return np.array(angles, dtype=float)

    def get_status(self) -> Dict[str, Any]:
        """Get robot status information."""
        return {
            "connected": self.arm.connected,
            "state": self.arm.state,
            "mode": self.arm.mode,
            "error_code": self.arm.error_code,
            "warn_code": self.arm.warn_code,
            "has_error": self.arm.has_error,
            "has_warn": self.arm.has_warn,
            "motor_enable_states": self.arm.motor_enable_states,
        }

    def reset_state(self) -> int:
        """Clear errors/warnings and reset robot state."""
        self.arm.clean_error()
        self.arm.clean_warn()
        self.arm.motion_enable(enable=True)
        self.arm.set_mode(0)
        return self.arm.set_state(0)

    def set_vacuum_gripper(self, on: bool) -> int:
        """Turn vacuum gripper on or off."""
        code = self.arm.set_vacuum_gripper(on)
        if isinstance(code, tuple):
            code = code[0]
        return code

    def get_vacuum_gripper(self) -> Dict[str, Any]:
        """Get vacuum gripper status."""
        code, status = self.arm.get_vacuum_gripper()
        return {"code": code, "on": status == 1}

    def disconnect(self) -> int:
        return self.arm.disconnect()


# --- Pydantic models for API requests ---


class MoveToRequest(BaseModel):
    x: float
    y: float
    z: float
    speed: Optional[float] = None
    roll: Optional[float] = 180.0
    pitch: Optional[float] = 0.0
    yaw: Optional[float] = 0.0


class MoveToPoseRequest(BaseModel):
    x: float
    y: float
    z: float
    roll: float
    pitch: float
    yaw: float
    speed: Optional[float] = None


# --- FastAPI app factory ---


def create_api(
    helper: ULite6Helper, default_speed: float, default_units: str, vacuum_enabled: bool
) -> FastAPI:
    """Create FastAPI application with robot control endpoints."""
    app = FastAPI(
        title="ULite6 Robot Control API",
        description="REST API for controlling UFactory Lite6 robot",
        version="0.1.0",
    )

    @app.get("/", response_class=HTMLResponse)
    def web_ui():
        """Serve web control interface."""
        return """<!DOCTYPE html>
<html lang="en">
<head>
    <meta charset="UTF-8">
    <meta name="viewport" content="width=device-width, initial-scale=1.0">
    <title>ULite6 Control</title>
    <style>
        * { box-sizing: border-box; margin: 0; padding: 0; }
        body {
            font-family: -apple-system, BlinkMacSystemFont, 'Segoe UI', Roboto, sans-serif;
            background: #1a1a2e;
            color: #eee;
            min-height: 100vh;
            padding: 20px;
        }
        h1 {
            text-align: center;
            color: #00d4ff;
            margin-bottom: 20px;
            font-size: 1.8rem;
        }
        .container {
            max-width: 800px;
            margin: 0 auto;
            display: grid;
            gap: 16px;
        }
        .card {
            background: #16213e;
            border-radius: 8px;
            padding: 16px;
            border: 1px solid #0f3460;
        }
        .card h2 {
            color: #00d4ff;
            font-size: 1rem;
            margin-bottom: 12px;
            border-bottom: 1px solid #0f3460;
            padding-bottom: 8px;
        }
        .status-grid {
            display: grid;
            grid-template-columns: repeat(auto-fit, minmax(120px, 1fr));
            gap: 8px;
        }
        .status-item {
            background: #0f3460;
            padding: 8px;
            border-radius: 4px;
            text-align: center;
        }
        .status-item .label {
            font-size: 0.75rem;
            color: #888;
            margin-bottom: 4px;
        }
        .status-item .value {
            font-size: 0.9rem;
            font-weight: 600;
        }
        .status-item .value.ok { color: #4ade80; }
        .status-item .value.error { color: #f87171; }
        .pose-grid {
            display: grid;
            grid-template-columns: repeat(6, 1fr);
            gap: 8px;
        }
        .pose-item {
            background: #0f3460;
            padding: 8px;
            border-radius: 4px;
            text-align: center;
        }
        .pose-item .label {
            font-size: 0.7rem;
            color: #888;
        }
        .pose-item .value {
            font-size: 0.85rem;
            font-family: monospace;
        }
        .joints-grid {
            display: grid;
            grid-template-columns: repeat(6, 1fr);
            gap: 8px;
            margin-top: 12px;
        }
        .btn-group {
            display: flex;
            gap: 10px;
            flex-wrap: wrap;
        }
        button {
            background: #0f3460;
            color: #fff;
            border: 1px solid #00d4ff;
            padding: 10px 20px;
            border-radius: 4px;
            cursor: pointer;
            font-size: 0.9rem;
            transition: all 0.2s;
        }
        button:hover { background: #00d4ff; color: #1a1a2e; }
        button:disabled { opacity: 0.5; cursor: not-allowed; }
        button.danger { border-color: #f87171; }
        button.danger:hover { background: #f87171; }
        .form-grid {
            display: grid;
            grid-template-columns: repeat(auto-fit, minmax(100px, 1fr));
            gap: 10px;
            margin-bottom: 12px;
        }
        .form-group {
            display: flex;
            flex-direction: column;
        }
        .form-group label {
            font-size: 0.75rem;
            color: #888;
            margin-bottom: 4px;
        }
        .form-group input {
            background: #0f3460;
            border: 1px solid #16213e;
            color: #fff;
            padding: 8px;
            border-radius: 4px;
            font-size: 0.9rem;
        }
        .form-group input:focus {
            outline: none;
            border-color: #00d4ff;
        }
        #log {
            background: #0a0a15;
            border-radius: 4px;
            padding: 12px;
            font-family: monospace;
            font-size: 0.8rem;
            max-height: 150px;
            overflow-y: auto;
        }
        .log-entry {
            padding: 4px 0;
            border-bottom: 1px solid #16213e;
        }
        .log-entry:last-child { border-bottom: none; }
        .log-entry.success { color: #4ade80; }
        .log-entry.error { color: #f87171; }
        .log-entry .time { color: #666; margin-right: 8px; }
    </style>
</head>
<body>
    <h1>ULite6 Robot Control</h1>
    <div class="container">
        <div class="card">
            <h2>Status</h2>
            <div id="status" class="status-grid">
                <div class="status-item">
                    <div class="label">Connected</div>
                    <div class="value" id="st-connected">--</div>
                </div>
                <div class="status-item">
                    <div class="label">State</div>
                    <div class="value" id="st-state">--</div>
                </div>
                <div class="status-item">
                    <div class="label">Mode</div>
                    <div class="value" id="st-mode">--</div>
                </div>
                <div class="status-item">
                    <div class="label">Error</div>
                    <div class="value" id="st-error">--</div>
                </div>
                <div class="status-item">
                    <div class="label">Warning</div>
                    <div class="value" id="st-warn">--</div>
                </div>
            </div>
        </div>

        <div class="card">
            <h2>TCP Pose (mm, deg)</h2>
            <div class="pose-grid">
                <div class="pose-item"><div class="label">X</div><div class="value" id="pose-x">--</div></div>
                <div class="pose-item"><div class="label">Y</div><div class="value" id="pose-y">--</div></div>
                <div class="pose-item"><div class="label">Z</div><div class="value" id="pose-z">--</div></div>
                <div class="pose-item"><div class="label">Roll</div><div class="value" id="pose-roll">--</div></div>
                <div class="pose-item"><div class="label">Pitch</div><div class="value" id="pose-pitch">--</div></div>
                <div class="pose-item"><div class="label">Yaw</div><div class="value" id="pose-yaw">--</div></div>
            </div>
            <h2 style="margin-top: 16px;">Joint Angles (deg)</h2>
            <div class="joints-grid">
                <div class="pose-item"><div class="label">J1</div><div class="value" id="j0">--</div></div>
                <div class="pose-item"><div class="label">J2</div><div class="value" id="j1">--</div></div>
                <div class="pose-item"><div class="label">J3</div><div class="value" id="j2">--</div></div>
                <div class="pose-item"><div class="label">J4</div><div class="value" id="j3">--</div></div>
                <div class="pose-item"><div class="label">J5</div><div class="value" id="j4">--</div></div>
                <div class="pose-item"><div class="label">J6</div><div class="value" id="j5">--</div></div>
            </div>
        </div>

        <div class="card">
            <h2>Controls</h2>
            <div class="btn-group" style="margin-bottom: 16px;">
                <button onclick="goHome()" id="btn-home">Home</button>
                <button onclick="resetState()" id="btn-reset">Reset</button>
            </div>
            <div id="vacuum-controls" style="display: none; margin-bottom: 16px;">
                <h2>Vacuum Gripper</h2>
                <div class="btn-group" style="margin-top: 8px;">
                    <button onclick="vacuumOn()" id="btn-vacuum-on">Vacuum ON</button>
                    <button onclick="vacuumOff()" id="btn-vacuum-off">Vacuum OFF</button>
                    <span id="vacuum-status" style="margin-left: 12px; padding: 8px;">Status: --</span>
                </div>
            </div>
            <h2>Move To Position</h2>
            <form id="moveForm" onsubmit="moveTo(event)">
                <div class="form-grid">
                    <div class="form-group">
                        <label for="move-x">X (mm)</label>
                        <input type="number" id="move-x" step="0.1" required>
                    </div>
                    <div class="form-group">
                        <label for="move-y">Y (mm)</label>
                        <input type="number" id="move-y" step="0.1" required>
                    </div>
                    <div class="form-group">
                        <label for="move-z">Z (mm)</label>
                        <input type="number" id="move-z" step="0.1" required>
                    </div>
                    <div class="form-group">
                        <label for="move-speed">Speed (mm/s)</label>
                        <input type="number" id="move-speed" step="1" value="30" min="1" max="200">
                    </div>
                </div>
                <button type="submit" id="btn-move">Move</button>
            </form>
        </div>

        <div class="card">
            <h2>Response Log</h2>
            <div id="log"></div>
        </div>
    </div>

    <script>
        const $ = id => document.getElementById(id);

        function log(msg, isError = false) {
            const logEl = $('log');
            const time = new Date().toLocaleTimeString();
            const entry = document.createElement('div');
            entry.className = 'log-entry ' + (isError ? 'error' : 'success');
            entry.innerHTML = '<span class="time">' + time + '</span>' + msg;
            logEl.insertBefore(entry, logEl.firstChild);
            if (logEl.children.length > 50) logEl.removeChild(logEl.lastChild);
        }

        async function fetchJson(url, opts = {}) {
            try {
                const res = await fetch(url, opts);
                return await res.json();
            } catch (e) {
                return { error: e.message };
            }
        }

        async function updateStatus() {
            const data = await fetchJson('/api/status');
            if (data.error) {
                $('st-connected').textContent = 'Error';
                $('st-connected').className = 'value error';
                return;
            }
            $('st-connected').textContent = data.connected ? 'Yes' : 'No';
            $('st-connected').className = 'value ' + (data.connected ? 'ok' : 'error');
            $('st-state').textContent = data.state;
            $('st-mode').textContent = data.mode;
            $('st-error').textContent = data.error_code;
            $('st-error').className = 'value ' + (data.error_code === 0 ? 'ok' : 'error');
            $('st-warn').textContent = data.warn_code;
            $('st-warn').className = 'value ' + (data.warn_code === 0 ? 'ok' : 'error');
        }

        async function updatePose() {
            const pose = await fetchJson('/api/pose');
            if (!pose.error) {
                $('pose-x').textContent = pose.x.toFixed(1);
                $('pose-y').textContent = pose.y.toFixed(1);
                $('pose-z').textContent = pose.z.toFixed(1);
                $('pose-roll').textContent = pose.roll.toFixed(1);
                $('pose-pitch').textContent = pose.pitch.toFixed(1);
                $('pose-yaw').textContent = pose.yaw.toFixed(1);
            }
            const joints = await fetchJson('/api/joints');
            if (!joints.error && joints.joints) {
                for (let i = 0; i < 6; i++) {
                    $('j' + i).textContent = joints.joints[i].toFixed(1);
                }
            }
        }

        async function goHome() {
            $('btn-home').disabled = true;
            log('Sending Home command...');
            const res = await fetchJson('/api/home', { method: 'POST' });
            $('btn-home').disabled = false;
            if (res.ok) {
                log('Home completed (code: ' + res.code + ')');
            } else {
                log('Home failed: ' + (res.detail || res.code), true);
            }
        }

        async function resetState() {
            $('btn-reset').disabled = true;
            log('Resetting robot state...');
            const res = await fetchJson('/api/reset', { method: 'POST' });
            $('btn-reset').disabled = false;
            if (res.ok) {
                log('Reset completed (code: ' + res.code + ')');
            } else {
                log('Reset failed: ' + (res.detail || res.code), true);
            }
        }

        async function moveTo(e) {
            e.preventDefault();
            const x = parseFloat($('move-x').value);
            const y = parseFloat($('move-y').value);
            const z = parseFloat($('move-z').value);
            const speed = parseFloat($('move-speed').value) || 30;

            $('btn-move').disabled = true;
            log('Moving to (' + x + ', ' + y + ', ' + z + ') at ' + speed + ' mm/s...');

            const res = await fetchJson('/api/move_to', {
                method: 'POST',
                headers: { 'Content-Type': 'application/json' },
                body: JSON.stringify({ x, y, z, speed })
            });

            $('btn-move').disabled = false;
            if (res.ok) {
                log('Move completed (code: ' + res.code + ')');
            } else {
                log('Move failed: ' + (res.detail || res.code), true);
            }
        }

        // Vacuum gripper functions
        let vacuumEnabled = false;

        async function initConfig() {
            const config = await fetchJson('/api/config');
            if (config.vacuum_enabled) {
                vacuumEnabled = true;
                $('vacuum-controls').style.display = 'block';
                updateVacuumStatus();
                setInterval(updateVacuumStatus, 1000);
            }
        }

        async function updateVacuumStatus() {
            if (!vacuumEnabled) return;
            const data = await fetchJson('/api/vacuum');
            if (!data.error) {
                const statusEl = $('vacuum-status');
                statusEl.textContent = 'Status: ' + (data.on ? 'ON' : 'OFF');
                statusEl.style.color = data.on ? '#4ade80' : '#888';
            }
        }

        async function vacuumOn() {
            $('btn-vacuum-on').disabled = true;
            log('Turning vacuum ON...');
            const res = await fetchJson('/api/vacuum/on', { method: 'POST' });
            $('btn-vacuum-on').disabled = false;
            if (res.ok) {
                log('Vacuum ON (code: ' + res.code + ')');
                updateVacuumStatus();
            } else {
                log('Vacuum ON failed: ' + (res.detail || res.code), true);
            }
        }

        async function vacuumOff() {
            $('btn-vacuum-off').disabled = true;
            log('Turning vacuum OFF...');
            const res = await fetchJson('/api/vacuum/off', { method: 'POST' });
            $('btn-vacuum-off').disabled = false;
            if (res.ok) {
                log('Vacuum OFF (code: ' + res.code + ')');
                updateVacuumStatus();
            } else {
                log('Vacuum OFF failed: ' + (res.detail || res.code), true);
            }
        }

        // Auto-refresh
        setInterval(updateStatus, 1000);
        setInterval(updatePose, 500);
        updateStatus();
        updatePose();
        initConfig();
    </script>
</body>
</html>"""

    @app.get("/api/status")
    def get_status():
        """Get robot status (connected, enabled, errors)."""
        try:
            return helper.get_status()
        except Exception as e:
            raise HTTPException(status_code=500, detail=str(e))

    @app.get("/api/pose")
    def get_pose():
        """Get current TCP pose [x, y, z, roll, pitch, yaw]."""
        try:
            return helper.get_current_position()
        except Exception as e:
            raise HTTPException(status_code=500, detail=str(e))

    @app.get("/api/joints")
    def get_joints():
        """Get current joint angles in degrees."""
        try:
            angles = helper.get_joint_angles_deg()
            return {"joints": angles.tolist(), "units": "deg"}
        except Exception as e:
            raise HTTPException(status_code=500, detail=str(e))

    @app.post("/api/home")
    def go_home():
        """Send robot to home position."""
        try:
            code = helper.go_home()
            return {"ok": code == 0, "code": code}
        except Exception as e:
            raise HTTPException(status_code=500, detail=str(e))

    @app.post("/api/move_to")
    def move_to(request: MoveToRequest):
        """Move to position with optional orientation."""
        try:
            speed = request.speed if request.speed is not None else default_speed
            code = helper.move_to_pose(
                request.x,
                request.y,
                request.z,
                request.roll,
                request.pitch,
                request.yaw,
                speed,
                default_units,
            )
            return {"ok": code == 0, "code": code}
        except Exception as e:
            raise HTTPException(status_code=500, detail=str(e))

    @app.post("/api/move_to_pose")
    def move_to_pose(request: MoveToPoseRequest):
        """Move to full pose with position and orientation."""
        try:
            speed = request.speed if request.speed is not None else default_speed
            code = helper.move_to_pose(
                request.x,
                request.y,
                request.z,
                request.roll,
                request.pitch,
                request.yaw,
                speed,
                default_units,
            )
            return {"ok": code == 0, "code": code}
        except Exception as e:
            raise HTTPException(status_code=500, detail=str(e))

    @app.post("/api/reset")
    def reset_state():
        """Clear errors/warnings and reset robot state."""
        try:
            code = helper.reset_state()
            return {"ok": code == 0, "code": code}
        except Exception as e:
            raise HTTPException(status_code=500, detail=str(e))

    @app.post("/api/disconnect")
    def disconnect():
        """Disconnect from robot."""
        try:
            code = helper.disconnect()
            return {"ok": code == 0, "code": code}
        except Exception as e:
            raise HTTPException(status_code=500, detail=str(e))

    # Vacuum gripper endpoints (only if enabled)
    if vacuum_enabled:

        @app.get("/api/vacuum")
        def get_vacuum():
            """Get vacuum gripper status."""
            try:
                return helper.get_vacuum_gripper()
            except Exception as e:
                raise HTTPException(status_code=500, detail=str(e))

        @app.post("/api/vacuum/on")
        def vacuum_on():
            """Turn vacuum gripper on."""
            try:
                code = helper.set_vacuum_gripper(True)
                return {"ok": code == 0, "code": code}
            except Exception as e:
                raise HTTPException(status_code=500, detail=str(e))

        @app.post("/api/vacuum/off")
        def vacuum_off():
            """Turn vacuum gripper off."""
            try:
                code = helper.set_vacuum_gripper(False)
                return {"ok": code == 0, "code": code}
            except Exception as e:
                raise HTTPException(status_code=500, detail=str(e))

    @app.get("/api/config")
    def get_config():
        """Get API configuration."""
        return {"vacuum_enabled": vacuum_enabled}

    return app


def run_uvicorn(app: FastAPI, host: str, port: int) -> None:
    """Run uvicorn server (for use in background thread)."""
    config = uvicorn.Config(app, host=host, port=port, log_level="warning")
    server = uvicorn.Server(config)
    server.run()


def _send_joint_pose(node: Node, joints_deg: np.ndarray) -> None:
    metadata = {"units": "deg", "timestamp_ns": time.time_ns()}
    node.send_output("joint_pose", pa.array(joints_deg.tolist()), metadata=metadata)


def _send_tcp_pose(node: Node, tcp_pose: Dict[str, float]) -> None:
    metadata = {
        "units_position": "mm",
        "units_rotation": "deg",
        "timestamp_ns": time.time_ns(),
    }
    vec = [
        tcp_pose["x"],
        tcp_pose["y"],
        tcp_pose["z"],
        tcp_pose["roll"],
        tcp_pose["pitch"],
        tcp_pose["yaw"],
    ]
    node.send_output("tcp_pose", pa.array(vec), metadata=metadata)


def _send_error_status(
    node: Node, action: str, message: str, status: Optional[Dict[str, Any]] = None
) -> None:
    payload = {
        "ok": False,
        "action": action,
        "message": message,
        "timestamp_ns": time.time_ns(),
    }
    if status:
        payload.update(status)
    node.send_output("status", pa.array([json.dumps(payload)]))


def _send_command_status(
    node: Node,
    command_id: str,
    action: str,
    ok: bool,
    message: str,
    code: Optional[int] = None,
    status: Optional[Dict[str, Any]] = None,
) -> None:
    payload = {
        "ok": ok,
        "action": action,
        "message": message,
        "timestamp_ns": time.time_ns(),
        "command_id": command_id,
    }
    if code is not None:
        payload["code"] = code
    if status:
        payload.update(status)
    node.send_output("status", pa.array([json.dumps(payload)]))


def _status_snapshot(helper: ULite6Helper) -> Dict[str, Any]:
    status = helper.get_status()
    return {
        "state": status.get("state"),
        "mode": status.get("mode"),
        "error_code": status.get("error_code"),
        "warn_code": status.get("warn_code"),
        "has_error": status.get("has_error"),
        "has_warn": status.get("has_warn"),
    }


def _log(msg: str) -> None:
    """Print a timestamped log message."""
    timestamp = time.strftime("%H:%M:%S")
    print(f"[ulite6 {timestamp}] {msg}", flush=True)


def main() -> None:
    node = Node()

    robot_ip = os.getenv("ROBOT_IP", "192.168.1.192")
    default_speed = float(os.getenv("DEFAULT_SPEED", "30"))
    default_units = os.getenv("DEFAULT_UNITS", "mm")
    api_host = os.getenv("API_HOST", "0.0.0.0")
    api_port = int(os.getenv("API_PORT", "8080"))
    vacuum_enabled = os.getenv("VACUUM_ENABLED", "false").lower() in ("true", "1", "yes")

    # Initial position settings
    init_mode = os.getenv("INIT_MODE", "none").lower()  # "home", "pose", or "none"
    init_x = float(os.getenv("INIT_X", "300.0"))
    init_y = float(os.getenv("INIT_Y", "0.0"))
    init_z = float(os.getenv("INIT_Z", "250.0"))
    init_roll = float(os.getenv("INIT_ROLL", "180.0"))
    init_pitch = float(os.getenv("INIT_PITCH", "0.0"))
    init_yaw = float(os.getenv("INIT_YAW", "0.0"))
    init_speed = float(os.getenv("INIT_SPEED", "50.0"))

    _log(f"Connecting to robot at {robot_ip}...")
    helper = ULite6Helper(robot_ip)
    _log("Robot connected")

    # Move to initial position on startup
    if init_mode == "home":
        _log("Moving to home position...")
        code = helper.go_home()
        if code == 0:
            _log("Home position reached")
        else:
            _log(f"Home failed with code {code}")
    elif init_mode == "pose":
        _log(f"Moving to initial pose: [{init_x}, {init_y}, {init_z}] roll={init_roll} pitch={init_pitch} yaw={init_yaw}")
        code = helper.move_to_pose(
            init_x, init_y, init_z,
            init_roll, init_pitch, init_yaw,
            speed=init_speed,
            units="mm",
        )
        if code == 0:
            _log("Initial pose reached")
        else:
            _log(f"Move to initial pose failed with code {code}")
    else:
        _log("Skipping initial position (INIT_MODE=none)")

    # Create and start FastAPI server in background thread
    app = create_api(helper, default_speed, default_units, vacuum_enabled)
    api_thread = threading.Thread(
        target=run_uvicorn, args=(app, api_host, api_port), daemon=True
    )
    api_thread.start()
    _log(f"Web interface at http://{api_host}:{api_port}")

    # Dora event loop - only handles tick for state publishing
    for event in node:
        if event["type"] != "INPUT":
            continue

        if event["id"] == "tick":
            try:
                joints = helper.get_joint_angles_deg()
                tcp_pose = helper.get_current_position()
                _send_joint_pose(node, joints)
                _send_tcp_pose(node, tcp_pose)
            except Exception as exc:
                _send_error_status(
                    node, "publish_state", str(exc), status=_status_snapshot(helper)
                )
        elif event["id"] == "command":
            try:
                value = event["value"]
                raw = value[0].as_py() if len(value) else None
                if not raw:
                    _send_command_status(
                        node,
                        command_id="",
                        action="command",
                        ok=False,
                        message="Empty command payload",
                        status=_status_snapshot(helper),
                    )
                    continue
                payload = json.loads(raw)
            except Exception as exc:
                _send_command_status(
                    node,
                    command_id="",
                    action="command",
                    ok=False,
                    message=f"Invalid command payload: {exc}",
                    status=_status_snapshot(helper),
                )
                continue

            command_id = str(payload.get("id", ""))
            action = str(payload.get("action", ""))
            data = payload.get("payload", {}) if isinstance(payload.get("payload", {}), dict) else {}

            if not command_id:
                _send_command_status(
                    node,
                    command_id="",
                    action=action or "command",
                    ok=False,
                    message="Missing command id",
                    status=_status_snapshot(helper),
                )
                continue

            try:
                if action == "home":
                    code = helper.go_home()
                    _send_command_status(
                        node,
                        command_id=command_id,
                        action=action,
                        ok=code == 0,
                        message="Home command executed",
                        code=code,
                        status=_status_snapshot(helper),
                    )
                elif action in ("move_to", "move_to_pose"):
                    speed = float(data.get("speed", default_speed))
                    x = float(data.get("x", 0.0))
                    y = float(data.get("y", 0.0))
                    z = float(data.get("z", 0.0))
                    roll = float(data.get("roll", 180.0))
                    pitch = float(data.get("pitch", 0.0))
                    yaw = float(data.get("yaw", 0.0))
                    code = helper.move_to_pose(
                        x,
                        y,
                        z,
                        roll,
                        pitch,
                        yaw,
                        speed,
                        default_units,
                    )
                    _send_command_status(
                        node,
                        command_id=command_id,
                        action=action,
                        ok=code == 0,
                        message="Move command executed",
                        code=code,
                        status=_status_snapshot(helper),
                    )
                elif action in ("vacuum_on", "vacuum_off"):
                    if not vacuum_enabled:
                        _send_command_status(
                            node,
                            command_id=command_id,
                            action=action,
                            ok=False,
                            message="Vacuum gripper not enabled",
                            status=_status_snapshot(helper),
                        )
                        continue
                    vacuum_on = action == "vacuum_on"
                    code = helper.set_vacuum_gripper(vacuum_on)
                    _send_command_status(
                        node,
                        command_id=command_id,
                        action=action,
                        ok=code == 0,
                        message="Vacuum command executed",
                        code=code,
                        status=_status_snapshot(helper),
                    )
                else:
                    _send_command_status(
                        node,
                        command_id=command_id,
                        action=action or "command",
                        ok=False,
                        message=f"Unknown action: {action}",
                        status=_status_snapshot(helper),
                    )
            except Exception as exc:
                _send_command_status(
                    node,
                    command_id=command_id,
                    action=action or "command",
                    ok=False,
                    message=str(exc),
                    status=_status_snapshot(helper),
                )


if __name__ == "__main__":
    main()
