"""
High-level Twin abstraction for intuitive digital twin control
"""

import asyncio
import json
import math
from pathlib import Path
from typing import TYPE_CHECKING, Optional, Dict, Any, List, Callable, Type

if TYPE_CHECKING:
    from .client import Cyberwave
    from .camera import CameraStreamer
    from .motion import TwinMotionHandle, TwinNavigationHandle
    from .keyboard import KeyboardTeleop
    from .alerts import TwinAlertManager
    from cyberwave.rest.models.twin_joint_calibration_schema import (
        TwinJointCalibrationSchema,
    )

from .exceptions import CyberwaveError


# Load capabilities cache for runtime class selection
_CAPABILITIES_CACHE: Optional[Dict[str, Any]] = None


def _load_capabilities_cache() -> Dict[str, Any]:
    """Load the capabilities cache from JSON file."""
    global _CAPABILITIES_CACHE
    if _CAPABILITIES_CACHE is None:
        cache_path = Path(__file__).parent / "assets_capabilities.json"
        if cache_path.exists():
            with open(cache_path, "r") as f:
                _CAPABILITIES_CACHE = json.load(f)
        else:
            _CAPABILITIES_CACHE = {}
    return _CAPABILITIES_CACHE


def _get_asset_capabilities(registry_id: str) -> Dict[str, Any]:
    """Get capabilities for an asset by registry_id."""
    cache = _load_capabilities_cache()
    asset_data = cache.get(registry_id, {})
    return asset_data.get("capabilities", {})


class JointController:
    """Controller for robot joints"""

    def __init__(self, twin: "Twin"):
        self.twin = twin
        self._joint_states: Optional[Dict[str, float]] = None

    def refresh(self):
        """Refresh joint states from the server"""
        try:
            states = self.twin.client.twins.get_joint_states(self.twin.uuid)
            if hasattr(states, "joint_states"):
                self._joint_states = {
                    js.joint_name: js.position for js in states.joint_states
                }
            else:
                self._joint_states = {}
        except Exception as e:
            raise CyberwaveError(f"Failed to refresh joint states: {e}")

    def get(self, joint_name: str) -> float:
        """Get current position of a joint"""
        if self._joint_states is None:
            self.refresh()

        # After refresh, _joint_states should be a dict
        if self._joint_states is None or joint_name not in self._joint_states:
            raise CyberwaveError(f"Joint '{joint_name}' not found")

        return self._joint_states[joint_name]

    def set(
        self,
        joint_name: str,
        position: float,
        degrees: bool = True,
        timestamp: Optional[float] = None,
        source_type: Optional[str] = None,
    ):
        """
        Set position of a joint

        Args:
            joint_name: Name of the joint
            position: Target position
            degrees: If True, position is in degrees; otherwise radians
            timestamp: Unix timestamp for the update
            source_type: Source type (e.g. SOURCE_TYPE_EDGE_LEADER, SOURCE_TYPE_EDGE_FOLLOWER)
        """
        if degrees:
            position = math.radians(position)

        try:
            # Connect to MQTT if not already connected
            self.twin._connect_to_mqtt_if_not_connected()

            # Update joint state via MQTT
            self.twin.client.mqtt.update_joint_state(
                self.twin.uuid,
                joint_name,
                position=position,
                timestamp=timestamp,
                source_type=source_type,
            )

            # Update cached state
            if self._joint_states is None:
                self._joint_states = {}
            self._joint_states[joint_name] = position

        except Exception as e:
            raise CyberwaveError(f"Failed to set joint '{joint_name}': {e}")

    def __getattr__(self, name: str) -> float:
        """Allow accessing joints as attributes (e.g., joints.arm_joint)"""
        if name.startswith("_"):
            raise AttributeError(
                f"'{type(self).__name__}' object has no attribute '{name}'"
            )
        return self.get(name)

    def __setattr__(self, name: str, value: float):
        """Allow setting joints as attributes (e.g., joints.arm_joint = 45)"""
        if name in ["twin", "_joint_states"]:
            super().__setattr__(name, value)
        else:
            self.set(name, value)

    def list(self) -> List[str]:
        """Get list of all joint names"""
        if self._joint_states is None:
            self.refresh()
        if self._joint_states is None:
            return []
        return list(self._joint_states.keys())

    def get_all(self) -> Dict[str, float]:
        """Get all joint states as a dictionary"""
        if self._joint_states is None:
            self.refresh()
        if self._joint_states is None:
            return {}
        return self._joint_states.copy()


class TwinControllerHandle:
    """Handle for controller functionality like keyboard teleop."""

    def __init__(self, twin: "Twin"):
        self._twin = twin

    def keyboard(
        self,
        bindings: Any,
        *,
        step: float = 0.05,
        rate_hz: int = 20,
        fetch_initial: bool = True,
        verbose: bool = True,
    ) -> "KeyboardTeleop":
        """
        Create a keyboard teleop controller.

        Args:
            bindings: KeyboardBindings instance or list of binding dicts
            step: Position change per keypress (degrees)
            rate_hz: Polling rate in Hz
            fetch_initial: Whether to fetch initial joint positions
            verbose: Whether to print status messages

        Returns:
            KeyboardTeleop instance ready to run
        """
        from .keyboard import KeyboardBindings, KeyboardTeleop

        payload = (
            bindings.build() if isinstance(bindings, KeyboardBindings) else bindings
        )
        return KeyboardTeleop(
            self._twin,
            payload,
            step=step,
            rate_hz=rate_hz,
            fetch_initial=fetch_initial,
            verbose=verbose,
        )


