rustsim_spaces/

hybrid.rs

//! Hybrid space combining a `GraphSpace` topology with `ContinuousSpace3D`
//! surfaces at each graph node.
//!
//! Each graph node represents a zone (room, corridor, terrain patch, etc.)
//! with its own 3D bounding box. Agents have both a graph position (which
//! zone they occupy) and a continuous 3D position within that zone.
//!
//! Movement within a zone uses continuous 3D displacement. Movement between
//! zones follows graph edges.
//!
//! # Current semantic scope
//!
//! `HybridSpace` guarantees:
//! - one owning zone per agent
//! - graph-constrained inter-zone transitions
//! - zone-local 3D movement and obstacle checks
//! - same-zone Euclidean queries
//!
//! `HybridSpace` does **not** define a global geometric embedding across zones.
//! As a result, cross-zone queries are topological rather than geometric unless
//! client code supplies a higher-level portal/global-coordinate interpretation.
//!
//! ## Relationship to `GraphSpace` and `ContinuousSpace3D`
//!
//! `HybridSpace` **composes** `GraphSpace` for the graph topology (edges,
//! BFS, directed/undirected) -- it delegates all graph operations to an
//! internal `GraphSpace` instance. It does **not** compose `ContinuousSpace3D`
//! because:
//!
//! - Each zone has a different extent (rooms, corridors, stairwells).
//! - Agent IDs must be globally unique across zones with single-owner tracking.
//! - Walkmap integration is per-zone, tightly coupled to movement validation.
//! - For zones with many agents, an optional per-zone spatial hash provides
//!   O(k) neighbor queries without the overhead of N separate `ContinuousSpace3D`
//!   instances.
//!
//! ## Obstacle Maps
//!
//! Zones can optionally carry a 3D voxel walkability grid via
//! [`set_zone_walkmap`](HybridSpace::set_zone_walkmap). When present,
//! movement functions ([`move_within_zone`](HybridSpace::move_within_zone),
//! [`move_to_offset`](HybridSpace::move_to_offset)) reject displacements
//! into non-walkable voxels, keeping the agent at its previous position.
//!
//! The walkmap format is identical to the one used by
//! [`ContinuousAStar3D`](crate::continuous3d) in `rustsim-pathfinding`:
//! a flat `Vec<bool>` of size `gw * gh * gd`, indexed as
//! `walkmap[z * gh * gw + y * gw + x]`.
//!
//! ## Per-Zone Spatial Hashing
//!
//! For zones with many agents (stadium concourses, large open areas),
//! call [`enable_zone_spatial_hash`](HybridSpace::enable_zone_spatial_hash)
//! with a cell spacing. This accelerates within-zone Euclidean neighbor
//! queries from O(n) to O(k) using the same uniform-grid spatial hashing
//! algorithm as `ContinuousSpace3D`.
//!
//! Zones without spatial hashing use linear scan, which is faster for
//! small populations (< ~500 agents per zone).
//!
//! Neighbor queries combine graph-hop reachability with zone-local geometric
//! filtering only for the origin zone.

use rand::Rng;
use rustsim_core::{
    interaction::{PositionedAgent, SpaceInteraction},
    space::Space,
    types::{AgentId, ZoneId},
};
use std::collections::{HashMap, HashSet, VecDeque};
use thiserror::Error;

use crate::continuous3d::ContinuousPos3D;
use crate::graph::{GraphPos, GraphSpace, NeighborType};

// ---------------------------------------------------------------------------
// Position type
// ---------------------------------------------------------------------------

/// Position in the hybrid space: a graph node plus a 3D offset within that node's zone.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct HybridPos {
    /// Which graph node (zone) the agent occupies.
    pub node: ZoneId,
    /// Continuous 3D position within the zone's local coordinate system.
    pub offset: ContinuousPos3D,
}

impl HybridPos {
    pub fn new(node: ZoneId, offset: ContinuousPos3D) -> Self {
        Self { node, offset }
    }

    pub fn at_node(node: ZoneId) -> Self {
        Self {
            node,
            offset: ContinuousPos3D::zero(),
        }
    }
}

