geographdb_core/algorithms/

four_d.rs

//! 4D graph traversal primitives.
//!
//! These algorithms treat graph topology, 3D position, and temporal validity as
//! co-primary filters. They are intentionally standalone so existing CFG
//! algorithms keep their current API.

use glam::Vec3;
use std::cmp::Ordering;
use std::collections::{BTreeMap, BinaryHeap, HashMap, HashSet, VecDeque};

pub type GraphProperties = BTreeMap<String, serde_json::Value>;

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct TemporalWindow {
    pub start: u64,
    pub end: u64,
}

impl TemporalWindow {
    pub fn overlaps(self, begin_ts: u64, end_ts: u64) -> bool {
        begin_ts < self.end && (end_ts == 0 || end_ts > self.start)
    }
}

#[derive(Debug, Clone, Copy, PartialEq)]
pub enum SpatialRegion {
    Sphere { center: Vec3, radius: f32 },
    Aabb { min: Vec3, max: Vec3 },
}

impl SpatialRegion {
    pub fn contains(self, point: Vec3) -> bool {
        match self {
            SpatialRegion::Sphere { center, radius } => point.distance(center) <= radius,
            SpatialRegion::Aabb { min, max } => {
                point.x >= min.x
                    && point.x <= max.x
                    && point.y >= min.y
                    && point.y <= max.y
                    && point.z >= min.z
                    && point.z <= max.z
            }
        }
    }
}

#[derive(Debug, Clone, PartialEq)]
pub struct TraversalContext4D {
    pub time_window: Option<TemporalWindow>,
    pub spatial_region: Option<SpatialRegion>,
    /// Pre-computed spatial candidates from octree query.
    /// When set, `node_is_valid` uses O(1) set lookup instead of per-node containment.
    pub spatial_candidates: Option<HashSet<u64>>,
    pub graph_weight: f32,
    pub spatial_weight: f32,
    pub temporal_weight: f32,
}

impl Default for TraversalContext4D {
    fn default() -> Self {
        Self {
            time_window: None,
            spatial_region: None,
            spatial_candidates: None,
            graph_weight: 1.0,
            spatial_weight: 0.0,
            temporal_weight: 0.0,
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq)]
pub struct TemporalEdge {
    pub dst: u64,
    pub weight: f32,
    pub begin_ts: u64,
    pub end_ts: u64,
}

#[derive(Debug, Clone, PartialEq)]
pub struct GraphNode4D {
    pub id: u64,
    pub x: f32,
    pub y: f32,
    pub z: f32,
    pub begin_ts: u64,
    pub end_ts: u64,
    pub properties: GraphProperties,
    pub successors: Vec<TemporalEdge>,
}

impl GraphNode4D {
    pub fn position(&self) -> Vec3 {
        Vec3::new(self.x, self.y, self.z)
    }
}

#[derive(Debug, Clone, PartialEq)]
pub struct GraphPath4D {
    pub node_ids: Vec<u64>,
    pub total_cost: f32,
    pub heuristic_cost: f32,
    pub actual_cost: f32,
}

#[derive(Debug, Clone, PartialEq, Eq)]
pub struct TemporalJourney4D {
    pub node_ids: Vec<u64>,
    pub departure_time: u64,
    pub arrival_time: u64,
    pub duration: u64,
}

#[derive(Debug, Clone, PartialEq)]
pub struct TemporalArrival4D {
    pub node_id: u64,
    pub arrival_time: u64,
    pub cost: f32,
    pub path: Vec<u64>,
}

#[derive(Debug, Clone, PartialEq)]
pub struct TemporalDijkstraResult4D {
    pub start_node: u64,
    pub departure_time: u64,
    pub reachable: Vec<TemporalArrival4D>,
    pub unreachable: Vec<u64>,
}

#[derive(Debug, Clone, Copy, PartialEq)]
struct QueueNode4D {
    node_id: u64,
    f_score: f32,
    g_score: f32,
}

impl Eq for QueueNode4D {}

impl Ord for QueueNode4D {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .f_score
            .partial_cmp(&self.f_score)
            .unwrap_or(Ordering::Equal)
    }
}