class Twin:
    """
    High-level abstraction for a digital twin.

    Provides intuitive methods for controlling position, rotation, scale,
    and joint states of a digital twin.

    Example:
        >>> twin = client.twin("the-robot-studio/so101")
        >>> twin.edit_position(x=1, y=0, z=0.5)
        >>> twin.rotate(yaw=90)
        >>> twin.joints.arm_joint = 45
    """

    def __init__(self, client: "Cyberwave", twin_data: Any):
        """
        Initialize a Twin instance

        Args:
            client: Cyberwave client instance
            twin_data: Twin schema data from API
        """
        self.client = client
        self._data = twin_data
        self.joints = JointController(self)

        # Cache for current state
        self._position: Optional[Dict[str, float]] = None
        self._rotation: Optional[Dict[str, float]] = None

        # Lazy-initialized motion, navigation, and alerts handles
        self._motion: Optional["TwinMotionHandle"] = None
        self._navigation: Optional["TwinNavigationHandle"] = None
        self._alerts: Optional["TwinAlertManager"] = None
        self._scale: Optional[Dict[str, float]] = None

    @property
    def uuid(self) -> str:
        """Get twin UUID"""
        return (
            self._data.uuid
            if hasattr(self._data, "uuid")
            else str(self._data.get("uuid", ""))
        )

    @property
    def name(self) -> str:
        """Get twin name"""
        return (
            self._data.name
            if hasattr(self._data, "name")
            else str(self._data.get("name", ""))
        )

    @property
    def asset_id(self) -> str:
        """Get asset ID"""
        return (
            self._data.asset_uuid
            if hasattr(self._data, "asset_uuid")
            else str(self._data.get("asset_uuid", ""))
        )

    @property
    def environment_id(self) -> str:
        """Get environment ID"""
        return (
            self._data.environment_uuid
            if hasattr(self._data, "environment_uuid")
            else str(self._data.get("environment_uuid", ""))
        )

    @property
    def motion(self) -> "TwinMotionHandle":
        """
        Access motion control for poses and animations.

        Example:
            >>> twin.motion.asset.pose("Picking from below", transition_ms=800)
            >>> twin.motion.twin.animation("wave", transition_ms=500)
            >>> keyframes = twin.motion.asset.list_keyframes()

        Returns:
            TwinMotionHandle for motion control
        """
        if self._motion is None:
            from .motion import TwinMotionHandle

            self._motion = TwinMotionHandle(self)
        return self._motion

    @property
    def navigation(self) -> "TwinNavigationHandle":
        """
        Access navigation control for waypoint-based movement.

        Example:
            >>> twin.navigation.goto([1, 2, 0])
            >>> twin.navigation.follow_path([[0, 0, 0], [1, 0, 0], [1, 1, 0]])
            >>> twin.navigation.stop()

        Returns:
            TwinNavigationHandle for navigation control
        """
        if self._navigation is None:
            from .motion import TwinNavigationHandle

            self._navigation = TwinNavigationHandle(self)
        return self._navigation

    @property
    def alerts(self) -> "TwinAlertManager":
        """
        Access alert management for this twin.

        Example:
            >>> alert = twin.alerts.create(name="Calibration needed")
            >>> for a in twin.alerts.list():
            ...     print(a.name, a.status)
            >>> alert.resolve()

        Returns:
            TwinAlertManager for creating / listing / managing alerts
        """
        if self._alerts is None:
            from .alerts import TwinAlertManager

            self._alerts = TwinAlertManager(self)
        return self._alerts

    @property
    def controller(self) -> "TwinControllerHandle":
        """
        Access controller functionality for keyboard teleop.

        Example:
            >>> from cyberwave import KeyboardBindings
            >>> bindings = KeyboardBindings().bind("W", "joint1", "increase")
            >>> teleop = twin.controller.keyboard(bindings, step=2.0)
            >>> teleop.run()

        Returns:
            TwinControllerHandle for controller access
        """
        return TwinControllerHandle(self)

    def refresh(self):
        """Refresh twin data from the server"""
        try:
            self._data = self.client.twins.get_raw(self.uuid)
            self._position = None
            self._rotation = None
            self._scale = None
        except Exception as e:
            raise CyberwaveError(f"Failed to refresh twin: {e}")

    def edit_position(
        self,
        x: Optional[float] = None,
        y: Optional[float] = None,
        z: Optional[float] = None,
    ):
        """
        Edit the twin's position in the environment.

        NOTE: Does not move the twin in the real world.

        Args:
            x: X coordinate (optional, keeps current if None)
            y: Y coordinate (optional, keeps current if None)
            z: Z coordinate (optional, keeps current if None)
        """
        # Get current position if needed
        current = self._get_current_position()

        update_data = {
            "position_x": x if x is not None else current.get("x", 0),
            "position_y": y if y is not None else current.get("y", 0),
            "position_z": z if z is not None else current.get("z", 0),
        }

        self._update_state(update_data)

        # Update cache
        self._position = {
            "x": update_data["position_x"],
            "y": update_data["position_y"],
            "z": update_data["position_z"],
        }

    def edit_rotation(
        self,
        yaw: Optional[float] = None,
        pitch: Optional[float] = None,
        roll: Optional[float] = None,
        quaternion: Optional[List[float]] = None,
    ):
        """
        Edit the twin's rotation in the environment.
        NOTE: Does not rotate the twin in the real world.

        Args:
            yaw: Yaw angle in degrees (rotation around Z axis)
            pitch: Pitch angle in degrees (rotation around Y axis)
            roll: Roll angle in degrees (rotation around X axis)
            quaternion: Quaternion [x, y, z, w] (alternative to euler angles)
        """
        if quaternion is not None:
            if len(quaternion) != 4:
                raise CyberwaveError("Quaternion must be [x, y, z, w]")

            update_data = {
                "rotation_x": quaternion[0],
                "rotation_y": quaternion[1],
                "rotation_z": quaternion[2],
                "rotation_w": quaternion[3],
            }
        else:
            # Convert euler angles to quaternion
            quat = self._euler_to_quaternion(roll or 0, pitch or 0, yaw or 0)
            update_data = {
                "rotation_x": quat[0],
                "rotation_y": quat[1],
                "rotation_z": quat[2],
                "rotation_w": quat[3],
            }

        self._update_state(update_data)

        # Update cache
        self._rotation = {
            "x": update_data["rotation_x"],
            "y": update_data["rotation_y"],
            "z": update_data["rotation_z"],
            "w": update_data["rotation_w"],
        }

    def edit_scale(
        self,
        x: Optional[float] = None,
        y: Optional[float] = None,
        z: Optional[float] = None,
    ):
        """
        Edit the twin's scale in the environment.
        NOTE: Does not scale the twin in the real world (nothing can be scaled in the real world).

        Args:
            x: X scale factor
            y: Y scale factor
            z: Z scale factor
        """
        current = self._get_current_scale()

        update_data = {
            "scale_x": x if x is not None else current.get("x", 1),
            "scale_y": y if y is not None else current.get("y", 1),
            "scale_z": z if z is not None else current.get("z", 1),
        }

        self._update_state(update_data)

        # Update cache
        self._scale = {
            "x": update_data["scale_x"],
            "y": update_data["scale_y"],
            "z": update_data["scale_z"],
        }

    def delete(self) -> None:
        """Delete this twin"""
        try:
            self.client.twins.delete(self.uuid)  # type: ignore
        except Exception as e:
            raise CyberwaveError(f"Failed to delete twin: {e}")

    def _update_state(self, data: Dict[str, Any]):
        """Update twin state via API"""
        try:
            self.client.twins.update_state(self.uuid, data)  # type: ignore
        except Exception as e:
            raise CyberwaveError(f"Failed to update twin state: {e}")

    def _get_current_position(self) -> Dict[str, float]:
        """Get current position from cache or server"""
        if self._position is None:
            # First try to use existing data without making an API call
            if hasattr(self._data, "position_x"):
                self._position = {
                    "x": self._data.position_x,
                    "y": self._data.position_y,
                    "z": self._data.position_z,
                }
            elif isinstance(self._data, dict) and "position_x" in self._data:
                self._position = {
                    "x": self._data.get("position_x", 0),
                    "y": self._data.get("position_y", 0),
                    "z": self._data.get("position_z", 0),
                }
            else:
                # Only refresh from server if we don't have the data
                self.refresh()
                if hasattr(self._data, "position_x"):
                    self._position = {
                        "x": self._data.position_x,
                        "y": self._data.position_y,
                        "z": self._data.position_z,
                    }
                else:
                    self._position = {"x": 0, "y": 0, "z": 0}
        return self._position

    def _get_current_scale(self) -> Dict[str, float]:
        """Get current scale from cache or server"""
        if self._scale is None:
            # First try to use existing data without making an API call
            if hasattr(self._data, "scale_x"):
                self._scale = {
                    "x": self._data.scale_x,
                    "y": self._data.scale_y,
                    "z": self._data.scale_z,
                }
            elif isinstance(self._data, dict) and "scale_x" in self._data:
                self._scale = {
                    "x": self._data.get("scale_x", 1),
                    "y": self._data.get("scale_y", 1),
                    "z": self._data.get("scale_z", 1),
                }
            else:
                # Only refresh from server if we don't have the data
                self.refresh()
                if hasattr(self._data, "scale_x"):
                    self._scale = {
                        "x": self._data.scale_x,
                        "y": self._data.scale_y,
                        "z": self._data.scale_z,
                    }
                else:
                    self._scale = {"x": 1, "y": 1, "z": 1}
        return self._scale

    def _get_current_rotation(self) -> Dict[str, float]:
        """Get current rotation from cache or server"""
        if self._rotation is None:
            # First try to use existing data without making an API call
            if hasattr(self._data, "rotation_w"):
                self._rotation = {
                    "w": self._data.rotation_w,
                    "x": self._data.rotation_x,
                    "y": self._data.rotation_y,
                    "z": self._data.rotation_z,
                }
            elif isinstance(self._data, dict) and "rotation_w" in self._data:
                self._rotation = {
                    "w": self._data.get("rotation_w", 1.0),
                    "x": self._data.get("rotation_x", 0.0),
                    "y": self._data.get("rotation_y", 0.0),
                    "z": self._data.get("rotation_z", 0.0),
                }
            else:
                # Only refresh from server if we don't have the data
                self.refresh()
                if hasattr(self._data, "rotation_w"):
                    self._rotation = {
                        "w": self._data.rotation_w,
                        "x": self._data.rotation_x,
                        "y": self._data.rotation_y,
                        "z": self._data.rotation_z,
                    }
                else:
                    self._rotation = {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}
        return self._rotation

    @staticmethod
    def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> List[float]:
        """
        Convert euler angles (degrees) to quaternion

        Args:
            roll: Roll angle in degrees
            pitch: Pitch angle in degrees
            yaw: Yaw angle in degrees

        Returns:
            [x, y, z, w] quaternion
        """
        # Convert to radians
        roll = math.radians(roll)
        pitch = math.radians(pitch)
        yaw = math.radians(yaw)

        # Calculate quaternion
        cy = math.cos(yaw * 0.5)
        sy = math.sin(yaw * 0.5)
        cp = math.cos(pitch * 0.5)
        sp = math.sin(pitch * 0.5)
        cr = math.cos(roll * 0.5)
        sr = math.sin(roll * 0.5)

        w = cr * cp * cy + sr * sp * sy
        x = sr * cp * cy - cr * sp * sy
        y = cr * sp * cy + sr * cp * sy
        z = cr * cp * sy - sr * sp * cy

        return [x, y, z, w]

    def __repr__(self) -> str:
        return f"Twin(uuid='{self.uuid}', name='{self.name}')"

    def _connect_to_mqtt_if_not_connected(self):
        """Connect to MQTT if not connected"""
        if not self.client.mqtt.connected:
            self.client.mqtt.connect()

    def subscribe(self, on_update: Callable[[Dict[str, Any]], None]):
        """Subscribe to real-time updates"""
        self._connect_to_mqtt_if_not_connected()
        self.client.mqtt.subscribe_twin(self.uuid, on_update)

    def subscribe_position(self, on_update: Callable[[Dict[str, Any]], None]):
        """Subscribe to movement updates"""
        self._connect_to_mqtt_if_not_connected()
        self.client.mqtt.subscribe_twin_position(self.uuid, on_update)

    def subscribe_rotation(self, on_update: Callable[[Dict[str, Any]], None]):
        """Subscribe to rotation updates"""
        self._connect_to_mqtt_if_not_connected()
        self.client.mqtt.subscribe_twin_rotation(self.uuid, on_update)

    def subscribe_joints(self, on_update: Callable[[Dict[str, Any]], None]):
        """Subscribe to joint updates"""
        self._connect_to_mqtt_if_not_connected()
        self.client.mqtt.subscribe_joint_states(self.uuid, on_update)

    @property
    def capabilities(self) -> Dict[str, Any]:
        """Get twin capabilities from the underlying data."""
        if hasattr(self._data, "capabilities"):
            return self._data.capabilities or {}
        elif isinstance(self._data, dict):
            return self._data.get("capabilities", {})
        return {}

    def has_capability(self, capability: str) -> bool:
        """Check if the twin has a specific capability."""
        return bool(self.capabilities.get(capability, False))

    def has_sensor(self, sensor_type: Optional[str] = None) -> bool:
        """Check if the twin has sensors, optionally of a specific type."""
        sensors = self.capabilities.get("sensors", [])
        if not sensors:
            return False
        if sensor_type is None:
            return True
        return any(s.get("type") == sensor_type for s in sensors)

    # =========================================================================
    # Universal Schema APIs
    # =========================================================================

    def get_controllable_joint_names(self) -> List[str]:
        """
        Get controllable joint names from the twin's universal schema.

        Returns joint names for revolute, prismatic, and continuous joints,
        sorted by name for consistent ordering (e.g. _1, _2, _3 for SO101).

        Matches the logic used by the backend (get_controllable_joints) and
        recording tasks. Use these names for joint updates and initial observations.

        Returns:
            List of joint names (e.g. ["_1", "_2", "_3", "_4", "_5", "_6"] for SO101)

        Example:
            >>> joint_names = twin.get_controllable_joint_names()
            >>> twin.joints.set(joint_names[0], 0.5, degrees=False)
        """
        CONTROLLABLE_JOINT_TYPES = frozenset({"revolute", "prismatic", "continuous"})
        schema = self.get_schema()
        if not schema:
            return []
        joints = schema.get("joints", [])
        controllable = [
            j["name"]
            for j in joints
            if isinstance(j, dict)
            and j.get("name")
            and j.get("type") in CONTROLLABLE_JOINT_TYPES
        ]
        return sorted(controllable)

    def get_schema(self, path: str = "") -> Any:
        """Get value at a specific JSON Pointer path in the twin's universal schema.

        Args:
            path: JSON Pointer path (e.g., "/sensors/0", "/extensions/cyberwave/capabilities")
                 Empty string returns the entire schema

        Returns:
            The value at the specified path (can be dict, list, string, etc.)

        Example:
            # Get entire schema
            schema = twin.get_schema()

            # Get specific path
            sensor = twin.get_schema("/sensors/0")

            # Get capabilities
            capabilities = twin.get_schema("/extensions/cyberwave/capabilities")
        """
        result = self.client.twins.get_universal_schema_at_path(self.uuid, path)
        return result.get("value")

    def update_schema(
        self, path: str, value: Any, op: str = "replace"
    ) -> Dict[str, Any]:
        """Update the twin's universal schema using JSON Pointer operations.

        Args:
            path: JSON Pointer path to update (e.g., "/sensors/0/parameters/id")
            value: Value to set at the path
            op: Operation type - "add" or "replace" (default: "replace")

        Returns:
            Dict with the updated schema and operation details

        Example:
            # Update a sensor ID
            twin.update_schema(
                path="/sensors/0/parameters/id",
                value="my_camera"
            )

            # Add a new capability
            twin.update_schema(
                path="/extensions/cyberwave/capabilities/can_fly",
                value=True,
                op="add"
            )
        """
        return self.client.twins.patch_universal_schema(self.uuid, path, value, op)

    def get_calibration(
        self, robot_type: Optional[str] = None
    ) -> "TwinJointCalibrationSchema":
        """
        Get calibration data for this twin.

        Args:
            robot_type: Optional robot type filter ("leader" or "follower").
                       If None, returns all calibration data.

        Returns:
            TwinJointCalibrationSchema containing calibration data

        Example:
            >>> calibration = twin.get_calibration(robot_type="leader")
            >>> print(calibration.joint_calibration["shoulder_pan"].range_min)
        """
        return self.client.twins.get_calibration(self.uuid, robot_type=robot_type)

    def update_calibration(
        self,
        joint_calibration: Dict[str, Dict[str, Any]],
        robot_type: str,
    ) -> "TwinJointCalibrationSchema":
        """
        Update calibration data for this twin.

        Args:
            joint_calibration: Dictionary mapping joint names to calibration data.
                             Each calibration dict should contain:
                             - range_min: float
                             - range_max: float
                             - homing_offset: float
                             - drive_mode: int or str
                             - id: int or str (motor ID)
            robot_type: Robot type ("leader" or "follower")

        Returns:
            Updated TwinJointCalibrationSchema

        Example:
            >>> calibration = {
            ...     "shoulder_pan": {
            ...         "range_min": 0.0,
            ...         "range_max": 4095.0,
            ...         "homing_offset": 2047.5,
            ...         "drive_mode": 0,
            ...         "id": 1
            ...     },
            ... }
            >>> result = twin.update_calibration(calibration, "leader")
            >>> print(result.joint_calibration["shoulder_pan"].range_min)
        """
        return self.client.twins.update_calibration(
            self.uuid, joint_calibration, robot_type
        )