// ---------------------------------------------------------------------------
// Zone descriptor
// ---------------------------------------------------------------------------

/// Errors returned by zone-extent validation.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
pub enum ZoneExtentError {
    #[error("zone extents must be positive")]
    InvalidExtent,
}

/// Describes the 3D extent of a zone attached to a graph node.
#[derive(Debug, Clone, Copy)]
pub struct ZoneExtent {
    pub size_x: f64,
    pub size_y: f64,
    pub size_z: f64,
}

impl ZoneExtent {
    pub fn new(size_x: f64, size_y: f64, size_z: f64) -> Result<Self, ZoneExtentError> {
        if size_x <= 0.0 || size_y <= 0.0 || size_z <= 0.0 {
            return Err(ZoneExtentError::InvalidExtent);
        }
        Ok(Self {
            size_x,
            size_y,
            size_z,
        })
    }

    pub fn cube(size: f64) -> Result<Self, ZoneExtentError> {
        Self::new(size, size, size)
    }

    pub fn contains(&self, p: &ContinuousPos3D) -> bool {
        p.x >= 0.0
            && p.x < self.size_x
            && p.y >= 0.0
            && p.y < self.size_y
            && p.z >= 0.0
            && p.z < self.size_z
    }

    pub fn clamp(&self, p: &ContinuousPos3D) -> ContinuousPos3D {
        ContinuousPos3D::new(
            p.x.clamp(0.0, self.size_x - 1e-9),
            p.y.clamp(0.0, self.size_y - 1e-9),
            p.z.clamp(0.0, self.size_z - 1e-9),
        )
    }

    pub fn center(&self) -> ContinuousPos3D {
        ContinuousPos3D::new(self.size_x / 2.0, self.size_y / 2.0, self.size_z / 2.0)
    }
}

impl Default for ZoneExtent {
    fn default() -> Self {
        Self {
            size_x: 10.0,
            size_y: 10.0,
            size_z: 10.0,
        }
    }
}

// ---------------------------------------------------------------------------
// Zone walkability map
// ---------------------------------------------------------------------------

/// Errors returned by zone-walkmap validation.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
pub enum ZoneWalkmapError {
    #[error("walkmap dimensions must be positive")]
    InvalidDimensions,
    #[error("walkmap size mismatch: expected {expected}, got {actual}")]
    SizeMismatch { expected: usize, actual: usize },
}

/// Optional 3D voxel walkability grid for a zone.
#[derive(Debug, Clone)]
pub struct ZoneWalkmap {
    grid_w: usize,
    grid_h: usize,
    grid_d: usize,
    walkmap: Vec<bool>,
}

impl ZoneWalkmap {
    /// Create a walkmap for a zone.
    pub fn new(
        walkmap: Vec<bool>,
        grid_w: usize,
        grid_h: usize,
        grid_d: usize,
    ) -> Result<Self, ZoneWalkmapError> {
        if grid_w == 0 || grid_h == 0 || grid_d == 0 {
            return Err(ZoneWalkmapError::InvalidDimensions);
        }
        let expected = grid_w * grid_h * grid_d;
        if walkmap.len() != expected {
            return Err(ZoneWalkmapError::SizeMismatch {
                expected,
                actual: walkmap.len(),
            });
        }
        Ok(Self {
            grid_w,
            grid_h,
            grid_d,
            walkmap,
        })
    }

    /// Create a fully walkable (no obstacles) map.
    pub fn all_walkable(
        grid_w: usize,
        grid_h: usize,
        grid_d: usize,
    ) -> Result<Self, ZoneWalkmapError> {
        Self::new(vec![true; grid_w * grid_h * grid_d], grid_w, grid_h, grid_d)
    }

    /// Grid dimensions as `(w, h, d)`.
    pub fn dimensions(&self) -> (usize, usize, usize) {
        (self.grid_w, self.grid_h, self.grid_d)
    }

    /// Raw walkmap slice.
    pub fn as_slice(&self) -> &[bool] {
        &self.walkmap
    }