impl PartialOrd for QueueNode4D {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

fn node_is_valid(node: &GraphNode4D, ctx: &TraversalContext4D) -> bool {
    if let Some(window) = ctx.time_window {
        if !window.overlaps(node.begin_ts, node.end_ts) {
            return false;
        }
    }

    match (&ctx.spatial_candidates, ctx.spatial_region) {
        (Some(candidates), _) => {
            if !candidates.contains(&node.id) {
                return false;
            }
        }
        (None, Some(region)) => {
            if !region.contains(node.position()) {
                return false;
            }
        }
        (None, None) => {}
    }

    true
}

fn edge_is_valid(edge: TemporalEdge, ctx: &TraversalContext4D) -> bool {
    ctx.time_window
        .map(|window| window.overlaps(edge.begin_ts, edge.end_ts))
        .unwrap_or(true)
}

fn instant_in_validity(ts: u64, begin_ts: u64, end_ts: u64) -> bool {
    ts >= begin_ts && (end_ts == 0 || ts <= end_ts)
}

fn traversal_duration(edge: TemporalEdge) -> Option<u64> {
    if !edge.weight.is_finite() || edge.weight < 0.0 {
        return None;
    }
    Some(edge.weight.ceil() as u64)
}

fn edge_departure_and_arrival(edge: TemporalEdge, current_time: u64) -> Option<(u64, u64)> {
    let departure = current_time.max(edge.begin_ts);
    if edge.end_ts != 0 && departure > edge.end_ts {
        return None;
    }

    let arrival = departure.checked_add(traversal_duration(edge)?)?;
    if edge.end_ts != 0 && arrival > edge.end_ts {
        return None;
    }

    Some((departure, arrival))
}

fn temporal_node_is_usable(
    node: &GraphNode4D,
    arrival_time: u64,
    region: Option<SpatialRegion>,
) -> bool {
    instant_in_validity(arrival_time, node.begin_ts, node.end_ts)
        && region
            .map(|spatial_region| spatial_region.contains(node.position()))
            .unwrap_or(true)
}

fn temporal_delay(edge: TemporalEdge, ctx: &TraversalContext4D) -> f32 {
    match ctx.time_window {
        Some(window) if edge.begin_ts > window.start => (edge.begin_ts - window.start) as f32,
        _ => 0.0,
    }
}

fn edge_cost(
    current: &GraphNode4D,
    next: &GraphNode4D,
    edge: TemporalEdge,
    ctx: &TraversalContext4D,
) -> f32 {
    let graph = ctx.graph_weight * edge.weight.max(0.0);
    let spatial = ctx.spatial_weight * current.position().distance(next.position());
    let temporal = ctx.temporal_weight * temporal_delay(edge, ctx);
    graph + spatial + temporal
}

fn heuristic(current: &GraphNode4D, goal: &GraphNode4D, ctx: &TraversalContext4D) -> f32 {
    ctx.spatial_weight * current.position().distance(goal.position())
}

pub fn reachable_4d(nodes: &[GraphNode4D], start_id: u64, ctx: &TraversalContext4D) -> Vec<u64> {
    let node_map: HashMap<u64, &GraphNode4D> = nodes.iter().map(|n| (n.id, n)).collect();
    let Some(start) = node_map.get(&start_id) else {
        return Vec::new();
    };
    if !node_is_valid(start, ctx) {
        return Vec::new();
    }

    let mut visited = HashSet::new();
    let mut queue = VecDeque::from([start_id]);
    let mut result = Vec::new();

    while let Some(current_id) = queue.pop_front() {
        if !visited.insert(current_id) {
            continue;
        }
        let Some(current) = node_map.get(&current_id) else {
            continue;
        };
        if !node_is_valid(current, ctx) {
            continue;
        }

        result.push(current_id);

        for edge in &current.successors {
            if !edge_is_valid(*edge, ctx) || visited.contains(&edge.dst) {
                continue;
            }
            if let Some(next) = node_map.get(&edge.dst) {
                if node_is_valid(next, ctx) {
                    queue.push_back(edge.dst);
                }
            }
        }
    }

    result
}

pub fn astar_find_path_4d(
    nodes: &[GraphNode4D],
    start_id: u64,
    goal_id: u64,
    ctx: &TraversalContext4D,
) -> Option<GraphPath4D> {
    let node_map: HashMap<u64, &GraphNode4D> = nodes.iter().map(|n| (n.id, n)).collect();
    let start = *node_map.get(&start_id)?;
    let goal = *node_map.get(&goal_id)?;

    if !node_is_valid(start, ctx) || !node_is_valid(goal, ctx) {
        return None;
    }

    let initial_h = heuristic(start, goal, ctx);
    let mut open_set = BinaryHeap::from([QueueNode4D {
        node_id: start_id,
        f_score: initial_h,
        g_score: 0.0,
    }]);
    let mut closed = HashSet::new();
    let mut g_score = HashMap::from([(start_id, 0.0)]);
    let mut came_from = HashMap::new();

    while let Some(current) = open_set.pop() {
        if !closed.insert(current.node_id) {
            continue;
        }

        if current.node_id == goal_id {
            let mut path = vec![goal_id];
            let mut cursor = goal_id;
            while let Some(prev) = came_from.get(&cursor).copied() {
                path.push(prev);
                cursor = prev;
            }
            path.reverse();
            return Some(GraphPath4D {
                node_ids: path,
                total_cost: current.f_score,
                heuristic_cost: initial_h,
                actual_cost: current.g_score,
            });
        }

        let current_node = *node_map.get(&current.node_id)?;
        for edge in &current_node.successors {
            if !edge_is_valid(*edge, ctx) || closed.contains(&edge.dst) {
                continue;
            }
            let Some(next) = node_map.get(&edge.dst).copied() else {
                continue;
            };
            if !node_is_valid(next, ctx) {
                continue;
            }

            let tentative_g = current.g_score + edge_cost(current_node, next, *edge, ctx);
            let existing_g = g_score.get(&edge.dst).copied().unwrap_or(f32::INFINITY);
            if tentative_g < existing_g {
                came_from.insert(edge.dst, current.node_id);
                g_score.insert(edge.dst, tentative_g);
                open_set.push(QueueNode4D {
                    node_id: edge.dst,
                    f_score: tentative_g + heuristic(next, goal, ctx),
                    g_score: tentative_g,
                });
            }
        }
    }

    None
}

pub fn strongly_connected_components_4d(
    nodes: &[GraphNode4D],
    ctx: &TraversalContext4D,
) -> Vec<Vec<u64>> {
    struct Tarjan<'a> {
        nodes: HashMap<u64, &'a GraphNode4D>,
        ctx: &'a TraversalContext4D,
        index: usize,
        stack: Vec<u64>,
        on_stack: HashSet<u64>,
        indices: HashMap<u64, usize>,
        lowlinks: HashMap<u64, usize>,
        components: Vec<Vec<u64>>,
    }

