1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
use pyo3::prelude::*;
use std::cmp::Ordering;

#[pyclass]
#[derive(Debug)]
struct Point {
    coords: Vec<f64>,
}

#[pymethods]
impl Point {
    #[new]
    fn new(coords: Vec<f64>) -> Self {
        Point { coords }
    }
}

/// A node in the KDTree
#[derive(Debug)]
struct Node {
    point: Vec<f64>,
    left: Option<Box<Node>>,
    right: Option<Box<Node>>,
}

impl Node {
    fn new(point: Vec<f64>) -> Self {
        Node {
            point,
            left: None,
            right: None,
        }
    }
}

/// KDTree implementation for organizing points in an N-dimensional space
#[pyclass]
#[derive(Debug)]
pub struct KdTree {
    root: Option<Box<Node>>,
    dimensions: usize,
}

#[pymethods]
impl KdTree {
    /// Create a new KDTree with a given number of dimensions
    ///
    /// # Arguments
    ///
    /// * `dimensions` - The number of dimensions for the KDTree
    #[new]
    pub fn new(dimensions: usize) -> Self {
        KdTree {
            root: None,
            dimensions,
        }
    }

    /// Insert a point into the KDTree
    ///
    /// # Arguments
    ///
    /// * `point` - The point to insert into the KDTree
    pub fn insert(&mut self, point: Vec<f64>) {
        let root = self.root.take();
        self.root = self.insert_recursive(root, point, 0);
    }
}

impl KdTree {
    fn insert_recursive(&mut self, node: Option<Box<Node>>, point: Vec<f64>, depth: usize) -> Option<Box<Node>> {
        match node {
            Some(mut n) => {
                let dim = depth % self.dimensions;
                let ordering = point[dim].partial_cmp(&n.point[dim]);

                match ordering {
                    Some(Ordering::Less) | Some(Ordering::Equal) => {
                        n.left = self.insert_recursive(n.left.take(), point, depth + 1);
                    }
                    Some(Ordering::Greater) => {
                        n.right = self.insert_recursive(n.right.take(), point, depth + 1);
                    }
                    None => {
                        // Handle NaNs or other unordered comparisons
                        panic!("NaNs or unordered comparison encountered!");
                    }
                }

                Some(n)
            }
            None => Some(Box::new(Node::new(point))),
        }
    }
}