    /// Check if a specific voxel is walkable.
    pub fn is_walkable_voxel(&self, vx: usize, vy: usize, vz: usize) -> bool {
        if vx >= self.grid_w || vy >= self.grid_h || vz >= self.grid_d {
            return false;
        }
        self.walkmap[vz * self.grid_h * self.grid_w + vy * self.grid_w + vx]
    }

    /// Set a specific voxel's walkability.
    pub fn set_walkable_voxel(&mut self, vx: usize, vy: usize, vz: usize, walkable: bool) {
        if vx < self.grid_w && vy < self.grid_h && vz < self.grid_d {
            self.walkmap[vz * self.grid_h * self.grid_w + vy * self.grid_w + vx] = walkable;
        }
    }

    /// Check if a continuous position is in a walkable voxel within the given zone extent.
    fn is_walkable_pos(&self, pos: &ContinuousPos3D, extent: &ZoneExtent) -> bool {
        let vx = ((pos.x / extent.size_x * self.grid_w as f64).floor() as usize)
            .min(self.grid_w.saturating_sub(1));
        let vy = ((pos.y / extent.size_y * self.grid_h as f64).floor() as usize)
            .min(self.grid_h.saturating_sub(1));
        let vz = ((pos.z / extent.size_z * self.grid_d as f64).floor() as usize)
            .min(self.grid_d.saturating_sub(1));
        self.is_walkable_voxel(vx, vy, vz)
    }
}

// ---------------------------------------------------------------------------
// Per-zone spatial hash (optional)
// ---------------------------------------------------------------------------

/// Uniform-grid spatial hash for fast Euclidean neighbor queries within a zone.
///
/// Uses the same algorithm as `ContinuousSpace3D` but scoped to a single zone.
/// Only created for zones where [`enable_zone_spatial_hash`](HybridSpace::enable_zone_spatial_hash)
/// is called.
#[derive(Debug, Clone)]
struct ZoneSpatialHash {
    spacing: f64,
    grid_w: usize,
    grid_h: usize,
    grid_d: usize,
    cells: Vec<Vec<AgentId>>,
}

impl ZoneSpatialHash {
    fn new(extent: &ZoneExtent, spacing: f64) -> Result<Self, HybridSpaceError> {
        if spacing <= 0.0 {
            return Err(HybridSpaceError::InvalidSpacing(spacing));
        }
        let grid_w = (extent.size_x / spacing).ceil() as usize;
        let grid_h = (extent.size_y / spacing).ceil() as usize;
        let grid_d = (extent.size_z / spacing).ceil() as usize;
        Ok(Self {
            spacing,
            grid_w,
            grid_h,
            grid_d,
            cells: vec![Vec::new(); grid_w * grid_h * grid_d],
        })
    }

    fn pos_to_cell(&self, pos: &ContinuousPos3D) -> (usize, usize, usize) {
        let cx = ((pos.x / self.spacing).floor() as usize).min(self.grid_w.saturating_sub(1));
        let cy = ((pos.y / self.spacing).floor() as usize).min(self.grid_h.saturating_sub(1));
        let cz = ((pos.z / self.spacing).floor() as usize).min(self.grid_d.saturating_sub(1));
        (cx, cy, cz)
    }

    fn cell_index(&self, cx: usize, cy: usize, cz: usize) -> usize {
        (cz * self.grid_h + cy) * self.grid_w + cx
    }

    fn insert(&mut self, id: AgentId, pos: &ContinuousPos3D) {
        let (cx, cy, cz) = self.pos_to_cell(pos);
        let idx = self.cell_index(cx, cy, cz);
        self.cells[idx].push(id);
    }

    fn remove(&mut self, id: AgentId, pos: &ContinuousPos3D) {
        let (cx, cy, cz) = self.pos_to_cell(pos);
        let idx = self.cell_index(cx, cy, cz);
        if let Some(i) = self.cells[idx].iter().position(|&a| a == id) {
            self.cells[idx].swap_remove(i);
        }
    }

