"""Unified Robot factory - convenience layer over ``strands_robots.simulation``
and ``strands_robots.hardware_robot``.

Provides:
    - ``Robot("so100")`` → returns a simulation by default (safe)
    - ``Robot("so100", mode="real")`` → explicit real hardware
    - ``Robot("so100", mode="auto")`` → auto-detects sim/real
    - ``list_robots()``  → what's available

Environment Variables:
    STRANDS_ROBOT_MODE: Override mode detection ("sim", "real", "auto").
        Case-insensitive; surrounding whitespace ignored.

Examples::

    # Default: simulation (safe - no physical hardware interaction)
    sim = Robot("so100")

    # Explicit real hardware
    hw = Robot("so100", mode="real", cameras={...})

    # Auto-detect (probes USB for servo controllers)
    robot = Robot("so100", mode="auto")

    # With custom URDF/MJCF path
    sim = Robot("my_arm", urdf_path="/path/to/robot.xml")

GPU backends (resolved through ``create_simulation``; require the backend's
optional dependency or the ``strands-robots-sim`` plugin to be installed)::

    sim = Robot("unitree_go2", backend="isaac", num_envs=4096)
    sim = Robot("so100", backend="newton", num_envs=4096, device="cuda:0")
"""

from __future__ import annotations

import contextlib
import logging
import os
from typing import TYPE_CHECKING, Any, Literal, cast, overload

from strands_robots.registry import (
    get_hardware_type,
    get_robot,
    has_hardware,
    has_sim,
    is_discoverable,
    resolve_name,
)

if TYPE_CHECKING:
    from strands_robots.hardware_robot import Robot as HardwareRobot
    from strands_robots.simulation import Simulation

logger = logging.getLogger(__name__)

_VALID_MODES = ("sim", "real", "auto")


def _normalize_mode(mode: Any) -> str:
    """Lowercase + strip a mode value if it's a string. Pass non-str through unchanged
    so the caller can produce a clean ValueError later."""
    if isinstance(mode, str):
        return mode.lower().strip()
    return mode


def _auto_detect_mode(canonical: str) -> str:
    """Auto-detect sim vs real mode.

    Priority:
        1. ``STRANDS_ROBOT_MODE`` env var (explicit override)
        2. Robot-specific USB detection (Feetech/Dynamixel servo controllers)
        3. Default to sim (safest - never accidentally send commands to hardware)
    """
    env_mode = os.getenv("STRANDS_ROBOT_MODE", "").lower().strip()
    if env_mode in ("sim", "real"):
        return env_mode
    if env_mode == "auto":
        # Explicit no-op: user asked for detection, which is what we already do.
        pass
    elif env_mode:
        logger.warning("STRANDS_ROBOT_MODE=%r ignored (expected 'sim', 'real', or 'auto')", env_mode)

    # Only probe USB if the robot actually has hardware support
    if has_hardware(canonical):
        try:
            import serial.tools.list_ports

            ports = list(serial.tools.list_ports.comports())
            servo_keywords = ["feetech", "dynamixel", "sts3215", "xl430", "xl330"]
            exclude = ["bluetooth", "internal", "debug", "apple", "modem"]
            robot_ports = [
                p
                for p in ports
                if any(
                    kw in ((p.description or "") + (getattr(p, "manufacturer", None) or "")).lower()
                    for kw in servo_keywords
                )
                and not any(s in (p.description or "").lower() for s in exclude)
            ]
            if robot_ports:
                logger.info(
                    "Auto-detected robot hardware: %s",
                    [p.device for p in robot_ports],
                )
                return "real"
        except Exception as e:
            # USB enumeration is best-effort. pyserial usually raises OSError
            # (incl. PermissionError, SerialException) but libusb backends have
            # been observed to raise RuntimeError on hub glitches. Falling back
            # to sim is always safe; we log at debug for diagnosis.
            logger.debug("USB probe failed (%s: %s); falling back to sim", type(e).__name__, e)

    return "sim"