    impl<'a> Tarjan<'a> {
        fn strong_connect(&mut self, node_id: u64) {
            self.indices.insert(node_id, self.index);
            self.lowlinks.insert(node_id, self.index);
            self.index += 1;
            self.stack.push(node_id);
            self.on_stack.insert(node_id);

            let Some(node) = self.nodes.get(&node_id) else {
                return;
            };
            for edge in &node.successors {
                if !edge_is_valid(*edge, self.ctx) {
                    continue;
                }
                let Some(next) = self.nodes.get(&edge.dst).copied() else {
                    continue;
                };
                if !node_is_valid(next, self.ctx) {
                    continue;
                }

                if !self.indices.contains_key(&edge.dst) {
                    self.strong_connect(edge.dst);
                    let low = self.lowlinks[&node_id].min(self.lowlinks[&edge.dst]);
                    self.lowlinks.insert(node_id, low);
                } else if self.on_stack.contains(&edge.dst) {
                    let low = self.lowlinks[&node_id].min(self.indices[&edge.dst]);
                    self.lowlinks.insert(node_id, low);
                }
            }

            if self.indices[&node_id] == self.lowlinks[&node_id] {
                let mut component = Vec::new();
                while let Some(member) = self.stack.pop() {
                    self.on_stack.remove(&member);
                    component.push(member);
                    if member == node_id {
                        break;
                    }
                }
                component.sort_unstable();
                self.components.push(component);
            }
        }
    }