    fn update(&mut self, id: AgentId, old_pos: &ContinuousPos3D, new_pos: &ContinuousPos3D) {
        let old_cell = self.pos_to_cell(old_pos);
        let new_cell = self.pos_to_cell(new_pos);
        if old_cell != new_cell {
            self.remove(id, old_pos);
            self.insert(id, new_pos);
        }
    }

    fn nearby_ids(
        &self,
        pos: &ContinuousPos3D,
        radius: f64,
        all_positions: &HashMap<AgentId, HybridPos>,
    ) -> Vec<AgentId> {
        let grid_r = ((radius / self.spacing).ceil() as i32) + 1;
        let (focal_cx, focal_cy, focal_cz) = self.pos_to_cell(pos);
        let radius_sq = radius * radius;
        let mut ids = Vec::new();

        for dz in -grid_r..=grid_r {
            for dy in -grid_r..=grid_r {
                for dx in -grid_r..=grid_r {
                    let nx = focal_cx as i32 + dx;
                    let ny = focal_cy as i32 + dy;
                    let nz = focal_cz as i32 + dz;

                    if nx >= 0
                        && ny >= 0
                        && nz >= 0
                        && (nx as usize) < self.grid_w
                        && (ny as usize) < self.grid_h
                        && (nz as usize) < self.grid_d
                    {
                        let idx = self.cell_index(nx as usize, ny as usize, nz as usize);
                        for &id in &self.cells[idx] {
                            if let Some(agent_pos) = all_positions.get(&id) {
                                let d = pos.distance_to(&agent_pos.offset);
                                if d * d <= radius_sq {
                                    ids.push(id);
                                }
                            }
                        }
                    }
                }
            }
        }

        ids
    }
}

// ---------------------------------------------------------------------------
// Errors
// ---------------------------------------------------------------------------

#[derive(Debug, Clone, Copy, PartialEq, Error)]
pub enum HybridSpaceError {
    #[error("invalid graph node index {0}")]
    InvalidNode(GraphPos),
    #[error("position is out of bounds")]
    OutOfBounds,
    #[error("no edge from node {from} to node {to}")]
    NoEdge { from: GraphPos, to: GraphPos },
    #[error("target position is blocked by an obstacle")]
    Blocked,
    #[error("spacing must be positive, got {0}")]
    InvalidSpacing(f64),
}

// ---------------------------------------------------------------------------
// HybridSpace
// ---------------------------------------------------------------------------

/// A space that layers continuous 3D zones on top of a graph topology.
///
/// Internally composes a [`GraphSpace`] for the graph structure (edges, BFS,
/// directed/undirected). Each graph node carries a [`ZoneExtent`] defining
/// its 3D bounding box, an optional [`ZoneWalkmap`] for obstacle collision,
/// and an optional spatial hash for fast Euclidean queries within zones
/// that contain many agents.
///
/// Agents can:
/// - **Move within a zone** (continuous 3D displacement, respecting obstacles)
/// - **Transition between zones** along graph edges
/// - **Query neighbors** across both hops and Euclidean distance
///
/// Zones can optionally carry a [`ZoneWalkmap`] that marks non-walkable voxels
/// (columns, walls, barriers). Movement functions check the walkmap and reject
/// displacements into blocked cells.
#[derive(Debug, Clone)]
pub struct HybridSpace {
    graph: GraphSpace,
    zones: Vec<ZoneExtent>,
    walkmaps: Vec<Option<ZoneWalkmap>>,
    spatial_hashes: Vec<Option<ZoneSpatialHash>>,
    /// Per-zone spatial index: agent positions tracked by zone.
    agent_positions: HashMap<AgentId, HybridPos>,
    /// Agent IDs stored per node (mirrors GraphSpace's internal tracking).
    agents_at_node: Vec<Vec<AgentId>>,
}

impl HybridSpace {
    /// Create a hybrid space with `n` zones, each having the given extents.
    ///
    /// The underlying graph starts with no edges -- add them with `add_edge`.
    /// Zones start with no obstacle maps (fully walkable) and no spatial hashing.
    pub fn new(zone_extents: Vec<ZoneExtent>) -> Self {
        let n = zone_extents.len();
        Self {
            graph: GraphSpace::new(n),
            zones: zone_extents,
            walkmaps: vec![None; n],
            spatial_hashes: vec![None; n],
            agent_positions: HashMap::new(),
            agents_at_node: vec![Vec::new(); n],
        }
    }