def _validate_known_robot(canonical: str, original: str, urdf_path: str | None) -> None:
    """Reject empty/unknown robot names with a single clean error before we
    descend into the sim or hardware backends. ``urdf_path`` short-circuits the
    check because users supplying an explicit MJCF/URDF don't need a registry
    entry."""
    if urdf_path:
        return
    if not canonical:
        raise ValueError(
            f"Invalid robot name {original!r}. Pass a registered name (see ``list_robots()``) or supply ``urdf_path=``."
        )
    if (
        get_robot(canonical) is None
        and not (has_sim(canonical) or has_hardware(canonical))
        and not is_discoverable(canonical)
    ):
        # A leader arm is an input device, not a robot: it carries the same
        # servo bus and USB-serial shape as its follower, so "so101_leader"
        # reads as a plausible Robot() name. Answering it with the generic
        # registry listing invites the caller to retry with the follower name
        # on the leader's port - the exact mistake that torque-enables the arm
        # a human is holding. Name the teleoperator entry point instead.
        if canonical.endswith("_leader"):
            raise ValueError(
                f"{original!r} is a teleoperator (leader) device, not a robot. "
                f"Build it with ``Teleoperator({canonical!r}, port=...)`` and attach it to the "
                f"follower it drives: ``Robot('<follower>', mode='real', port=...)"
                f".attach_teleop({canonical!r}, port=...)``. Passing a leader to ``Robot()`` would "
                "drive the arm a human is holding as a position servo."
            )
        raise ValueError(
            f"Unknown robot {original!r} (resolved to {canonical!r}). "
            "Pass a registered name (see ``list_robots()``), one of the "
            "``robot_descriptions`` robots (see ``list_discoverable()``), "
            "or supply ``urdf_path=``."
        )


@overload
def Robot(
    name: str,
    mode: Literal["sim"] = ...,
    backend: str = ...,
    urdf_path: str | None = ...,
    cameras: dict[str, dict[str, Any]] | None = ...,
    position: list[float] | None = ...,
    data_config: str | None = ...,
    **kwargs: Any,
) -> Simulation: ...


@overload
def Robot(
    name: str,
    mode: Literal["real"],
    backend: str = ...,
    urdf_path: str | None = ...,
    cameras: dict[str, dict[str, Any]] | None = ...,
    position: list[float] | None = ...,
    data_config: str | None = ...,
    **kwargs: Any,
) -> HardwareRobot: ...


@overload
def Robot(
    name: str,
    mode: Literal["auto"] | str = ...,
    backend: str = ...,
    urdf_path: str | None = ...,
    cameras: dict[str, dict[str, Any]] | None = ...,
    position: list[float] | None = ...,
    data_config: str | None = ...,
    **kwargs: Any,
) -> Simulation | HardwareRobot: ...