    let node_map: HashMap<u64, &GraphNode4D> = nodes
        .iter()
        .filter(|node| node_is_valid(node, ctx))
        .map(|node| (node.id, node))
        .collect();
    let mut ids: Vec<u64> = node_map.keys().copied().collect();
    ids.sort_unstable();

    let mut tarjan = Tarjan {
        nodes: node_map,
        ctx,
        index: 0,
        stack: Vec::new(),
        on_stack: HashSet::new(),
        indices: HashMap::new(),
        lowlinks: HashMap::new(),
        components: Vec::new(),
    };

    for id in ids {
        if !tarjan.indices.contains_key(&id) {
            tarjan.strong_connect(id);
        }
    }

    tarjan.components.sort_by_key(|component| component[0]);
    tarjan.components
}

pub fn earliest_arrival_path_4d(
    nodes: &[GraphNode4D],
    start_id: u64,
    goal_id: u64,
    departure_time: u64,
    spatial_region: Option<SpatialRegion>,
) -> Option<TemporalJourney4D> {
    let node_map: HashMap<u64, &GraphNode4D> = nodes.iter().map(|node| (node.id, node)).collect();
    let start = *node_map.get(&start_id)?;
    if !temporal_node_is_usable(start, departure_time, spatial_region) {
        return None;
    }

    let mut best_arrival = HashMap::from([(start_id, departure_time)]);
    let mut came_from = HashMap::new();
    let mut queue = BinaryHeap::from([TemporalQueueNode {
        node_id: start_id,
        arrival_time: departure_time,
    }]);

    while let Some(current) = queue.pop() {
        if current.arrival_time > best_arrival[&current.node_id] {
            continue;
        }
        if current.node_id == goal_id {
            return Some(reconstruct_temporal_journey(
                &came_from,
                start_id,
                goal_id,
                departure_time,
                current.arrival_time,
            ));
        }

        let current_node = *node_map.get(&current.node_id)?;
        for edge in &current_node.successors {
            let Some(next) = node_map.get(&edge.dst).copied() else {
                continue;
            };
            let Some((_, arrival_time)) = edge_departure_and_arrival(*edge, current.arrival_time)
            else {
                continue;
            };
            if !temporal_node_is_usable(next, arrival_time, spatial_region) {
                continue;
            }

            if arrival_time < best_arrival.get(&edge.dst).copied().unwrap_or(u64::MAX) {
                best_arrival.insert(edge.dst, arrival_time);
                came_from.insert(edge.dst, current.node_id);
                queue.push(TemporalQueueNode {
                    node_id: edge.dst,
                    arrival_time,
                });
            }
        }
    }

    None
}

pub fn fastest_temporal_path_4d(
    nodes: &[GraphNode4D],
    start_id: u64,
    goal_id: u64,
    earliest_departure: u64,
    latest_departure: u64,
    spatial_region: Option<SpatialRegion>,
) -> Option<TemporalJourney4D> {
    const MAX_WINDOW: u64 = 100_000;
    if earliest_departure > latest_departure {
        return None;
    }
    if latest_departure.saturating_sub(earliest_departure) > MAX_WINDOW {
        return None;
    }

    let mut best: Option<TemporalJourney4D> = None;

    for departure_time in earliest_departure..=latest_departure {
        let Some(journey) =
            earliest_arrival_path_4d(nodes, start_id, goal_id, departure_time, spatial_region)
        else {
            continue;
        };

        let should_replace = best
            .as_ref()
            .map(|current| {
                journey.duration < current.duration
                    || (journey.duration == current.duration
                        && journey.arrival_time < current.arrival_time)
            })
            .unwrap_or(true);
        if should_replace {
            best = Some(journey);
        }
    }

    best
}