    /// Create a hybrid space where all zones share the same extent.
    pub fn uniform(n: usize, extent: ZoneExtent) -> Self {
        Self::new(vec![extent; n])
    }

    // -- Graph delegation --

    /// Number of zone nodes in the graph.
    pub fn num_nodes(&self) -> usize {
        self.graph.num_vertices()
    }

    /// Number of edges connecting zones.
    pub fn num_edges(&self) -> usize {
        self.graph.num_edges()
    }

    /// Add an undirected edge between two zone nodes.
    ///
    /// Returns `false` if the edge already exists or either node is invalid.
    pub fn add_edge(&mut self, a: GraphPos, b: GraphPos) -> bool {
        self.graph.add_edge(a, b)
    }

    /// Remove an edge between two zone nodes.
    ///
    /// Returns `false` if the edge did not exist.
    pub fn rem_edge(&mut self, a: GraphPos, b: GraphPos) -> bool {
        self.graph.rem_edge(a, b)
    }

    /// Get neighboring zone nodes according to a [`NeighborType`] selector.
    pub fn neighbors(&self, node: GraphPos, kind: NeighborType) -> Vec<GraphPos> {
        self.graph.neighbors(node, kind)
    }

    /// Immutable reference to the underlying graph topology.
    pub fn graph(&self) -> &GraphSpace {
        &self.graph
    }

    // -- Zone configuration --

    /// Get the 3D extent descriptor for a zone.
    pub fn zone_extent(&self, node: GraphPos) -> &ZoneExtent {
        &self.zones[node]
    }

    /// Set a walkability map for a zone.
    pub fn set_zone_walkmap(
        &mut self,
        node: GraphPos,
        walkmap: ZoneWalkmap,
    ) -> Result<(), HybridSpaceError> {
        if node >= self.num_nodes() {
            return Err(HybridSpaceError::InvalidNode(node));
        }
        self.walkmaps[node] = Some(walkmap);
        Ok(())
    }

    /// Remove the walkability map for a zone (making it fully walkable again).
    pub fn clear_zone_walkmap(&mut self, node: GraphPos) {
        if node < self.num_nodes() {
            self.walkmaps[node] = None;
        }
    }

    /// Get the walkability map for a zone, if any.
    pub fn zone_walkmap(&self, node: GraphPos) -> Option<&ZoneWalkmap> {
        self.walkmaps.get(node).and_then(|w| w.as_ref())
    }

    /// Check if a continuous position within a zone is walkable.
    ///
    /// Returns `true` if the zone has no walkmap (fully walkable) or
    /// if the voxel containing the position is marked walkable.
    pub fn is_walkable(&self, node: GraphPos, pos: &ContinuousPos3D) -> bool {
        match self.walkmaps.get(node).and_then(|w| w.as_ref()) {
            Some(wm) => wm.is_walkable_pos(pos, &self.zones[node]),
            None => true,
        }
    }

    /// Enable spatial hashing for a zone to accelerate Euclidean neighbor queries.
    pub fn enable_zone_spatial_hash(
        &mut self,
        node: GraphPos,
        spacing: f64,
    ) -> Result<(), HybridSpaceError> {
        if node >= self.num_nodes() {
            return Err(HybridSpaceError::InvalidNode(node));
        }
        let mut hash = ZoneSpatialHash::new(&self.zones[node], spacing)?;
        for &id in &self.agents_at_node[node] {
            if let Some(pos) = self.agent_positions.get(&id) {
                hash.insert(id, &pos.offset);
            }
        }
        self.spatial_hashes[node] = Some(hash);
        Ok(())
    }

    /// Disable spatial hashing for a zone (reverts to linear scan).
    pub fn disable_zone_spatial_hash(&mut self, node: GraphPos) {
        if node < self.num_nodes() {
            self.spatial_hashes[node] = None;
        }
    }

