use crate::models::Node;
use std::collections::HashMap;
use std::sync::{Arc, RwLock};
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Coordinate {
pub x: f32, pub y: f32, pub z: f32, }
impl Coordinate {
pub fn distance(&self, other: &Coordinate) -> f32 {
((self.x - other.x).powi(2) + (self.y - other.y).powi(2) + (self.z - other.z).powi(2))
.sqrt()
}
}
#[derive(Clone, Default)]
pub struct SpatialMemoryStore {
grid: Arc<RwLock<HashMap<String, (Node, Coordinate)>>>,
}
impl SpatialMemoryStore {
pub fn new() -> Self {
Self {
grid: Arc::new(RwLock::new(HashMap::new())),
}
}
pub fn insert(&self, node: Node, coord: Coordinate) {
if let Ok(mut grid) = self.grid.write() {
grid.insert(node.id.clone(), (node, coord));
}
}
pub fn search_nearest(&self, center: Coordinate, k: usize) -> Vec<(Node, f32)> {
let grid = match self.grid.read() {
Ok(g) => g,
Err(_) => return vec![],
};
let mut distances: Vec<(Node, f32)> = grid
.values()
.map(|(node, coord)| (node.clone(), center.distance(coord)))
.collect();
distances.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap_or(std::cmp::Ordering::Equal));
distances.truncate(k);
distances
}
}