class CameraTwin(Twin):
    """
    Twin with camera/sensor capabilities.

    Provides methods for video streaming and frame capture for twins
    that have RGB or depth sensors.

    Example:
        >>> twin = client.twin("unitree/go2")  # Returns CameraTwin if has sensors
        >>> await twin.stream_video_background(fps=15)
        >>> frame = twin.capture_frame()
    """

    _camera_streamer: Optional["CameraStreamer"] = None

    def streamer(self) -> "CameraStreamer":
        """Get the camera streamer."""
        if self._camera_streamer is None:
            raise CyberwaveError("Camera streamer not initialized")
        return self._camera_streamer

    async def stream_video_background(
        self, fps: int = 30, camera_id: int | str = 0, fourcc: Optional[str] = None
    ) -> "CameraStreamer":
        """
        Start video streaming in the background. Non-blocking.

        Returns immediately with the streamer so you can run other code.
        Use stream_video_background() for simple blocking scripts.

        Args:
            fps: Frames per second (default: 30)
            camera_id: Camera device ID or stream URL (default: 0)
            fourcc: Optional FOURCC code for V4L2/USB cameras (e.g. ``'MJPG'``).
                Forces the pixel format before resolution is negotiated.

        Returns:
            CameraStreamer instance for managing the stream
        """
        self._camera_streamer = self.client.video_stream(
            twin_uuid=self.uuid,
            camera_id=camera_id,
            fps=fps,
            fourcc=fourcc,
        )
        await self._camera_streamer.start()
        return self._camera_streamer

    async def stop_streaming(self) -> None:
        """Stop camera streaming."""
        if self._camera_streamer is not None:
            await self._camera_streamer.stop()
            self._camera_streamer = None

    def start_streaming(self, fps: int = 30, camera_id: int | str = 0) -> None:
        """Stream video until Ctrl+C. Blocking.

        Starts video streaming and blocks until KeyboardInterrupt (Ctrl+C).
        Ideal for 2-line scripts: twin = cw.twin(...); twin.start_streaming()

        Args:
            fps: Frames per second (default: 30)
            camera_id: Camera device ID or stream URL (default: 0)
        """
        self._camera_streamer = self.client.video_stream(
            twin_uuid=self.uuid,
            camera_id=camera_id,
            fps=fps,
        )

        async def _run():
            await self._camera_streamer.start()
            try:
                while True:
                    await asyncio.sleep(1)
            except asyncio.CancelledError:
                pass
            finally:
                await self._camera_streamer.stop()
                self._camera_streamer = None

        try:
            asyncio.run(_run())
        except KeyboardInterrupt:
            pass
        finally:
            if self._camera_streamer is not None:
                try:
                    asyncio.run(self._camera_streamer.stop())
                except Exception:
                    pass
                self._camera_streamer = None

    def capture_frame(self) -> bytes:
        """
        Capture a single frame from the RGB camera.

        Note: Requires an active stream or will start one temporarily.

        Returns:
            Raw frame bytes
        """
        raise NotImplementedError(
            "capture_frame() requires an active stream. "
            "Use stream_video_background() first to begin capturing frames."
        )

    def __repr__(self) -> str:
        sensors = self.capabilities.get("sensors", [])
        sensor_types = [s.get("type", "unknown") for s in sensors]
        return f"CameraTwin(uuid='{self.uuid}', name='{self.name}', sensors={sensor_types})"