def Robot(  # noqa: N802 - uppercase by design (factory mimicking a class constructor)
    name: str,
    mode: str = "sim",
    backend: str = "mujoco",
    urdf_path: str | None = None,
    cameras: dict[str, dict[str, Any]] | None = None,
    position: list[float] | None = None,
    data_config: str | None = None,
    mesh: bool | None = None,
    peer_id: str | None = None,
    **kwargs: Any,
) -> Simulation | HardwareRobot:
    """Create a robot - returns a Simulation or HardwareRobot instance.

    This is a convenience factory, NOT a wrapper class.  You get the real
    backend instance back - with full access to all its methods.

    Defaults to simulation mode so that ``Robot("so100")`` never
    accidentally sends commands to physical hardware.  Use
    ``mode="real"`` to explicitly opt into hardware control.

    Args:
        name: Robot name ("so100", "aloha", "unitree_g1", "panda", ...)
              Accepts any alias defined in ``registry/robots.json``.
        mode: "sim" (default - safe), "real" (explicit hardware), or
              "auto" (probes USB for servo controllers, falls back to sim).
              Case-insensitive; surrounding whitespace ignored.
        backend: Simulation backend name or alias, resolved through
                 ``strands_robots.simulation.create_simulation`` - the same
                 registry that powers ``create_simulation()`` directly. Built-in:
                 ``"mujoco"`` (CPU, default; aliases ``"mj"``/``"mjc"``/``"mjx"``)
                 and ``"newton"`` (GPU, warp-lang based; alias ``"nt"``). Heavy
                 out-of-tree backends (``"isaac"``) ship in the ``strands-robots-sim``
                 plugin package and resolve once installed. An unavailable backend
                 surfaces the factory's actionable install hint. Backend-specific
                 kwargs (e.g. ``num_envs``, ``device``) are forwarded to the
                 backend constructor. Only applies to ``mode="sim"``; ignored for
                 ``mode="real"``.
        urdf_path: Explicit path to URDF/MJCF file. If not provided,
                   resolved via ``strands_robots.simulation.model_registry``
                   (asset manager or ``STRANDS_ASSETS_DIR`` search paths).
        cameras: Camera config for real hardware. Example::

            {"wrist": {"type": "opencv", "index_or_path": "/dev/video0", "fps": 30}}

            Note: In ``mode="sim"``, cameras must be added after creation
            via the simulation tool (``add_camera`` action). They cannot
            be passed to the factory yet.

        position: Robot base position in the sim world, ``[x, y, z]``. Passed
            through to the backend's ``add_robot`` verbatim, so the backend's
            contract governs: omitting it spawns at the origin, and a
            wrong-length, non-numeric or non-finite vector is refused with an
            actionable message (surfaced here as ``RuntimeError``) instead of
            being replaced by the origin.
        data_config: Data configuration name for observation/action schema.
                     Defaults to the canonical robot name. For multi-camera
                     setups, specify explicitly: ``data_config="so100_dualcam"``.
        mesh: Attach a Zenoh fleet-coordination mesh. ``None`` (default) keeps
              mesh OFF for a quiet bare ``Robot(...)`` but honours the
              ``STRANDS_MESH=true`` opt-in. Pass ``True`` to force it on or
              ``False`` to force it off; an explicit value always wins over the
              env default. ``STRANDS_MESH=false`` is a hard kill switch.
        peer_id: Optional mesh peer identifier. Auto-generated when omitted.
        **kwargs: Forwarded to the underlying backend constructor.

    Returns:
        ``strands_robots.simulation.Simulation`` (sim) or
        ``strands_robots.hardware_robot.Robot`` (real hardware).

    Raises:
        ValueError: If ``mode`` is not 'sim'/'real'/'auto', if ``cameras=``
                    is passed in sim mode, if the robot name is empty
                    or not in the registry (and no ``urdf_path=`` given), or if
                    ``backend`` is not a known backend (the message lists the
                    available backends and any install hint).
        ImportError: If a known backend's optional dependency is missing
                     (e.g. ``newton`` without ``warp``); the message names the
                     pip extra to install.
        RuntimeError: If the sim world or robot fails to initialize.

    Examples::

        # Simulation (default - safe)
        sim = Robot("so100")

        # Explicit MJCF model path
        sim = Robot("my_arm", urdf_path="path/to/robot.xml")

        # Real hardware (explicit opt-in)
        hw = Robot("so100", mode="real", cameras={...})

        # Auto-detect (probes USB, falls back to sim)
        robot = Robot("so100", mode="auto")

        # The 5-line promise (defaults to sim - safe, no hardware needed)
        from strands_robots import Robot
        from strands import Agent
        robot = Robot("so100")  # mode="sim" (default)
        agent = Agent(tools=[robot])
        agent("Pick up the red cube")
    """
    canonical = resolve_name(name)
    _validate_known_robot(canonical, name, urdf_path)

    mode = _normalize_mode(mode)

    if mode == "auto":
        mode = _auto_detect_mode(canonical)

    # Resolve the mesh opt-in. Mesh is OFF by default so a bare
    # ``Robot("so100")`` is quiet and never spins up Zenoh/ACL/e-stop
    # machinery. ``mesh=None`` (the default) means "consult the
    # STRANDS_MESH env var": STRANDS_MESH=true opts in without a code
    # change. An explicit ``mesh=True``/``mesh=False`` always wins over
    # the env default. ``STRANDS_MESH=false`` remains a hard kill switch
    # enforced in ``init_mesh`` regardless of this resolution.
    if mesh is None:
        mesh = os.getenv("STRANDS_MESH", "").strip().lower() in ("true", "1", "yes")

    # --- Simulation ---
    if mode == "sim":
        if cameras is not None:
            raise ValueError(
                "cameras= is only supported in mode='real'. "
                "For sim cameras, add them via the simulation tool's "
                "'add_camera' action after creation."
            )

        from strands_robots.simulation import create_simulation

        # Resolve the backend through create_simulation - the single source of
        # truth for backend selection (built-in registry + entry-point plugins +
        # runtime registrations). The MuJoCo path is unchanged (returns the same
        # MuJoCoSimEngine); ``newton`` and plugin backends (``isaac``) construct
        # with the forwarded kwargs (``num_envs``, ``device``, ...), and an
        # unavailable backend surfaces the factory's actionable install hint
        # (e.g. ``pip install strands-robots[sim-newton]``) instead of a blanket
        # NotImplementedError. World/robot population goes through the backend-
        # agnostic SimEngine ABC methods, so it works for every backend.
        # The sim-mode overloads contract a ``Simulation`` return; create_simulation
        # is typed to the SimEngine ABC, so cast to keep that public contract.
        sim = cast("Simulation", create_simulation(backend, tool_name=f"{name}_sim", **kwargs))

        try:
            result = sim.create_world()
            if result.get("status") == "error":
                content = result.get("content", [])
                msg = content[0].get("text", str(result)) if content else str(result)
                raise RuntimeError(f"Failed to create sim world for {canonical!r}: {msg}")

            # Forward ``position`` exactly as the caller wrote it. Reading it
            # by truthiness (``position or [0.0, 0.0, 0.0]``) broke the
            # parameter twice over: a NumPy pose - what any pose arithmetic
            # produces, and a value ``add_robot`` itself accepts - raised a bare
            # ``ValueError: truth value of an array ... is ambiguous`` out of the
            # factory, and an empty vector read as "omitted", so the origin was
            # substituted for a caller mistake that ``add_robot`` refuses with an
            # actionable message. Membership (``is not None``) is the only
            # correct supplied-test for a vector, and here even that is
            # unnecessary: ``position=None`` is what ``add_robot`` already
            # documents as "spawn at the origin", so passing it through keeps ONE
            # source of truth for that default instead of a copy that can drift.
            result = sim.add_robot(
                name=name,
                urdf_path=urdf_path,
                data_config=data_config or canonical,
                position=position,
            )
            if result.get("status") == "error":
                content = result.get("content", [])
                msg = content[0].get("text", str(result)) if content else str(result)
                raise RuntimeError(f"Failed to create sim robot {canonical!r}: {msg}")
        except BaseException:
            # Cleanup ANY partial-init failure: explicit RuntimeError above OR
            # an unexpected exception from _dispatch_action itself (OOM, OS
            # error during temp-file write, MuJoCo error surfaced as exception).
            # KeyboardInterrupt during creation also lands here so the executor
            # + temp dir + MuJoCo world get released.
            # suppress() ensures destroy() errors don't mask the original exception.
            with contextlib.suppress(Exception):
                sim.destroy()
            raise

        # Attach a Zenoh mesh so the Simulation auto-discovers other peers.
        # Failure to start the mesh must NOT bring down the sim - the user
        # explicitly asked for a Simulation, mesh is an enrichment.
        try:
            from strands_robots.mesh import init_mesh

            sim_mesh = init_mesh(
                sim,
                peer_id=peer_id,
                peer_type="sim",
                mesh=mesh,
            )
            if sim_mesh is not None:
                sim.mesh = sim_mesh
                sim.peer_id = sim_mesh.peer_id
        except Exception as exc:  # noqa: BLE001 - mesh enrichment is best-effort
            logger.warning("Failed to initialise mesh for %r: %s", canonical, exc)

        _attach_device_connect(sim, canonical, mode, peer_id)
        return sim

    # --- Real hardware (explicit opt-in) ---
    elif mode == "real":
        if backend != "mujoco":
            logger.debug(
                "backend=%r ignored in mode='real' (hardware uses direct servo control)",
                backend,
            )

        from strands_robots.hardware_robot import Robot as HardwareRobotCls

        real_type = get_hardware_type(canonical) or canonical
        hw = HardwareRobotCls(
            tool_name=canonical,
            robot=real_type,
            cameras=cameras,
            **kwargs,
        )

        # Attach a Zenoh mesh so the hardware Robot auto-discovers peers.
        # Best-effort: a mesh failure must not kill a working hardware robot.
        try:
            from strands_robots.mesh import init_mesh

            hw_mesh = init_mesh(
                hw,
                peer_id=peer_id,
                peer_type="robot",
                mesh=mesh,
            )
            if hw_mesh is not None:
                hw.mesh = hw_mesh
                hw.peer_id = hw_mesh.peer_id
        except Exception as exc:  # noqa: BLE001 - mesh enrichment is best-effort
            logger.warning("Failed to initialise mesh for %r: %s", canonical, exc)

        _attach_device_connect(hw, canonical, mode, peer_id)
        return hw

    else:
        raise ValueError(f"Invalid mode {mode!r}. Choose 'sim', 'real', or 'auto' (case-insensitive).")