pub fn articulation_points_4d(nodes: &[GraphNode4D], ctx: &TraversalContext4D) -> Vec<u64> {
    let adjacency = active_undirected_adjacency(nodes, ctx);
    let mut ids: Vec<u64> = adjacency.keys().copied().collect();
    ids.sort_unstable();

    let mut finder = LowLinkFinder {
        adjacency: &adjacency,
        time: 0,
        visited: HashSet::new(),
        discovery: HashMap::new(),
        low: HashMap::new(),
        parent: HashMap::new(),
        articulation_points: HashSet::new(),
        bridges: Vec::new(),
    };

    for id in ids {
        if !finder.visited.contains(&id) {
            finder.visit(id);
        }
    }

    let mut points: Vec<u64> = finder.articulation_points.into_iter().collect();
    points.sort_unstable();
    points
}

pub fn bridges_4d(nodes: &[GraphNode4D], ctx: &TraversalContext4D) -> Vec<(u64, u64)> {
    let adjacency = active_undirected_adjacency(nodes, ctx);
    let mut ids: Vec<u64> = adjacency.keys().copied().collect();
    ids.sort_unstable();

    let mut finder = LowLinkFinder {
        adjacency: &adjacency,
        time: 0,
        visited: HashSet::new(),
        discovery: HashMap::new(),
        low: HashMap::new(),
        parent: HashMap::new(),
        articulation_points: HashSet::new(),
        bridges: Vec::new(),
    };

    for id in ids {
        if !finder.visited.contains(&id) {
            finder.visit(id);
        }
    }

    finder.bridges.sort_unstable();
    finder.bridges
}

pub fn time_dependent_dijkstra_4d(
    nodes: &[GraphNode4D],
    start_id: u64,
    departure_time: u64,
    spatial_region: Option<SpatialRegion>,
) -> Option<TemporalDijkstraResult4D> {
    let node_map: HashMap<u64, &GraphNode4D> = nodes
        .iter()
        .filter(|node| {
            spatial_region
                .map(|region| region.contains(node.position()))
                .unwrap_or(true)
        })
        .map(|node| (node.id, node))
        .collect();
    let start = *node_map.get(&start_id)?;
    if !instant_in_validity(departure_time, start.begin_ts, start.end_ts) {
        return None;
    }

    let mut best_arrival = HashMap::from([(start_id, departure_time)]);
    let mut came_from = HashMap::new();
    let mut queue = BinaryHeap::from([TemporalQueueNode {
        node_id: start_id,
        arrival_time: departure_time,
    }]);

    while let Some(current) = queue.pop() {
        if current.arrival_time > best_arrival[&current.node_id] {
            continue;
        }

        let current_node = *node_map.get(&current.node_id)?;
        for edge in &current_node.successors {
            let Some(next) = node_map.get(&edge.dst).copied() else {
                continue;
            };
            let Some((_, arrival_time)) = edge_departure_and_arrival(*edge, current.arrival_time)
            else {
                continue;
            };
            if !instant_in_validity(arrival_time, next.begin_ts, next.end_ts) {
                continue;
            }

            if arrival_time < best_arrival.get(&edge.dst).copied().unwrap_or(u64::MAX) {
                best_arrival.insert(edge.dst, arrival_time);
                came_from.insert(edge.dst, current.node_id);
                queue.push(TemporalQueueNode {
                    node_id: edge.dst,
                    arrival_time,
                });
            }
        }
    }

    let mut reachable: Vec<TemporalArrival4D> = best_arrival
        .iter()
        .map(|(node_id, arrival_time)| TemporalArrival4D {
            node_id: *node_id,
            arrival_time: *arrival_time,
            cost: arrival_time.saturating_sub(departure_time) as f32,
            path: reconstruct_node_path(&came_from, start_id, *node_id),
        })
        .collect();
    reachable.sort_by_key(|arrival| (arrival.arrival_time, arrival.node_id));

    let mut unreachable: Vec<u64> = node_map
        .keys()
        .copied()
        .filter(|id| !best_arrival.contains_key(id))
        .collect();
    unreachable.sort_unstable();

    Some(TemporalDijkstraResult4D {
        start_node: start_id,
        departure_time,
        reachable,
        unreachable,
    })
}