class DepthCameraTwin(CameraTwin):
    """
    Twin with depth camera capabilities.

    Extends CameraTwin with depth-specific methods for point cloud
    generation and depth frame capture.
    """

    _camera_streamer: Optional["CameraStreamer"] = None

    def streamer(self) -> "CameraStreamer":
        """Get the camera streamer."""
        if self._camera_streamer is None:
            raise CyberwaveError("Camera streamer not initialized")
        return self._camera_streamer

    async def stop_streaming(self) -> None:
        """Stop camera streaming."""
        if self._camera_streamer is not None:
            # The streamer handles cleanup in its stop method
            await self._camera_streamer.stop()
            self._camera_streamer = None

    async def stream_video_background(
        self,
        fps: int = 10,
        camera_id: int | str = 0,
        fourcc: Optional[str] = None,
        *,
        enable_depth: bool = True,
    ) -> "CameraStreamer":
        """
        Start video streaming in the background. Non-blocking.

        Returns immediately with the streamer so you can run other code.
        Use start_streaming() for simple blocking scripts.

        Args:
            fps: Frames per second (default: 10)
            camera_id: Camera device ID (default: 0)
            fourcc: Optional FOURCC code (inherited from CameraTwin, unused for RealSense)
            enable_depth: Enable depth streaming (default: True for DepthCameraTwin)

        Returns:
            CameraStreamer instance for managing the stream
        """
        self._camera_streamer = self.client.video_stream(
            twin_uuid=self.uuid,
            camera_type="realsense",
            camera_id=camera_id,
            fps=fps,
            enable_depth=enable_depth,
        )
        await self._camera_streamer.start()
        return self._camera_streamer

    def start_streaming(
        self, fps: int = 10, camera_id: int | str = 0, *, enable_depth: bool = True
    ) -> None:
        """Stream video until Ctrl+C. Blocking.

        Starts video streaming and blocks until KeyboardInterrupt (Ctrl+C).
        Ideal for 2-line scripts: twin = cw.twin(...); twin.start_streaming()

        Args:
            fps: Frames per second (default: 10)
            camera_id: Camera device ID (default: 0)
            enable_depth: Enable depth streaming (default: True for DepthCameraTwin)
        """
        self._camera_streamer = self.client.video_stream(
            twin_uuid=self.uuid,
            camera_type="realsense",
            camera_id=camera_id,
            fps=fps,
            enable_depth=enable_depth,
        )

        async def _run():
            await self._camera_streamer.start()
            try:
                while True:
                    await asyncio.sleep(1)
            except asyncio.CancelledError:
                pass
            finally:
                await self._camera_streamer.stop()
                self._camera_streamer = None

        try:
            asyncio.run(_run())
        except KeyboardInterrupt:
            pass
        finally:
            if self._camera_streamer is not None:
                try:
                    asyncio.run(self._camera_streamer.stop())
                except Exception:
                    pass
                self._camera_streamer = None

    def capture_depth_frame(self) -> bytes:
        """
        Capture a single depth frame.

        Returns:
            Raw depth frame bytes
        """
        raise NotImplementedError(
            "capture_depth_frame() requires an active depth stream. "
            "Use stream_video_background() first."
        )

    def get_point_cloud(self) -> List[tuple]:
        """
        Get point cloud from depth sensor.

        Returns:
            List of (x, y, z) tuples representing 3D points
        """
        raise NotImplementedError(
            "get_point_cloud() requires depth sensor data processing. "
            "This feature is not yet implemented."
        )

    def __repr__(self) -> str:
        return f"DepthCameraTwin(uuid='{self.uuid}', name='{self.name}')"