def _attach_device_connect(instance: Any, canonical: str, mode: str, peer_id: str | None) -> None:
    """Attach a Device Connect ``.run()`` server hook to a robot/sim instance.

    Mirrors the mesh attach above: stores peer metadata and binds ``.run()`` so
    ``Robot("so100").run()`` brings the device online as a Device Connect device
    (the primary networking layer), blocking until Ctrl+C.
    """
    instance._peer_id = peer_id or getattr(instance, "peer_id", None) or f"{canonical}-{os.urandom(3).hex()}"
    instance._peer_type = "sim" if mode == "sim" else "robot"
    instance._device_connect_runtime = None
    instance.run = lambda: _run_device_connect_foreground(instance)


def _run_device_connect_foreground(instance: Any) -> None:
    """Start Device Connect and block - the robot listens for commands.

    Device Connect is the primary networking layer in server mode, so the
    auto-started built-in mesh (if any) is stopped first to avoid running two
    Zenoh presence systems in one process.
    """
    import time

    peer_id = getattr(instance, "_peer_id", None) or "robot"
    peer_type = getattr(instance, "_peer_type", "robot")

    # Device Connect supersedes the built-in mesh in run() mode.
    mesh = getattr(instance, "mesh", None)
    if mesh is not None:
        with contextlib.suppress(Exception):
            mesh.stop()
        instance.mesh = None

    try:
        from strands_robots.device_connect import init_device_connect_sync

        instance._device_connect_runtime = init_device_connect_sync(
            instance,
            peer_id=peer_id,
            peer_type=peer_type,
        )
    except Exception as e:  # noqa: BLE001 - surface but keep the process alive
        logger.warning("Device Connect init failed: %s", e)

    print(f"{peer_id} is online. Ctrl+C to stop.")
    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print(f"\nShutting down {peer_id}...", flush=True)
        print(f"{peer_id} stopped.", flush=True)
        os._exit(0)


__all__ = ["Robot"]