    /// Whether spatial hashing is enabled for a zone.
    pub fn has_zone_spatial_hash(&self, node: GraphPos) -> bool {
        self.spatial_hashes.get(node).is_some_and(|h| h.is_some())
    }

    /// Add a new zone node with the given extent and return its index.
    pub fn add_node(&mut self, extent: ZoneExtent) -> GraphPos {
        let idx = self.graph.add_vertex();
        self.zones.push(extent);
        self.walkmaps.push(None);
        self.spatial_hashes.push(None);
        self.agents_at_node.push(Vec::new());
        idx
    }

    // -- Agent queries --

    /// Look up the current hybrid position of an agent.
    pub fn agent_position(&self, id: AgentId) -> Option<&HybridPos> {
        self.agent_positions.get(&id)
    }

    /// Get the agent IDs stored at a given zone node.
    pub fn agents_at_node(&self, node: GraphPos) -> &[AgentId] {
        &self.agents_at_node[node]
    }

    // -- Agent lifecycle --

    /// Place an agent at a position in the hybrid space.
    ///
    /// The offset is clamped to the zone's bounds. If a walkmap is present,
    /// the target voxel must be walkable or [`HybridSpaceError::Blocked`] is returned.
    pub fn add_agent_internal(
        &mut self,
        id: AgentId,
        pos: HybridPos,
    ) -> Result<(), HybridSpaceError> {
        if pos.node >= self.num_nodes() {
            return Err(HybridSpaceError::InvalidNode(pos.node));
        }
        let clamped = self.zones[pos.node].clamp(&pos.offset);
        if !self.is_walkable(pos.node, &clamped) {
            return Err(HybridSpaceError::Blocked);
        }
        let final_pos = HybridPos::new(pos.node, clamped);
        self.agent_positions.insert(id, final_pos);
        self.agents_at_node[pos.node].push(id);
        if let Some(hash) = &mut self.spatial_hashes[pos.node] {
            hash.insert(id, &clamped);
        }
        Ok(())
    }

    fn remove_agent_internal(&mut self, id: AgentId) {
        if let Some(pos) = self.agent_positions.remove(&id) {
            if let Some(i) = self.agents_at_node[pos.node]
                .iter()
                .position(|&aid| aid == id)
            {
                self.agents_at_node[pos.node].swap_remove(i);
            }
            if let Some(hash) = &mut self.spatial_hashes[pos.node] {
                hash.remove(id, &pos.offset);
            }
        }
    }

    // -- Movement --

    /// Move an agent within its current zone by a 3D displacement.
    ///
    /// The resulting position is clamped to the zone bounds. If a walkmap
    /// is present and the target voxel is blocked, the agent stays at its
    /// current position and [`HybridSpaceError::Blocked`] is returned.
    pub fn move_within_zone(
        &mut self,
        id: AgentId,
        displacement: ContinuousPos3D,
    ) -> Result<HybridPos, HybridSpaceError> {
        let pos = self
            .agent_positions
            .get(&id)
            .copied()
            .ok_or(HybridSpaceError::OutOfBounds)?;

        let new_offset = pos.offset + displacement;
        let clamped = self.zones[pos.node].clamp(&new_offset);

        if !self.is_walkable(pos.node, &clamped) {
            return Err(HybridSpaceError::Blocked);
        }

        if let Some(hash) = &mut self.spatial_hashes[pos.node] {
            hash.update(id, &pos.offset, &clamped);
        }

        let new_pos = HybridPos::new(pos.node, clamped);
        self.agent_positions.insert(id, new_pos);
        Ok(new_pos)
    }

    /// Move an agent to a specific 3D position within its current zone.
    ///
    /// If a walkmap is present and the target voxel is blocked, the agent
    /// stays at its current position and [`HybridSpaceError::Blocked`] is returned.
    pub fn move_to_offset(
        &mut self,
        id: AgentId,
        offset: ContinuousPos3D,
    ) -> Result<HybridPos, HybridSpaceError> {
        let pos = self
            .agent_positions
            .get(&id)
            .copied()
            .ok_or(HybridSpaceError::OutOfBounds)?;

        let clamped = self.zones[pos.node].clamp(&offset);

        if !self.is_walkable(pos.node, &clamped) {
            return Err(HybridSpaceError::Blocked);
        }

        if let Some(hash) = &mut self.spatial_hashes[pos.node] {
            hash.update(id, &pos.offset, &clamped);
        }

        let new_pos = HybridPos::new(pos.node, clamped);
        self.agent_positions.insert(id, new_pos);
        Ok(new_pos)
    }