class LocomoteTwin(Twin):
    """
    Twin that can locomote across space.

    Provides methods for locomotion including movement and rotation.

    Note: Flying twins can locomoate AND fly, so a flying twin is a subset of the LocomoteTwin
    """

    def move(self, position: List[float]):
        """
        Move the digital twin to a specific position in the environment.

        NOTE: This updates the digital twin's position in the simulator/viewer,
        not the physical robot. To command the physical robot to navigate,
        use twin.navigation.goto() instead.

        Args:
            position: [x, y, z] coordinates
        """
        if len(position) != 3:
            raise CyberwaveError("Position must be [x, y, z]")

        self._connect_to_mqtt_if_not_connected()
        self.client.mqtt.update_twin_position(
            self.uuid, {"x": position[0], "y": position[1], "z": position[2]}
        )

    def move_forward(self, distance: float):
        """
        Move the digital twin forward in the direction it is facing.

        NOTE: This updates the digital twin's position in the simulator/viewer,
        not the physical robot. To command the physical robot to navigate,
        use twin.navigation.goto() instead.

        Args:
            distance: Distance to move in meters
        """
        # Get current position and rotation
        current_pos = self._get_current_position()
        current_rot = self._get_current_rotation()

        # Extract quaternion components
        w = current_rot["w"]
        x = current_rot["x"]
        y = current_rot["y"]
        z = current_rot["z"]

        # Transform the forward vector [1, 0, 0] by the quaternion
        # Using quaternion rotation formula: v' = q * v * q^(-1)
        # For forward vector [1, 0, 0], this gives us:
        forward_x = 1 - 2 * (y * y + z * z)
        forward_y = 2 * (x * y + w * z)
        forward_z = 2 * (x * z - w * y)

        # Calculate new position
        new_x = current_pos["x"] + forward_x * distance
        new_y = current_pos["y"] + forward_y * distance
        new_z = current_pos["z"] + forward_z * distance

        # Update position via MQTT
        self._connect_to_mqtt_if_not_connected()
        self.client.mqtt.update_twin_position(
            self.uuid, {"x": new_x, "y": new_y, "z": new_z}
        )

    def rotate(
        self,
        *,
        w: Optional[float] = None,
        x: Optional[float] = None,
        y: Optional[float] = None,
        z: Optional[float] = None,
        yaw: Optional[float] = None,
        pitch: Optional[float] = None,
        roll: Optional[float] = None,
    ) -> None:
        """
        Rotate the digital twin using either a quaternion or Euler angles.

        NOTE: This updates the digital twin's rotation in the simulator/viewer,
        not the physical robot. To command the physical robot to rotate,
        use twin.navigation.goto() with a yaw parameter instead.

        You can provide either:
        - Quaternion values (w, x, y, z)
        - Euler angles in degrees (yaw, pitch, roll)

        Mixing quaternion and Euler angle parameters will raise a ValueError.

        Args:
            w: Quaternion w component
            x: Quaternion x component
            y: Quaternion y component
            z: Quaternion z component
            yaw: Rotation around vertical axis in degrees
            pitch: Rotation around lateral axis in degrees
            roll: Rotation around longitudinal axis in degrees
        """
        # Check if quaternion values are provided
        quaternion_provided = any(v is not None for v in [w, x, y, z])
        euler_provided = any(v is not None for v in [yaw, pitch, roll])

        if quaternion_provided and euler_provided:
            raise ValueError(
                "Cannot mix quaternion (w, x, y, z) and Euler angle (yaw, pitch, roll) "
                "parameters. Use one format or the other."
            )

        if quaternion_provided:
            # Use quaternion directly, defaulting missing values
            rotation = {
                "w": w if w is not None else 1.0,
                "x": x if x is not None else 0.0,
                "y": y if y is not None else 0.0,
                "z": z if z is not None else 0.0,
            }
        elif euler_provided:
            # Convert Euler angles to quaternion
            # Default to 0 for any missing angle
            yaw_rad = math.radians(yaw if yaw is not None else 0.0)
            pitch_rad = math.radians(pitch if pitch is not None else 0.0)
            roll_rad = math.radians(roll if roll is not None else 0.0)

            # Euler to quaternion conversion (ZYX order)
            cy = math.cos(yaw_rad * 0.5)
            sy = math.sin(yaw_rad * 0.5)
            cp = math.cos(pitch_rad * 0.5)
            sp = math.sin(pitch_rad * 0.5)
            cr = math.cos(roll_rad * 0.5)
            sr = math.sin(roll_rad * 0.5)

            rotation = {
                "w": cr * cp * cy + sr * sp * sy,
                "x": sr * cp * cy - cr * sp * sy,
                "y": cr * sp * cy + sr * cp * sy,
                "z": cr * cp * sy - sr * sp * cy,
            }
        else:
            # Default to identity quaternion
            rotation = {"w": 1.0, "x": 0.0, "y": 0.0, "z": 0.0}

        self._connect_to_mqtt_if_not_connected()
        self.client.mqtt.update_twin_rotation(self.uuid, rotation)