fn active_undirected_adjacency(
    nodes: &[GraphNode4D],
    ctx: &TraversalContext4D,
) -> HashMap<u64, Vec<u64>> {
    let node_map: HashMap<u64, &GraphNode4D> = nodes
        .iter()
        .filter(|node| node_is_valid(node, ctx))
        .map(|node| (node.id, node))
        .collect();
    let mut adjacency: HashMap<u64, Vec<u64>> = node_map
        .keys()
        .copied()
        .map(|id| (id, Vec::new()))
        .collect();

    for node in node_map.values() {
        for edge in &node.successors {
            if !edge_is_valid(*edge, ctx) || !node_map.contains_key(&edge.dst) {
                continue;
            }
            add_undirected_edge(&mut adjacency, node.id, edge.dst);
        }
    }

    for neighbors in adjacency.values_mut() {
        neighbors.sort_unstable();
        neighbors.dedup();
    }

    adjacency
}

fn add_undirected_edge(adjacency: &mut HashMap<u64, Vec<u64>>, a: u64, b: u64) {
    if a == b {
        return;
    }
    adjacency.entry(a).or_default().push(b);
    adjacency.entry(b).or_default().push(a);
}

struct LowLinkFinder<'a> {
    adjacency: &'a HashMap<u64, Vec<u64>>,
    time: usize,
    visited: HashSet<u64>,
    discovery: HashMap<u64, usize>,
    low: HashMap<u64, usize>,
    parent: HashMap<u64, u64>,
    articulation_points: HashSet<u64>,
    bridges: Vec<(u64, u64)>,
}

