"""
sim_object.py
Base class for simulation objects with attachment support.
"""

from dataclasses import dataclass, field
from typing import Any, Callable, List, Optional, Dict, Tuple, Union
import logging

import pybullet as p

from .geometry import Pose
from .logging_utils import get_lazy_logger, get_named_lazy_logger
from .types import CollisionMode
from .tools import resolve_link_index

# Standard logger for info/warning/error
logger = logging.getLogger(__name__)

# Lazy logger for debug messages (avoids expensive string formatting when disabled)
lazy_logger = get_lazy_logger(__name__)


@dataclass
class ShapeParams:
    """
    Parameters for visual or collision shape.

    Similar to URDF's <visual> and <collision> tags, this allows full control
    over shape type, geometry, appearance, and frame offset.

    Attributes:
        shape_type: Shape type - "box", "sphere", "cylinder", "mesh", or None (no shape)
        mesh_path: Path to mesh file (required if shape_type="mesh")
        mesh_scale: Mesh scaling [sx, sy, sz] (for mesh shapes)
        half_extents: Box half extents [hx, hy, hz] (for box shapes)
        radius: Sphere/cylinder radius (for sphere/cylinder shapes)
        height: Cylinder height (for cylinder shapes)
        rgba_color: RGBA color [r, g, b, a] (for visual shapes, ignored for collision)
        frame_pose: Shape offset as Pose object (relative to body frame)

    Example::

        # Box visual
        visual = ShapeParams(
            shape_type="box",
            half_extents=[0.5, 0.3, 0.2],
            rgba_color=[0.8, 0.2, 0.2, 1.0]
        )

        # Mesh collision
        collision = ShapeParams(
            shape_type="mesh",
            mesh_path="collision.obj",
            mesh_scale=[1.0, 1.0, 1.0]
        )

        # Sphere with offset
        sphere = ShapeParams(
            shape_type="sphere",
            radius=0.5,
            rgba_color=[0.0, 1.0, 0.0, 1.0],
            frame_pose=Pose.from_xyz(0, 0, 0.5)
        )
    """

    shape_type: Optional[str] = None  # "box", "sphere", "cylinder", "mesh", or None
    mesh_path: Optional[str] = None
    mesh_scale: List[float] = field(default_factory=lambda: [1.0, 1.0, 1.0])
    half_extents: List[float] = field(default_factory=lambda: [0.5, 0.5, 0.5])
    radius: float = 0.5
    height: float = 1.0
    rgba_color: List[float] = field(default_factory=lambda: [0.8, 0.8, 0.8, 1.0])
    frame_pose: Optional[Pose] = None


@dataclass
class SimObjectSpawnParams:
    """
    Parameters for spawning a SimObject.

    Attributes:
        visual_shape: Visual shape parameters (ShapeParams or None)
        collision_shape: Collision shape parameters (ShapeParams or None)
        initial_pose: Initial Pose (position and orientation) in world coordinates (default: origin)
        mass: Object mass in kg (0.0 for static objects, >0 for dynamic)
        pickable: Whether this object can be picked by robots (default: True)
        name: Optional string name for human-readable identification (default: None).
              Duplicates allowed - multiple objects can share the same name.
              Use for debugging, logging, filtering (e.g., "LeftRobot", "Pallet_A").
              For unique identification, use object_id instead.
        user_data: Optional dictionary for custom metadata (default: empty dict)

    Note:
        Managers calculate positions automatically and may override initial_pose.

    Example::

        # Box visual + sphere collision with name
        params = SimObjectSpawnParams(
            visual_shape=ShapeParams(
                shape_type="box",
                half_extents=[0.5, 0.3, 0.2],
                rgba_color=[1.0, 0.0, 0.0, 1.0]
            ),
            collision_shape=ShapeParams(
                shape_type="sphere",
                radius=0.3
            ),
            initial_pose=Pose.from_xyz(0, 0, 1),
            mass=1.0,
            name="Pallet_A"  # Human-readable name (duplicates allowed)
        )
        obj = SimObject.from_params(params, sim_core)

        # Later: Use object_id for unique lookup
        retrieved_obj = sim_core.get_object_by_id(obj.object_id)

        # Or filter by name (may return multiple objects)
        pallets = [o for o in sim_core.objects if o.name == "Pallet_A"]
    """

    visual_shape: Optional[ShapeParams] = None
    collision_shape: Optional[ShapeParams] = None
    initial_pose: Pose = field(default_factory=lambda: Pose.from_xyz(0.0, 0.0, 0.0))
    mass: float = 0.0
    pickable: bool = True
    name: Optional[str] = None  # Human-readable name (duplicates allowed, use object_id for unique lookup)
    visual_frame_pose: Optional[Pose] = None
    collision_frame_pose: Optional[Pose] = None
    collision_mode: CollisionMode = CollisionMode.NORMAL_3D  # Collision detection mode
    user_data: Dict[str, Any] = field(default_factory=dict)  # Custom metadata storage

    @classmethod
    def from_dict(cls, config: Dict[str, Any]) -> "SimObjectSpawnParams":
        """
        Create SimObjectSpawnParams from configuration dictionary.

        Args:
            config: Dictionary with object parameters.

        Returns:
            SimObjectSpawnParams instance

        Example::

            config = {
                'visual_shape': ShapeParams(shape_type='box', half_extents=[0.5, 0.3, 0.2]),
                'collision_shape': ShapeParams(shape_type='sphere', radius=0.3),
                'initial_pose': Pose.from_xyz(1.0, 2.0, 0.0),
                'mass': 1.0,
                'name': 'Pallet_A',
            }
            params = SimObjectSpawnParams.from_dict(config)
        """
        # Convert collision_mode string to enum if needed
        collision_mode_value = config.get("collision_mode", CollisionMode.NORMAL_3D)
        if isinstance(collision_mode_value, str):
            collision_mode_value = CollisionMode(collision_mode_value)

        return cls(
            visual_shape=config.get("visual_shape"),
            collision_shape=config.get("collision_shape"),
            initial_pose=config.get("initial_pose", Pose.from_xyz(0.0, 0.0, 0.0)),
            mass=config.get("mass", 0.0),
            pickable=config.get("pickable", True),
            name=config.get("name"),
            collision_mode=collision_mode_value,
            user_data=config.get("user_data", {}),
        )