class FlyingTwin(Twin):
    """
    Twin with flight capabilities (drones, UAVs).

    Provides methods for aerial control including takeoff, landing,
    and hovering.
    """

    def takeoff(self, altitude: float = 1.0) -> None:
        """
        Take off to the specified altitude.

        Args:
            altitude: Target altitude in meters (default: 1.0)
        """
        self._connect_to_mqtt_if_not_connected()
        # Send takeoff command via MQTT
        self.client.mqtt.publish(
            f"twins/{self.uuid}/commands/takeoff", {"altitude": altitude}
        )

    def land(self) -> None:
        """Land the drone."""
        self._connect_to_mqtt_if_not_connected()
        self.client.mqtt.publish(f"twins/{self.uuid}/commands/land", {})

    def hover(self) -> None:
        """Hover in place."""
        self._connect_to_mqtt_if_not_connected()
        self.client.mqtt.publish(f"twins/{self.uuid}/commands/hover", {})

    def __repr__(self) -> str:
        return f"FlyingTwin(uuid='{self.uuid}', name='{self.name}')"


class GripperTwin(Twin):
    """
    Twin with gripper/manipulation capabilities.

    Provides methods for controlling grippers and end effectors.
    """

    def grip(self, force: float = 1.0) -> None:
        """
        Close the gripper with specified force.

        Args:
            force: Grip force (0.0 to 1.0, default: 1.0)
        """
        self._connect_to_mqtt_if_not_connected()
        self.client.mqtt.publish(
            f"twins/{self.uuid}/commands/grip", {"force": max(0.0, min(1.0, force))}
        )

    def release(self) -> None:
        """Open the gripper."""
        self._connect_to_mqtt_if_not_connected()
        self.client.mqtt.publish(f"twins/{self.uuid}/commands/release", {})

    def __repr__(self) -> str:
        return f"GripperTwin(uuid='{self.uuid}', name='{self.name}')"