    /// Transition an agent to an adjacent zone along a graph edge.
    ///
    /// The agent is placed at `entry_offset` in the destination zone
    /// (clamped to the zone's bounds). Returns an error if no edge
    /// exists between the current node and `to_node`, or if the
    /// destination voxel is blocked.
    pub fn transition_to_zone(
        &mut self,
        id: AgentId,
        to_node: GraphPos,
        entry_offset: ContinuousPos3D,
    ) -> Result<HybridPos, HybridSpaceError> {
        let pos = self
            .agent_positions
            .get(&id)
            .copied()
            .ok_or(HybridSpaceError::OutOfBounds)?;

        if to_node >= self.num_nodes() {
            return Err(HybridSpaceError::InvalidNode(to_node));
        }

        // Verify edge exists
        let out_neighbors = self.graph.neighbors_out(pos.node);
        if !out_neighbors.contains(&to_node) {
            return Err(HybridSpaceError::NoEdge {
                from: pos.node,
                to: to_node,
            });
        }

        let clamped = self.zones[to_node].clamp(&entry_offset);

        if !self.is_walkable(to_node, &clamped) {
            return Err(HybridSpaceError::Blocked);
        }

        // Remove from old node
        if let Some(i) = self.agents_at_node[pos.node]
            .iter()
            .position(|&aid| aid == id)
        {
            self.agents_at_node[pos.node].swap_remove(i);
        }
        if let Some(hash) = &mut self.spatial_hashes[pos.node] {
            hash.remove(id, &pos.offset);
        }

        // Place in new node
        let new_pos = HybridPos::new(to_node, clamped);
        self.agent_positions.insert(id, new_pos);
        self.agents_at_node[to_node].push(id);
        if let Some(hash) = &mut self.spatial_hashes[to_node] {
            hash.insert(id, &clamped);
        }
        Ok(new_pos)
    }

    /// Transition to an adjacent zone, placing the agent at the center of the
    /// destination zone.
    pub fn transition_to_zone_center(
        &mut self,
        id: AgentId,
        to_node: GraphPos,
    ) -> Result<HybridPos, HybridSpaceError> {
        let center = self
            .zones
            .get(to_node)
            .ok_or(HybridSpaceError::InvalidNode(to_node))?
            .center();
        self.transition_to_zone(id, to_node, center)
    }

    // -- Neighbor queries --

    /// Find all agent IDs within `graph_radius` hops AND within `euclidean_radius`
    /// distance of the query position within each reached zone.
    ///
    /// Semantics:
    /// - for the origin zone, this performs a true Euclidean query
    /// - for reached non-origin zones, this returns all agents in those zones
    /// - no synthetic cross-zone Euclidean metric is imposed by the engine
    ///
    /// Use [`nearby_ids_same_zone`](Self::nearby_ids_same_zone) for geometric
    /// proximity and [`nearby_ids_by_hops`](Self::nearby_ids_by_hops) for purely
    /// topological reachability.
    pub fn nearby_ids(
        &self,
        pos: &HybridPos,
        graph_radius: usize,
        euclidean_radius: f64,
    ) -> Vec<AgentId> {
        let mut result = Vec::new();
        let mut visited = HashSet::new();
        let mut queue = VecDeque::new();

        visited.insert(pos.node);
        queue.push_back((pos.node, 0usize));

        while let Some((node, depth)) = queue.pop_front() {
            if node == pos.node {
                // Same zone: use spatial hash if available, otherwise linear scan
                self.collect_nearby_same_zone(pos, euclidean_radius, &mut result);
            } else {
                // Different zone: include all agents (cross-zone Euclidean
                // filtering is not meaningful without global coordinates).
                result.extend_from_slice(&self.agents_at_node[node]);
            }

            // BFS expansion
            if depth < graph_radius {
                for &neighbor in self.graph.neighbors_out(node) {
                    if visited.insert(neighbor) {
                        queue.push_back((neighbor, depth + 1));
                    }
                }
            }
        }

        result
    }