class SimObject:
    """
    Base class for simulation objects (robots, pallets, obstacles).
    Supports attachment/detachment and kinematic teleportation.

    Object Identification:
        - object_id: Unique integer ID assigned by sim_core (used for lookups and internal tracking).
                    This is the primary identifier for programmatic access.
        - name: Optional string name for human-readable identification (used for debugging, logging, filtering).
               Multiple objects can share the same name (duplicates allowed).
               Not guaranteed to be unique - use object_id for unique identification.

    Args:
        body_id: PyBullet body ID
        sim_core: Reference to simulation core (optional)
        pickable: Whether this object can be picked by robots (default: True)

    Attributes:
        object_id: Unique integer ID (assigned by sim_core, use for lookups)
        name: Optional string name (duplicates allowed, use for debugging/filtering)
        body_id: PyBullet body ID
        mass: Object mass in kg
        pickable: Whether this object can be picked by robots
        collision_mode: Collision detection mode (NORMAL_3D, NORMAL_2D, STATIC, DISABLED)
        user_data: Dictionary for custom metadata (default: empty dict)
    """

    # Class-level shared shapes cache (for optimization)
    _shared_shapes: Dict[str, Tuple[int, int]] = {}

    def __init__(
        self,
        body_id: int,
        sim_core=None,
        pickable: bool = True,
        mass: Optional[float] = None,
        collision_mode: CollisionMode = CollisionMode.NORMAL_3D,
        name: Optional[str] = None,
        user_data: Optional[Dict[str, Any]] = None,
    ):
        self.body_id = body_id
        self.sim_core = sim_core

        self.attached_objects: List["SimObject"] = []
        self.callbacks: List[dict] = []
        self.pickable = pickable
        self.collision_mode = collision_mode  # Collision detection mode

        # User-extensible data storage (custom metadata)
        self.user_data: Dict[str, Any] = dict(user_data) if user_data else {}

        # Optional name for human-readable identification (debugging, logging, filtering)
        # Note: Unlike object_id, name is NOT unique - multiple objects can share the same name.
        # For programmatic lookup/search, use object_id instead.
        # Examples: "LeftRobot_1", "Pallet_A", "Obstacle_Wall"
        self.name: Optional[str] = name

        # Assign unique ID from sim_core if available
        # Note: object_id is UNIQUE and should be used for programmatic object identification.
        # Use sim_core.get_object_by_id(object_id) for lookups.
        if sim_core is not None and hasattr(sim_core, "_next_object_id"):
            self.object_id = sim_core._next_object_id
            sim_core._next_object_id += 1
        else:
            self.object_id = -1  # No ID if not registered to sim_core

        # Instance-level logger with prefix for identification in multi-object scenarios.
        # Subclasses (Agent) can update the prefix via self._log.set_prefix(...).
        self._log = get_named_lazy_logger(__name__)
        self._update_log_prefix()

        # Attachment state (initialized with default zero offset)
        self._attach_offset: Pose = Pose(position=[0.0, 0.0, 0.0], orientation=[0.0, 0.0, 0.0, 1.0])
        self._constraint_id: Optional[int] = None
        self._attached_to: Optional["SimObject"] = None  # Which object is this attached to
        self._attached_link_index: int = -1  # Which link is this attached to

        # Mass (for optimization - query from PyBullet if not provided)
        if mass is not None:
            self.mass = mass
        else:
            # Query from PyBullet (getDynamicsInfo returns mass at index 0)
            self.mass = p.getDynamicsInfo(self.body_id, -1, physicsClientId=self._pid)[0]

        # Kinematic flag (mass=0 means kinematic/static object)
        self.is_kinematic = self.mass == 0.0

        # Pose caching for performance optimization (also used for movement detection)
        initial_pos, initial_orn = p.getBasePositionAndOrientation(self.body_id, physicsClientId=self._pid)
        self._cached_pose: Pose = Pose.from_pybullet(initial_pos, initial_orn)
        self._cached_pose_sim_time: float = -1.0  # Simulation time when pose was cached

        # Disable PyBullet physics collision if collision_mode is DISABLED
        if self.collision_mode == CollisionMode.DISABLED:
            # setCollisionFilterGroupMask: (bodyId, linkId, collisionFilterGroup, collisionFilterMask)
            # Setting mask=0 disables collision with all objects
            p.setCollisionFilterGroupMask(self.body_id, -1, 0, 0, physicsClientId=self._pid)
            self._log.debug(f"Disabled PyBullet collision (body {self.body_id})")

        # Auto-register to sim_core if provided
        if sim_core is not None:
            # Centralized registration via add_object() for consistency
            sim_core.add_object(self)

    @property
    def _pid(self) -> int:
        """PyBullet physicsClientId (falls back to 0 if no sim_core)."""
        return self.sim_core._client if self.sim_core is not None else 0

    def set_name(self, name: Optional[str]) -> None:
        """
        Set the object name and update the log prefix accordingly.

        Use this instead of directly setting ``self.name`` to ensure
        the log prefix stays in sync.

        Args:
            name: New human-readable name (None to clear)
        """
        if self.name != name:
            self.name = name
            self._update_log_prefix()

    def _update_log_prefix(self):
        """
        Update the instance logger prefix based on current identification.

        Called automatically during __init__. Subclasses (Agent) should call
        this again if they add extra identification info (e.g. "Agent" class name).

        Format: ``[ClassName:object_id:name]`` or ``[ClassName:object_id]`` if no name.
        """
        cls_name = self.__class__.__name__
        if self.name:
            self._log.set_prefix(f"[{cls_name}:{self.object_id}:{self.name}] ")
        else:
            self._log.set_prefix(f"[{cls_name}:{self.object_id}] ")

    @classmethod
    def create_shared_shapes(
        cls,
        visual_shape: Optional[ShapeParams] = None,
        collision_shape: Optional[ShapeParams] = None,
        physics_client_id: int = 0,
    ) -> Tuple[int, int]:
        """
        Create shared visual and collision shapes for fast object spawning.

        This method caches mesh shapes by their parameters, so multiple objects with
        identical mesh parameters will reuse the same PyBullet shape IDs.

        Note: Primitive shapes (box, sphere, cylinder) are NOT cached, as PyBullet
        handles them very efficiently and caching adds unnecessary overhead.

        Args:
            visual_shape: ShapeParams for visual geometry
            collision_shape: ShapeParams for collision geometry
            physics_client_id: PyBullet physics client ID

        Returns:
            (visual_id, collision_id) tuple

        Example::

            visual_id, collision_id = create_shared_shapes(
                visual_shape=ShapeParams(
                    shape_type="mesh",
                    mesh_path="visual.obj",
                    mesh_scale=[1.0, 1.0, 1.0],
                    rgba_color=[0.8, 0.2, 0.2, 1.0]
                ),
                collision_shape=ShapeParams(
                    shape_type="box",
                    half_extents=[0.5, 0.3, 0.2]
                ),
                physics_client_id=sim_core.client
            )
        """
        # Only cache mesh shapes (primitives are cheap to create)
        visual_needs_cache = visual_shape and visual_shape.shape_type == "mesh"
        collision_needs_cache = collision_shape and collision_shape.shape_type == "mesh"

        # Create cache key only for mesh shapes
        if visual_needs_cache or collision_needs_cache:
            visual_key = cls._shape_params_to_key(visual_shape) if visual_needs_cache else "none"
            collision_key = cls._shape_params_to_key(collision_shape) if collision_needs_cache else "none"
            shape_key = f"v:{visual_key}_c:{collision_key}"

            # Check cache
            if shape_key in cls._shared_shapes:
                return cls._shared_shapes[shape_key]

        # Create visual shape
        visual_id = (
            cls._create_visual_shape(visual_shape, physics_client_id) if visual_shape and visual_shape.shape_type else -1
        )

        # Create collision shape
        collision_id = (
            cls._create_collision_shape(collision_shape, physics_client_id)
            if collision_shape and collision_shape.shape_type
            else -1
        )

        # Cache only if at least one is a mesh shape
        if visual_needs_cache or collision_needs_cache:
            cls._shared_shapes[shape_key] = (visual_id, collision_id)

        return visual_id, collision_id

    @staticmethod
    def _shape_params_to_key(shape: ShapeParams) -> str:
        """Convert ShapeParams to a unique cache key string."""
        frame_pos = shape.frame_pose.position if shape.frame_pose else [0, 0, 0]
        frame_orn = shape.frame_pose.orientation if shape.frame_pose else [0, 0, 0, 1]

        return (
            f"{shape.shape_type}_{shape.mesh_path}_{tuple(shape.mesh_scale)}_"
            f"{tuple(shape.half_extents)}_{shape.radius}_{shape.height}_"
            f"{tuple(shape.rgba_color)}_{tuple(frame_pos)}_{tuple(frame_orn)}"
        )

    @staticmethod
    def _create_visual_shape(shape: ShapeParams, physics_client_id: int) -> int:
        """Create a PyBullet visual shape from ShapeParams."""
        # Get frame offset
        frame_position = [0.0, 0.0, 0.0]
        frame_orientation = [0.0, 0.0, 0.0, 1.0]
        if shape.frame_pose:
            frame_position, frame_orientation = shape.frame_pose.as_position_orientation()

        if shape.shape_type == "mesh":
            if not shape.mesh_path:
                raise ValueError("mesh_path is required for shape_type='mesh'")
            return p.createVisualShape(
                p.GEOM_MESH,
                fileName=shape.mesh_path,
                meshScale=shape.mesh_scale,
                rgbaColor=shape.rgba_color,
                visualFramePosition=frame_position,
                visualFrameOrientation=frame_orientation,
                physicsClientId=physics_client_id,
            )
        elif shape.shape_type == "box":
            return p.createVisualShape(
                p.GEOM_BOX,
                halfExtents=shape.half_extents,
                rgbaColor=shape.rgba_color,
                visualFramePosition=frame_position,
                visualFrameOrientation=frame_orientation,
                physicsClientId=physics_client_id,
            )
        elif shape.shape_type == "sphere":
            return p.createVisualShape(
                p.GEOM_SPHERE,
                radius=shape.radius,
                rgbaColor=shape.rgba_color,
                visualFramePosition=frame_position,
                visualFrameOrientation=frame_orientation,
                physicsClientId=physics_client_id,
            )
        elif shape.shape_type == "cylinder":
            return p.createVisualShape(
                p.GEOM_CYLINDER,
                radius=shape.radius,
                length=shape.height,
                rgbaColor=shape.rgba_color,
                visualFramePosition=frame_position,
                visualFrameOrientation=frame_orientation,
                physicsClientId=physics_client_id,
            )
        else:
            raise ValueError(f"Unknown visual shape_type: {shape.shape_type}")

    @staticmethod
    def _create_collision_shape(shape: ShapeParams, physics_client_id: int) -> int:
        """Create a PyBullet collision shape from ShapeParams."""
        # Get frame offset
        frame_position = [0.0, 0.0, 0.0]
        frame_orientation = [0.0, 0.0, 0.0, 1.0]
        if shape.frame_pose:
            frame_position, frame_orientation = shape.frame_pose.as_position_orientation()

        if shape.shape_type == "mesh":
            if not shape.mesh_path:
                raise ValueError("mesh_path is required for shape_type='mesh'")
            return p.createCollisionShape(
                p.GEOM_MESH,
                fileName=shape.mesh_path,
                meshScale=shape.mesh_scale,
                collisionFramePosition=frame_position,
                collisionFrameOrientation=frame_orientation,
                physicsClientId=physics_client_id,
            )
        elif shape.shape_type == "box":
            return p.createCollisionShape(
                p.GEOM_BOX,
                halfExtents=shape.half_extents,
                collisionFramePosition=frame_position,
                collisionFrameOrientation=frame_orientation,
                physicsClientId=physics_client_id,
            )
        elif shape.shape_type == "sphere":
            return p.createCollisionShape(
                p.GEOM_SPHERE,
                radius=shape.radius,
                collisionFramePosition=frame_position,
                collisionFrameOrientation=frame_orientation,
                physicsClientId=physics_client_id,
            )
        elif shape.shape_type == "cylinder":
            return p.createCollisionShape(
                p.GEOM_CYLINDER,
                radius=shape.radius,
                height=shape.height,
                collisionFramePosition=frame_position,
                collisionFrameOrientation=frame_orientation,
                physicsClientId=physics_client_id,
            )
        else:
            raise ValueError(f"Unknown collision shape_type: {shape.shape_type}")

    @classmethod
    def _create_body_from_shapes(
        cls,
        visual_id: int,
        collision_id: int,
        pose: Pose,
        mass: float = 0.0,
        physics_client_id: int = 0,
    ) -> int:
        """
        Create a PyBullet multibody from pre-created visual and collision shapes.

        This is a helper method to reduce code duplication between SimObject and Agent.

        Args:
            visual_id: Visual shape ID from create_shared_shapes()
            collision_id: Collision shape ID from create_shared_shapes()
            pose: Initial Pose (position and orientation)
            mass: Body mass (kg)
            physics_client_id: PyBullet physics client ID

        Returns:
            PyBullet body ID
        """
        position, orientation = pose.as_position_orientation()
        body_id = p.createMultiBody(
            baseMass=mass,
            baseCollisionShapeIndex=collision_id,
            baseVisualShapeIndex=visual_id,
            basePosition=position,
            baseOrientation=orientation,
            physicsClientId=physics_client_id,
        )
        return body_id

    @classmethod
    def from_mesh(
        cls,
        visual_shape: Optional[ShapeParams] = None,
        collision_shape: Optional[ShapeParams] = None,
        pose: Pose = None,
        mass: float = 0.0,
        pickable: bool = True,
        sim_core=None,
        collision_mode: CollisionMode = CollisionMode.NORMAL_3D,
        name: Optional[str] = None,
        user_data: Optional[Dict[str, Any]] = None,
    ) -> "SimObject":
        """
        Create a SimObject with optional visual and collision shapes.

        Args:
            visual_shape: ShapeParams for visual geometry
            collision_shape: ShapeParams for collision geometry
            pose: Initial Pose (position and orientation, default: origin)
            mass: Object mass (0.0 for static)
            pickable: Whether object can be picked by robots
            sim_core: Reference to simulation core
            name: Optional human-readable name (duplicates allowed)

        Returns:
            SimObject instance

        Example::

            # Box visual + sphere collision
            obj = SimObject.from_mesh(
                visual_shape=ShapeParams(
                    shape_type="box",
                    half_extents=[0.5, 0.3, 0.2],
                    rgba_color=[0.8, 0.2, 0.2, 1.0]
                ),
                collision_shape=ShapeParams(
                    shape_type="sphere",
                    radius=0.4
                ),
                pose=Pose.from_xyz(0, 0, 1),
                mass=1.0,
                sim_core=sim_core
            )
        """
        if pose is None:
            pose = Pose.from_xyz(0.0, 0.0, 0.0)

        # Get physics client ID from sim_core
        physics_client_id = sim_core.client if sim_core is not None else 0

        # Use create_shared_shapes() to get or create cached shapes
        visual_id, collision_id = cls.create_shared_shapes(
            visual_shape=visual_shape,
            collision_shape=collision_shape,
            physics_client_id=physics_client_id,
        )

        # Create multibody using helper method
        body_id = cls._create_body_from_shapes(
            visual_id=visual_id,
            collision_id=collision_id,
            pose=pose,
            mass=mass,
            physics_client_id=physics_client_id,
        )

        return cls(
            body_id=body_id,
            sim_core=sim_core,
            pickable=pickable,
            mass=mass,
            collision_mode=collision_mode,
            name=name,
            user_data=user_data,
        )

    @classmethod
    def from_params(cls, spawn_params: SimObjectSpawnParams, sim_core=None) -> "SimObject":
        """
        Create a SimObject from SimObjectSpawnParams.

        Args:
            spawn_params: SimObjectSpawnParams instance
            sim_core: Reference to simulation core

        Returns:
            SimObject instance

        Example::

            params = SimObjectSpawnParams(
                visual_shape=ShapeParams(
                    shape_type="mesh",
                    mesh_path="pallet.obj",
                    mesh_scale=[0.5, 0.5, 0.5],
                    rgba_color=[0.8, 0.6, 0.4, 1.0]
                ),
                collision_shape=ShapeParams(
                    shape_type="box",
                    half_extents=[0.5, 0.4, 0.1]
                ),
                initial_pose=Pose.from_xyz(0, 0, 0.1),
                mass=1.0
            )
            obj = SimObject.from_params(params, sim_core)
        """
        obj = cls.from_mesh(
            visual_shape=spawn_params.visual_shape,
            collision_shape=spawn_params.collision_shape,
            pose=spawn_params.initial_pose,
            mass=spawn_params.mass,
            pickable=spawn_params.pickable,
            sim_core=sim_core,
            collision_mode=spawn_params.collision_mode,
            name=spawn_params.name,
            user_data=spawn_params.user_data,
        )

        return obj

    @property
    def is_static(self) -> bool:
        """
        Whether this object is static (never moves).

        Returns True if collision_mode is STATIC.
        This property is read-only. To change collision mode,
        use set_collision_mode() method.
        """
        return self.collision_mode == CollisionMode.STATIC

    def set_collision_mode(self, mode: CollisionMode) -> None:
        """
        Change collision detection mode for this object.

        This method updates the collision mode and notifies sim_core to
        update all collision-related caches (AABB, spatial grid, etc.).

        Args:
            mode: New CollisionMode

        Example::

            # Change to static
            obj.set_collision_mode(CollisionMode.STATIC)

            # Disable collision
            obj.set_collision_mode(CollisionMode.DISABLED)

            # Enable 2D collision
            obj.set_collision_mode(CollisionMode.NORMAL_2D)
        """
        if self.collision_mode == mode:
            return  # No change

        old_mode = self.collision_mode
        self.collision_mode = mode

        # Update PyBullet collision filter if switching to/from DISABLED
        if mode == CollisionMode.DISABLED:
            # Disable PyBullet collision
            p.setCollisionFilterGroupMask(self.body_id, -1, 0, 0, physicsClientId=self._pid)
        elif old_mode == CollisionMode.DISABLED:
            # Re-enable PyBullet collision (default group=1, mask=-1)
            p.setCollisionFilterGroupMask(self.body_id, -1, 1, -1, physicsClientId=self._pid)

        # Notify sim_core to update collision system
        if self.sim_core is not None:
            self.sim_core._update_object_collision_mode(self.object_id, old_mode, mode)

        self._log.info(f"collision_mode changed from {old_mode.value} -> {mode.value}")

    def get_pose(self) -> Pose:
        """
        Return current position and orientation as Pose object.

        Performance optimization:

        - Kinematic objects (mass=0): Always returns cached pose since position
          only changes via set_pose(), which updates the cache.
        - Dynamic objects: Caches within the same simulation timestep to avoid
          redundant PyBullet API calls. Cache is invalidated when sim_time advances.
        """
        # Kinematic objects never move on their own - cache is always valid
        # (position only changes via set_pose(), which updates the cache)
        if self.is_kinematic:
            return self._cached_pose

        # Dynamic objects: check if cache is valid for current sim time
        current_sim_time = self.sim_core.sim_time if self.sim_core is not None else -1.0

        if self._cached_pose is not None and current_sim_time >= 0 and self._cached_pose_sim_time == current_sim_time:
            return self._cached_pose

        # Cache miss: Query PyBullet and update cache
        position, orientation = p.getBasePositionAndOrientation(self.body_id, physicsClientId=self._pid)
        self._cached_pose = Pose.from_pybullet(position, orientation)
        self._cached_pose_sim_time = current_sim_time

        return self._cached_pose

    def set_pose(self, pose: Pose, preserve_velocity: bool = True) -> bool:
        """
        Set position and orientation from a Pose object.

        Args:
            pose: Pose object containing position and orientation
            preserve_velocity: If True, preserve current velocity after setting pose.
                             If False, velocity is reset to zero (default behavior of PyBullet).
                             For kinematic objects (mass=0), velocity preservation has no effect,
                             so this is automatically set to False for performance.

        Returns:
            True if the object moved (position or orientation changed), False otherwise
        """
        position, orientation = pose.as_position_orientation()
        moved = self._set_pose_internal(position, orientation, preserve_velocity)
        return moved

    def set_pose_raw(
        self,
        position,
        orientation,
        preserve_velocity: bool = True,
    ) -> bool:
        """
        Set position and orientation from raw lists/tuples (no Pose allocation).

        This is the fast path for internal use where position and orientation
        are already available as lists/tuples, avoiding Pose object creation overhead.

        Args:
            position: [x, y, z] position (list, tuple, or any sequence)
            orientation: [x, y, z, w] quaternion (list, tuple, or any sequence)
            preserve_velocity: If True, preserve current velocity after setting pose.

        Returns:
            True if the object moved, False otherwise
        """
        return self._set_pose_internal(position, orientation, preserve_velocity)

    def _set_pose_internal(
        self,
        position,
        orientation,
        preserve_velocity: bool = True,
    ) -> bool:
        """
        Internal implementation for setting position and orientation.

        Shared by set_pose() and set_pose_raw() to avoid code duplication.

        Args:
            position: [x, y, z] position
            orientation: [x, y, z, w] quaternion
            preserve_velocity: If True, preserve current velocity after setting pose.

        Returns:
            True if the object moved, False otherwise
        """
        # Static objects should never move - warn and return early
        if self.collision_mode == CollisionMode.STATIC:
            self._log.warning(
                "Attempting to move static object. "
                "Static objects (CollisionMode.STATIC) should not be moved. "
                "If this object needs to move, change collision_mode to NORMAL_3D/NORMAL_2D."
            )
            return False

        # Detect movement using cached pose (no PyBullet API call needed)
        # Use manual distance calculation instead of np.linalg.norm for ~10x speedup
        last_pos = self._cached_pose.position
        last_orn = self._cached_pose.orientation
        dx = position[0] - last_pos[0]
        dy = position[1] - last_pos[1]
        dz = position[2] - last_pos[2]
        pos_diff_sq = dx * dx + dy * dy + dz * dz

        # Quaternion angular distance: min(|q1 - q2|², |q1 + q2|²) to handle q = -q equivalence
        dq0 = orientation[0] - last_orn[0]
        dq1 = orientation[1] - last_orn[1]
        dq2 = orientation[2] - last_orn[2]
        dq3 = orientation[3] - last_orn[3]
        orn_diff_sq = dq0 * dq0 + dq1 * dq1 + dq2 * dq2 + dq3 * dq3
        # Check q = -q equivalence only if diff is significant
        if orn_diff_sq > 1e-12:
            sq0 = orientation[0] + last_orn[0]
            sq1 = orientation[1] + last_orn[1]
            sq2 = orientation[2] + last_orn[2]
            sq3 = orientation[3] + last_orn[3]
            orn_sum_sq = sq0 * sq0 + sq1 * sq1 + sq2 * sq2 + sq3 * sq3
            if orn_sum_sq < orn_diff_sq:
                orn_diff_sq = orn_sum_sq

        moved = pos_diff_sq > 1e-12 or orn_diff_sq > 1e-12

        # Optimization: Skip velocity operations for kinematic objects
        # Use cached is_kinematic flag to avoid PyBullet API calls
        if preserve_velocity and not self.is_kinematic:
            # Get current velocities to preserve them (for dynamic objects only)
            linear_vel, angular_vel = p.getBaseVelocity(self.body_id, physicsClientId=self._pid)
            p.resetBasePositionAndOrientation(self.body_id, position, orientation, physicsClientId=self._pid)
            p.resetBaseVelocity(self.body_id, linear_vel, angular_vel, physicsClientId=self._pid)
        else:
            # Fast path: Just set position without velocity operations
            # (kinematic objects or explicitly requested to skip velocity preservation)
            p.resetBasePositionAndOrientation(self.body_id, position, orientation, physicsClientId=self._pid)

        # Notify sim_core if movement detected
        if moved and self.sim_core is not None:
            self.sim_core._mark_object_moved(self.object_id)

            # Update AABB and spatial grid immediately for kinematic objects (optimization)
            # Physics objects are updated in step_once() to batch PyBullet calls
            if self.object_id in self.sim_core._kinematic_objects:
                self.sim_core._update_object_aabb(self.object_id)
                # Update spatial grid if cell_size is initialized
                if self.sim_core._cached_cell_size is not None:
                    self.sim_core._update_object_spatial_grid(self.object_id)

        # Update pose cache (reuse existing Pose object to avoid allocation)
        if self._cached_pose is not None:
            # Reuse existing Pose object - just update its internal lists
            self._cached_pose.position[0] = position[0]
            self._cached_pose.position[1] = position[1]
            self._cached_pose.position[2] = position[2]
            self._cached_pose.orientation[0] = orientation[0]
            self._cached_pose.orientation[1] = orientation[1]
            self._cached_pose.orientation[2] = orientation[2]
            self._cached_pose.orientation[3] = orientation[3]
        else:
            # Create new Pose object for cache
            self._cached_pose = Pose.from_pybullet(position, orientation)

        # Update cache timestamp to current sim_time
        self._cached_pose_sim_time = self.sim_core.sim_time if self.sim_core is not None else -1.0

        # Recursively apply the same coordinates and velocity to attached_objects
        self._log.debug(lambda: f"set_pose: attached_objects(before)={[o.body_id for o in self.attached_objects]}")
        for obj in self.attached_objects:
            # Skip objects attached to a specific link (not base link).
            # These are handled by Agent.update_attached_objects_kinematics()
            # which uses the actual link position from PyBullet.
            if obj._attached_link_index >= 0:
                continue
            # Follow using relative position and orientation from attachment
            # _attach_offset is always initialized with default (0,0,0) if not set
            new_pos, new_orn = p.multiplyTransforms(
                position, orientation, obj._attach_offset.position, obj._attach_offset.orientation
            )
            obj.set_pose_raw(new_pos, new_orn, preserve_velocity=preserve_velocity)
            self._log.debug(
                lambda obj=obj: f"attached_object set pose: obj_body_id={obj.body_id} "
                f"position={obj._attach_offset.position} orientation={obj._attach_offset.orientation}"
            )

        return moved  # Return movement detection result

    def attach_object(
        self,
        obj: "SimObject",
        parent_link_index: Union[int, str] = -1,
        relative_pose: Optional[Pose] = None,
        joint_type: int = p.JOINT_FIXED,
    ) -> bool:
        """
        Attach another SimObject to this object.

        Args:
            obj: Object to attach
            parent_link_index: Link index (int) or link name (str) to attach to.
                              -1 or ``"base_link"`` for the base link.
            relative_pose: Position and orientation offset in parent link's frame as Pose object
                          (default: None, which uses Pose(position=[0, 0, 0], orientation=[0, 0, 0, 1]))
            joint_type: PyBullet joint type (default: JOINT_FIXED)

        Returns:
            True if attachment successful, False otherwise

        Example::

            # Attach to base link at 0.5m in front
            agent.attach_object(pallet, relative_pose=Pose.from_xyz(0.5, 0, 0))

            # Attach with position and orientation
            agent.attach_object(pallet, relative_pose=Pose.from_euler(0.6, 0, -0.2,
                                                                       roll=1.5708, pitch=0, yaw=0))

            # Attach to specific link by index
            agent.attach_object(box, parent_link_index=2)

            # Attach to specific link by name
            agent.attach_object(box, parent_link_index="end_effector")
        """
        # Resolve link name to index
        parent_link_index = resolve_link_index(self.body_id, parent_link_index)

        # Default to zero offset if not specified
        if relative_pose is None:
            relative_pose = Pose(position=[0.0, 0.0, 0.0], orientation=[0.0, 0.0, 0.0, 1.0])

        # Check if object is pickable

        if not obj.pickable:
            self._log.info(f"Cannot attach: object {obj.body_id} is not pickable")
            return False

        # Prevent attaching an object that is already attached to another SimObject

        if obj.is_attached():
            self._log.info(
                f"Cannot attach: object {obj.body_id} is already attached to "
                f"another SimObject (body_id={obj._attached_to.body_id if obj._attached_to else None})"
            )
            return False

        if obj in self.attached_objects:
            self._log.info(f"Object {obj.body_id} already attached")
            return False

        # Add to attached list
        self.attached_objects.append(obj)
        self._log.debug(
            lambda: (
                f"attach_object: obj={obj.body_id} relative_pose={relative_pose.position} "
                f"orientation={relative_pose.orientation}"
            )
        )

        # Get parent link position and orientation
        if parent_link_index == -1:
            parent_pos, parent_orn = p.getBasePositionAndOrientation(self.body_id, physicsClientId=self._pid)
        else:
            link_state = p.getLinkState(self.body_id, parent_link_index, computeForwardKinematics=1, physicsClientId=self._pid)
            parent_pos, parent_orn = link_state[0], link_state[1]

        # Save attachment state with user-specified relative pose
        obj._attach_offset = relative_pose
        obj._attached_to = self
        obj._attached_link_index = parent_link_index
        self._log.debug(
            lambda: f"attach_object: obj={obj.body_id} _attach_offset.position={obj._attach_offset.position} "
            f"_attach_offset.orientation={obj._attach_offset.orientation}"
        )

        # Create constraint if object has mass (physics-based attachment)
        if obj.mass > 0:
            obj._constraint_id = p.createConstraint(
                parentBodyUniqueId=self.body_id,
                parentLinkIndex=parent_link_index,
                childBodyUniqueId=obj.body_id,
                childLinkIndex=-1,
                jointType=joint_type,
                jointAxis=[0, 0, 0],
                parentFramePosition=relative_pose.position,
                childFramePosition=[0, 0, 0],
                parentFrameOrientation=relative_pose.orientation,
                childFrameOrientation=[0, 0, 0, 1],
                physicsClientId=self._pid,
            )

        self._log.info(f"Attached object {obj.body_id} to link {parent_link_index}")
        return True

    def detach_object(self, obj: "SimObject") -> bool:
        """
        Detach an object from this object.

        Args:
            obj: Object to detach

        Returns:
            True if detachment successful, False otherwise

        Example::

            agent.detach_object(pallet)
        """

        if obj not in self.attached_objects:
            self._log.info(f"Object {obj.body_id} is not attached")
            return False

        # Remove from attached list
        self.attached_objects.remove(obj)

        # Debug: show attached_objects after removal
        self._log.debug(
            lambda: f"detach_object: detached={obj.body_id} "
            f"attached_objects(after)={[o.body_id for o in self.attached_objects]}"
        )

        # Remove constraint if it exists
        if obj._constraint_id is not None:
            p.removeConstraint(obj._constraint_id, physicsClientId=self._pid)
            obj._constraint_id = None

        # Reset attachment state to default values
        obj._attach_offset = Pose(position=[0.0, 0.0, 0.0], orientation=[0.0, 0.0, 0.0, 1.0])
        obj._attached_to = None
        obj._attached_link_index = -1

        self._log.info(f"Detached object {obj.body_id}")
        return True

    def get_attached_objects(self) -> List["SimObject"]:
        """
        Return list of objects attached to this object.

        Returns:
            List of attached SimObject instances
        """
        return self.attached_objects.copy()

    def is_attached(self) -> bool:
        """
        Check if this object is attached to another object.

        Returns:
            True if attached to another object, False otherwise
        """
        return self._attached_to is not None

    def register_callback(self, callback: Callable, frequency: float = 0.25):
        """Register a callback function to be executed periodically."""
        self.callbacks.append({"func": callback, "frequency": frequency, "last_executed": 0.0})

    def execute_callbacks(self, current_time: float):
        """Execute registered callbacks based on their frequency."""
        for cbinfo in self.callbacks:
            if current_time - cbinfo["last_executed"] >= cbinfo["frequency"]:
                cbinfo["func"](self)
                cbinfo["last_executed"] = current_time