class FlyingCameraTwin(FlyingTwin, CameraTwin):
    """Twin with both flight and camera capabilities (camera drones)."""

    def __repr__(self) -> str:
        return f"FlyingCameraTwin(uuid='{self.uuid}', name='{self.name}')"


class GripperCameraTwin(GripperTwin, CameraTwin):
    """Twin with both gripper and camera capabilities (manipulators with vision)."""

    def __repr__(self) -> str:
        return f"GripperCameraTwin(uuid='{self.uuid}', name='{self.name}')"


class GripperDepthCameraTwin(GripperTwin, DepthCameraTwin):
    """Twin with both gripper and depth camera capabilities (manipulators with vision)."""

    def __repr__(self) -> str:
        return f"GripperDepthCameraTwin(uuid='{self.uuid}', name='{self.name}')"


class LocomoteGripperTwin(LocomoteTwin, GripperTwin):
    """Twin with both locomotive and gripper capabilities (robots with grippers)."""

    def __repr__(self) -> str:
        return f"LocomoteGripperTwin(uuid='{self.uuid}', name='{self.name}')"


class FlyingGripperDepthCameraTwin(FlyingTwin, GripperDepthCameraTwin):
    """Twin with both flight and gripper and depth camera capabilities (drones with vision)."""

    def __repr__(self) -> str:
        return f"FlyingGripperDepthCameraTwin(uuid='{self.uuid}', name='{self.name}')"