impl LowLinkFinder<'_> {
    fn visit(&mut self, node_id: u64) {
        self.visited.insert(node_id);
        self.discovery.insert(node_id, self.time);
        self.low.insert(node_id, self.time);
        self.time += 1;

        let mut child_count = 0;
        let neighbors = self.adjacency.get(&node_id).cloned().unwrap_or_default();
        for next_id in neighbors {
            if !self.visited.contains(&next_id) {
                child_count += 1;
                self.parent.insert(next_id, node_id);
                self.visit(next_id);

                let next_low = self.low[&next_id];
                let node_low = self.low[&node_id].min(next_low);
                self.low.insert(node_id, node_low);

                let is_root = !self.parent.contains_key(&node_id);
                if is_root && child_count > 1 {
                    self.articulation_points.insert(node_id);
                }
                if !is_root && next_low >= self.discovery[&node_id] {
                    self.articulation_points.insert(node_id);
                }
                if next_low > self.discovery[&node_id] {
                    self.bridges
                        .push((node_id.min(next_id), node_id.max(next_id)));
                }
            } else if self.parent.get(&node_id).copied() != Some(next_id) {
                let node_low = self.low[&node_id].min(self.discovery[&next_id]);
                self.low.insert(node_id, node_low);
            }
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
struct TemporalQueueNode {
    node_id: u64,
    arrival_time: u64,
}

impl Ord for TemporalQueueNode {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .arrival_time
            .cmp(&self.arrival_time)
            .then_with(|| other.node_id.cmp(&self.node_id))
    }
}

impl PartialOrd for TemporalQueueNode {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

fn reconstruct_temporal_journey(
    came_from: &HashMap<u64, u64>,
    start_id: u64,
    goal_id: u64,
    departure_time: u64,
    arrival_time: u64,
) -> TemporalJourney4D {
    let mut node_ids = vec![goal_id];
    let mut cursor = goal_id;
    while cursor != start_id {
        let Some(prev) = came_from.get(&cursor).copied() else {
            break;
        };
        node_ids.push(prev);
        cursor = prev;
    }
    node_ids.reverse();

    TemporalJourney4D {
        node_ids,
        departure_time,
        arrival_time,
        duration: arrival_time.saturating_sub(departure_time),
    }
}

fn reconstruct_node_path(came_from: &HashMap<u64, u64>, start_id: u64, target_id: u64) -> Vec<u64> {
    let mut node_ids = vec![target_id];
    let mut cursor = target_id;
    while cursor != start_id {
        let Some(prev) = came_from.get(&cursor).copied() else {
            break;
        };
        node_ids.push(prev);
        cursor = prev;
    }
    node_ids.reverse();
    node_ids
}

/// Spatial index wrapping an octree for pre-filtering graph traversal candidates.
///
/// Build once from the node array, reuse across multiple algorithm calls.
/// Provides O(log n) spatial queries instead of O(n) brute-force containment checks.
#[derive(Debug, Clone)]
pub struct SpatialIndex {
    octree: crate::spatial::octree::Octree,
}

impl SpatialIndex {
    /// Build a spatial index from a slice of graph nodes.
    pub fn from_nodes(nodes: &[GraphNode4D]) -> Self {
        use crate::storage::data_structures::NodePoint;
        let points: Vec<NodePoint> = nodes
            .iter()
            .map(|n| NodePoint {
                id: n.id,
                x: n.x,
                y: n.y,
                z: n.z,
            })
            .collect();
        SpatialIndex {
            octree: crate::spatial::octree::Octree::from_nodes(&points),
        }
    }

    /// Pre-filter node IDs matching the spatial region using the octree.
    ///
    /// Returns a HashSet of node IDs that fall within the region.
    /// Use this to populate `TraversalContext4D::spatial_candidates`.
    pub fn prefilter(&self, region: &SpatialRegion) -> HashSet<u64> {
        use crate::spatial::octree::BoundingBox;
        match region {
            SpatialRegion::Sphere { center, radius } => self
                .octree
                .query_sphere(*center, *radius)
                .into_iter()
                .map(|np| np.id)
                .collect(),
            SpatialRegion::Aabb { min, max } => {
                let bbox = BoundingBox::new(*min, *max);
                self.octree
                    .query_aabb(&bbox)
                    .into_iter()
                    .map(|np| np.id)
                    .collect()
            }
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    fn make_test_nodes() -> Vec<GraphNode4D> {
        vec![
            GraphNode4D {
                id: 1,
                x: 0.0,
                y: 0.0,
                z: 0.0,
                begin_ts: 0,
                end_ts: 0,
                properties: GraphProperties::new(),
                successors: vec![TemporalEdge {
                    dst: 2,
                    weight: 1.0,
                    begin_ts: 0,
                    end_ts: 0,
                }],
            },
            GraphNode4D {
                id: 2,
                x: 3.0,
                y: 0.0,
                z: 0.0,
                begin_ts: 0,
                end_ts: 0,
                properties: GraphProperties::new(),
                successors: vec![TemporalEdge {
                    dst: 3,
                    weight: 1.0,
                    begin_ts: 0,
                    end_ts: 0,
                }],
            },
            GraphNode4D {
                id: 3,
                x: 10.0,
                y: 10.0,
                z: 10.0,
                begin_ts: 0,
                end_ts: 0,
                properties: GraphProperties::new(),
                successors: vec![],
            },
            GraphNode4D {
                id: 4,
                x: 50.0,
                y: 50.0,
                z: 50.0,
                begin_ts: 0,
                end_ts: 0,
                properties: GraphProperties::new(),
                successors: vec![],
            },
        ]
    }

    #[test]
    fn test_spatial_index_prefilter_sphere() {
        let nodes = make_test_nodes();
        let index = SpatialIndex::from_nodes(&nodes);
        let region = SpatialRegion::Sphere {
            center: Vec3::new(0.0, 0.0, 0.0),
            radius: 5.0,
        };
        let candidates = index.prefilter(&region);

        // Nodes 1 (0,0,0) and 2 (3,0,0) are within radius 5
        // Node 3 (10,10,10) distance ~17.3 and node 4 (50,50,50) distance ~86.6 are outside
        assert!(
            candidates.contains(&1),
            "node 1 at origin should be in sphere"
        );
        assert!(
            candidates.contains(&2),
            "node 2 at (3,0,0) should be in sphere"
        );
        assert!(
            !candidates.contains(&3),
            "node 3 at (10,10,10) should be outside sphere"
        );
        assert!(
            !candidates.contains(&4),
            "node 4 at (50,50,50) should be outside sphere"
        );
    }

    #[test]
    fn test_spatial_index_prefilter_aabb() {
        let nodes = make_test_nodes();
        let index = SpatialIndex::from_nodes(&nodes);
        let region = SpatialRegion::Aabb {
            min: Vec3::new(-1.0, -1.0, -1.0),
            max: Vec3::new(11.0, 11.0, 11.0),
        };
        let candidates = index.prefilter(&region);

        // Nodes 1, 2, 3 are within the AABB. Node 4 at (50,50,50) is outside.
        assert!(candidates.contains(&1), "node 1 should be in AABB");
        assert!(candidates.contains(&2), "node 2 should be in AABB");
        assert!(candidates.contains(&3), "node 3 should be in AABB");
        assert!(
            !candidates.contains(&4),
            "node 4 at (50,50,50) should be outside AABB"
        );
    }

    #[test]
    fn test_prefilter_matches_manual_containment() {
        let nodes = make_test_nodes();
        let index = SpatialIndex::from_nodes(&nodes);
        let region = SpatialRegion::Sphere {
            center: Vec3::new(0.0, 0.0, 0.0),
            radius: 12.0,
        };

        let candidates = index.prefilter(&region);

        // Verify against manual containment check
        for node in &nodes {
            let in_region = region.contains(node.position());
            let in_candidates = candidates.contains(&node.id);
            assert_eq!(
                in_region, in_candidates,
                "Mismatch for node {}: manual={} candidates={}",
                node.id, in_region, in_candidates
            );
        }
    }

    #[test]
    fn test_node_is_valid_with_candidates_matches_without() {
        let nodes = make_test_nodes();
        let index = SpatialIndex::from_nodes(&nodes);
        let region = SpatialRegion::Sphere {
            center: Vec3::new(0.0, 0.0, 0.0),
            radius: 5.0,
        };

        let ctx_plain = TraversalContext4D {
            spatial_region: Some(region),
            ..Default::default()
        };
        let ctx_indexed = TraversalContext4D {
            spatial_region: Some(region),
            spatial_candidates: Some(index.prefilter(&region)),
            ..Default::default()
        };

        for node in &nodes {
            assert_eq!(
                node_is_valid(node, &ctx_plain),
                node_is_valid(node, &ctx_indexed),
                "Mismatch for node {}: plain vs indexed",
                node.id
            );
        }
    }

    #[test]
    fn test_reachable_4d_with_octree_matches_without() {
        let nodes = make_test_nodes();
        let index = SpatialIndex::from_nodes(&nodes);
        let region = SpatialRegion::Sphere {
            center: Vec3::new(0.0, 0.0, 0.0),
            radius: 5.0,
        };

        let ctx_plain = TraversalContext4D {
            spatial_region: Some(region),
            ..Default::default()
        };
        let ctx_indexed = TraversalContext4D {
            spatial_region: Some(region),
            spatial_candidates: Some(index.prefilter(&region)),
            ..Default::default()
        };

        let result_plain = reachable_4d(&nodes, 1, &ctx_plain);
        let result_indexed = reachable_4d(&nodes, 1, &ctx_indexed);

        assert_eq!(
            result_plain, result_indexed,
            "reachable_4d should produce same results with and without octree"
        );
    }

    #[test]
    fn test_astar_with_octree_matches_without() {
        let nodes = make_test_nodes();
        let index = SpatialIndex::from_nodes(&nodes);
        let region = SpatialRegion::Sphere {
            center: Vec3::new(0.0, 0.0, 0.0),
            radius: 15.0,
        };

        let ctx_plain = TraversalContext4D {
            spatial_region: Some(region),
            ..Default::default()
        };
        let ctx_indexed = TraversalContext4D {
            spatial_region: Some(region),
            spatial_candidates: Some(index.prefilter(&region)),
            ..Default::default()
        };

        let result_plain = astar_find_path_4d(&nodes, 1, 3, &ctx_plain);
        let result_indexed = astar_find_path_4d(&nodes, 1, 3, &ctx_indexed);

        assert_eq!(
            result_plain, result_indexed,
            "astar should produce same results with and without octree"
        );
    }

    #[test]
    fn test_no_spatial_region_no_candidates_still_works() {
        let nodes = make_test_nodes();
        let ctx = TraversalContext4D::default();

        // Without spatial_region, all nodes are valid
        for node in &nodes {
            assert!(
                node_is_valid(node, &ctx),
                "node {} should be valid without spatial constraints",
                node.id
            );
        }
    }
}