    /// All agent IDs within `graph_radius` hops (ignoring Euclidean distance).
    pub fn nearby_ids_by_hops(&self, node: GraphPos, graph_radius: usize) -> Vec<AgentId> {
        let mut result = Vec::new();
        let mut visited = HashSet::new();
        let mut queue = VecDeque::new();

        visited.insert(node);
        queue.push_back((node, 0usize));

        while let Some((n, depth)) = queue.pop_front() {
            result.extend_from_slice(&self.agents_at_node[n]);
            if depth < graph_radius {
                for &neighbor in self.graph.neighbors_out(n) {
                    if visited.insert(neighbor) {
                        queue.push_back((neighbor, depth + 1));
                    }
                }
            }
        }

        result
    }

    /// All agent IDs in the same zone within Euclidean distance.
    ///
    /// Uses spatial hashing if enabled for the zone (O(k)), otherwise
    /// falls back to linear scan over all agents in the zone (O(n)).
    pub fn nearby_ids_same_zone(&self, pos: &HybridPos, radius: f64) -> Vec<AgentId> {
        let mut result = Vec::new();
        self.collect_nearby_same_zone(pos, radius, &mut result);
        result
    }

    /// Internal: collect nearby agents in the same zone into `result`.
    fn collect_nearby_same_zone(&self, pos: &HybridPos, radius: f64, result: &mut Vec<AgentId>) {
        if let Some(hash) = self.spatial_hashes.get(pos.node).and_then(|h| h.as_ref()) {
            // O(k) spatial hash query
            let ids = hash.nearby_ids(&pos.offset, radius, &self.agent_positions);
            result.extend(ids);
        } else {
            // O(n) linear scan fallback
            for &aid in &self.agents_at_node[pos.node] {
                if let Some(agent_pos) = self.agent_positions.get(&aid) {
                    if pos.offset.distance_to(&agent_pos.offset) <= radius {
                        result.push(aid);
                    }
                }
            }
        }
    }
}

impl Space for HybridSpace {}

impl<A> SpaceInteraction<A> for HybridSpace
where
    A: PositionedAgent<Position = HybridPos>,
{
    type Error = HybridSpaceError;

    fn random_position<R: rand::RngCore>(&self, rng: &mut R) -> A::Position {
        // Try to find a walkable random position (up to 100 attempts)
        for _ in 0..100 {
            let node = rng.gen_range(0..self.num_nodes());
            let zone = &self.zones[node];
            let offset = ContinuousPos3D::new(
                rng.gen_range(0.0..zone.size_x),
                rng.gen_range(0.0..zone.size_y),
                rng.gen_range(0.0..zone.size_z),
            );
            if self.is_walkable(node, &offset) {
                return HybridPos::new(node, offset);
            }
        }
        // Fallback: return any position (may be blocked)
        let node = rng.gen_range(0..self.num_nodes());
        let zone = &self.zones[node];
        let offset = ContinuousPos3D::new(
            rng.gen_range(0.0..zone.size_x),
            rng.gen_range(0.0..zone.size_y),
            rng.gen_range(0.0..zone.size_z),
        );
        HybridPos::new(node, offset)
    }

    fn add_agent(&mut self, agent: &A) -> Result<(), Self::Error> {
        self.add_agent_internal(agent.id(), *agent.position())
    }

    fn remove_agent(&mut self, agent: &A) -> Result<(), Self::Error> {
        self.remove_agent_internal(agent.id());
        Ok(())
    }

    fn nearby_ids(&self, position: &A::Position, radius: usize) -> Vec<AgentId> {
        // When called via SpaceInteraction, treat radius as graph hops and
        // include all agents at reachable nodes.
        self.nearby_ids_by_hops(position.node, radius)
    }
}