class LocomoteGripperDepthCameraTwin(LocomoteTwin, GripperDepthCameraTwin):
    """Twin with both locomotive and gripper and depth camera capabilities (robots with vision)."""

    def __repr__(self) -> str:
        return f"LocomoteGripperDepthCameraTwin(uuid='{self.uuid}', name='{self.name}')"


class LocomoteDepthCameraTwin(LocomoteTwin, DepthCameraTwin):
    """Twin with both locomotive and depth camera capabilities (robots with vision)."""

    def __repr__(self) -> str:
        return f"LocomoteDepthCameraTwin(uuid='{self.uuid}', name='{self.name}')"


class LocomoteGripperCameraTwin(LocomoteTwin, GripperCameraTwin):
    """Twin with both locomotive and gripper and camera capabilities (robots with vision)."""

    def __repr__(self) -> str:
        return f"LocomoteGripperCameraTwin(uuid='{self.uuid}', name='{self.name}')"


class LocomoteCameraTwin(LocomoteTwin, CameraTwin):
    """Twin with both locomotive and camera capabilities (robots with vision)."""

    def __repr__(self) -> str:
        return f"LocomoteCameraTwin(uuid='{self.uuid}', name='{self.name}')"


class FlyingGripperCameraTwin(FlyingTwin, GripperCameraTwin):
    """Twin with both flight and gripper and camera capabilities (drones with vision)."""

    def __repr__(self) -> str:
        return f"FlyingGripperCameraTwin(uuid='{self.uuid}', name='{self.name}')"


class FlyingDepthCameraTwin(FlyingTwin, DepthCameraTwin):
    """Twin with both flight and depth camera capabilities (drones with vision)."""

    def __repr__(self) -> str:
        return f"FlyingDepthCameraTwin(uuid='{self.uuid}', name='{self.name}')"


def _select_twin_class(capabilities: Dict[str, Any]) -> Type[Twin]:
    """
    Select the appropriate Twin subclass based on capabilities.

    Args:
        capabilities: Asset capabilities dictionary

    Returns:
        The most appropriate Twin subclass
    """
    has_sensors = bool(capabilities.get("sensors", []))
    has_depth = any(s.get("type") == "depth" for s in capabilities.get("sensors", []))
    can_fly = capabilities.get("can_fly", False)
    can_locomote = capabilities.get("can_locomote", False)
    can_grip = capabilities.get("can_grip", False)

    # Select class based on combination of capabilities
    if can_fly:
        if can_grip and has_depth:
            return FlyingGripperDepthCameraTwin
        elif can_grip and has_sensors:
            return FlyingGripperCameraTwin
        elif has_sensors:
            return FlyingCameraTwin
        elif has_depth:
            return FlyingDepthCameraTwin
        elif can_grip:
            return FlyingGripperCameraTwin
        else:
            return FlyingTwin
    elif can_locomote:
        if can_grip and has_depth:
            return LocomoteGripperDepthCameraTwin
        elif can_grip and has_sensors:
            return LocomoteGripperCameraTwin
        elif can_grip:
            return LocomoteGripperTwin
        elif has_depth:
            return LocomoteDepthCameraTwin
        elif has_sensors:
            return LocomoteCameraTwin
        else:
            return LocomoteTwin
    elif can_grip and has_sensors:
        return GripperCameraTwin
    elif can_grip and has_depth:
        return GripperDepthCameraTwin
    elif can_fly:
        return FlyingTwin
    elif can_locomote:
        return LocomoteTwin
    elif can_grip:
        return GripperTwin
    elif has_depth:
        return DepthCameraTwin
    elif has_sensors:
        return CameraTwin
    else:
        return Twin


def create_twin(
    client: "Cyberwave",
    twin_data: Any,
    registry_id: Optional[str] = None,
) -> Twin:
    """
    Factory function to create the appropriate Twin subclass.

    This function examines the twin's capabilities and returns an instance
    of the most appropriate Twin subclass, providing IDE autocomplete
    for capability-specific methods.

    Args:
        client: Cyberwave client instance
        twin_data: Twin schema data from API
        registry_id: Optional asset registry ID for capability lookup

    Returns:
        Appropriate Twin subclass instance (CameraTwin, FlyingTwin, etc.)

    Example:
        >>> twin = create_twin(client, twin_data, "unitree/go2")
        >>> # twin is CameraTwin with start_streaming() available
    """
    # Get capabilities - prefer cached JSON which has complete capability data
    capabilities = {}

    if registry_id:
        # Use cached capabilities from JSON (most complete source)
        capabilities = _get_asset_capabilities(registry_id)

    # Fall back to twin_data capabilities if no cached data
    if not capabilities:
        if hasattr(twin_data, "capabilities") and twin_data.capabilities:
            caps = twin_data.capabilities
            # Convert to dict if it's an object
            capabilities = (
                caps if isinstance(caps, dict) else getattr(caps, "__dict__", {})
            )
        elif isinstance(twin_data, dict) and twin_data.get("capabilities"):
            capabilities = twin_data["capabilities"]

    # Select and instantiate the appropriate class
    twin_class = _select_twin_class(capabilities)
    return twin_class(client, twin_data)
