1use alloc::format;
18use alloc::vec::Vec;
19
20use crate::domain::{BoundingBox, Cut};
21use crate::error::{RcfError, RcfResult};
22use crate::tree::node::{InternalData, LeafData, NodeRef, NodeView, NodeViewMut};
23
24#[derive(Debug)]
38#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
39pub struct NodeStore<const D: usize> {
40 internals: Vec<Option<InternalData<D>>>,
45 leaves: Vec<Option<LeafData>>,
49 internal_free: Vec<u32>,
51 leaf_free: Vec<u32>,
53 capacity: u32,
55}
56
57impl<const D: usize> NodeStore<D> {
58 pub fn new(capacity: u32) -> RcfResult<Self> {
66 if capacity == 0 {
67 return Err(RcfError::InvalidConfig(
68 "NodeStore capacity must be > 0".into(),
69 ));
70 }
71 if capacity > NodeRef::MAX_INDEX {
72 return Err(RcfError::InvalidConfig(
73 format!(
74 "NodeStore capacity {capacity} exceeds NodeRef::MAX_INDEX {}",
75 NodeRef::MAX_INDEX
76 )
77 .into(),
78 ));
79 }
80 let cap = capacity as usize;
81 let internals = (0..cap).map(|_| None).collect();
82 let leaves = (0..cap).map(|_| None).collect();
83 let internal_free = (0..capacity).rev().collect();
86 let leaf_free = (0..capacity).rev().collect();
87 Ok(Self {
88 internals,
89 leaves,
90 internal_free,
91 leaf_free,
92 capacity,
93 })
94 }
95
96 #[must_use]
98 pub fn capacity(&self) -> u32 {
99 self.capacity
100 }
101
102 #[must_use]
104 pub fn live_count(&self) -> usize {
105 self.live_internal_count() + self.live_leaf_count()
106 }
107
108 #[must_use]
110 pub fn live_internal_count(&self) -> usize {
111 self.capacity as usize - self.internal_free.len()
112 }
113
114 #[must_use]
116 pub fn live_leaf_count(&self) -> usize {
117 self.capacity as usize - self.leaf_free.len()
118 }
119
120 pub fn add_internal(
127 &mut self,
128 cut: Cut,
129 bbox: BoundingBox<D>,
130 left: NodeRef,
131 right: NodeRef,
132 parent: Option<NodeRef>,
133 mass: u64,
134 ) -> RcfResult<NodeRef> {
135 let idx = self
136 .internal_free
137 .pop()
138 .ok_or_else(|| RcfError::InvalidConfig("NodeStore internal arena exhausted".into()))?;
139 self.internals[idx as usize] = Some(InternalData {
140 cut,
141 bbox,
142 left,
143 right,
144 parent,
145 mass,
146 });
147 Ok(NodeRef::internal(idx))
148 }
149
150 pub fn add_leaf(
157 &mut self,
158 point_idx: usize,
159 parent: Option<NodeRef>,
160 mass: u64,
161 ) -> RcfResult<NodeRef> {
162 let idx = self
163 .leaf_free
164 .pop()
165 .ok_or_else(|| RcfError::InvalidConfig("NodeStore leaf arena exhausted".into()))?;
166 self.leaves[idx as usize] = Some(LeafData {
167 point_idx,
168 parent,
169 mass,
170 });
171 Ok(NodeRef::leaf(idx))
172 }
173
174 pub fn delete(&mut self, n: NodeRef) -> RcfResult<()> {
182 let idx = n.index();
183 if idx >= self.capacity as usize {
184 return Err(RcfError::OutOfBounds {
185 index: idx,
186 len: self.capacity as usize,
187 });
188 }
189 let was_live = if n.is_leaf() {
190 self.leaves[idx].take().is_some()
191 } else {
192 self.internals[idx].take().is_some()
193 };
194 if !was_live {
195 return Err(RcfError::OutOfBounds {
196 index: idx,
197 len: self.capacity as usize,
198 });
199 }
200 if n.is_leaf() {
201 #[allow(clippy::cast_possible_truncation)]
202 self.leaf_free.push(idx as u32);
203 } else {
204 #[allow(clippy::cast_possible_truncation)]
205 self.internal_free.push(idx as u32);
206 }
207 Ok(())
208 }
209
210 pub fn view(&self, n: NodeRef) -> RcfResult<NodeView<'_, D>> {
219 let idx = n.index();
220 if idx >= self.capacity as usize {
221 return Err(RcfError::OutOfBounds {
222 index: idx,
223 len: self.capacity as usize,
224 });
225 }
226 if n.is_leaf() {
227 self.leaves[idx]
228 .as_ref()
229 .map(NodeView::Leaf)
230 .ok_or(RcfError::OutOfBounds {
231 index: idx,
232 len: self.capacity as usize,
233 })
234 } else {
235 self.internals[idx]
236 .as_ref()
237 .map(NodeView::Internal)
238 .ok_or(RcfError::OutOfBounds {
239 index: idx,
240 len: self.capacity as usize,
241 })
242 }
243 }
244
245 pub fn view_mut(&mut self, n: NodeRef) -> RcfResult<NodeViewMut<'_, D>> {
252 let idx = n.index();
253 if idx >= self.capacity as usize {
254 return Err(RcfError::OutOfBounds {
255 index: idx,
256 len: self.capacity as usize,
257 });
258 }
259 if n.is_leaf() {
260 self.leaves[idx]
261 .as_mut()
262 .map(NodeViewMut::Leaf)
263 .ok_or(RcfError::OutOfBounds {
264 index: idx,
265 len: self.capacity as usize,
266 })
267 } else {
268 self.internals[idx]
269 .as_mut()
270 .map(NodeViewMut::Internal)
271 .ok_or(RcfError::OutOfBounds {
272 index: idx,
273 len: self.capacity as usize,
274 })
275 }
276 }
277
278 pub fn internal(&self, n: NodeRef) -> RcfResult<&InternalData<D>> {
287 if n.is_leaf() {
288 return Err(RcfError::InvalidConfig(
289 "NodeStore::internal: called on a leaf reference".into(),
290 ));
291 }
292 let idx = n.index();
293 if idx >= self.capacity as usize {
294 return Err(RcfError::OutOfBounds {
295 index: idx,
296 len: self.capacity as usize,
297 });
298 }
299 self.internals[idx].as_ref().ok_or(RcfError::OutOfBounds {
300 index: idx,
301 len: self.capacity as usize,
302 })
303 }
304
305 pub fn internal_mut(&mut self, n: NodeRef) -> RcfResult<&mut InternalData<D>> {
312 if n.is_leaf() {
313 return Err(RcfError::InvalidConfig(
314 "NodeStore::internal_mut: called on a leaf reference".into(),
315 ));
316 }
317 let idx = n.index();
318 if idx >= self.capacity as usize {
319 return Err(RcfError::OutOfBounds {
320 index: idx,
321 len: self.capacity as usize,
322 });
323 }
324 self.internals[idx].as_mut().ok_or(RcfError::OutOfBounds {
325 index: idx,
326 len: self.capacity as usize,
327 })
328 }
329
330 pub fn leaf(&self, n: NodeRef) -> RcfResult<&LeafData> {
337 if !n.is_leaf() {
338 return Err(RcfError::InvalidConfig(
339 "NodeStore::leaf: called on an internal reference".into(),
340 ));
341 }
342 let idx = n.index();
343 if idx >= self.capacity as usize {
344 return Err(RcfError::OutOfBounds {
345 index: idx,
346 len: self.capacity as usize,
347 });
348 }
349 self.leaves[idx].as_ref().ok_or(RcfError::OutOfBounds {
350 index: idx,
351 len: self.capacity as usize,
352 })
353 }
354
355 pub fn leaf_mut(&mut self, n: NodeRef) -> RcfResult<&mut LeafData> {
361 if !n.is_leaf() {
362 return Err(RcfError::InvalidConfig(
363 "NodeStore::leaf_mut: called on an internal reference".into(),
364 ));
365 }
366 let idx = n.index();
367 if idx >= self.capacity as usize {
368 return Err(RcfError::OutOfBounds {
369 index: idx,
370 len: self.capacity as usize,
371 });
372 }
373 self.leaves[idx].as_mut().ok_or(RcfError::OutOfBounds {
374 index: idx,
375 len: self.capacity as usize,
376 })
377 }
378
379 pub fn parent(&self, n: NodeRef) -> RcfResult<Option<NodeRef>> {
385 Ok(self.view(n)?.parent())
386 }
387
388 pub fn sibling(&self, n: NodeRef) -> RcfResult<Option<NodeRef>> {
400 let Some(parent_ref) = self.parent(n)? else {
401 return Ok(None);
402 };
403 if parent_ref.is_leaf() {
404 return Err(RcfError::InvalidConfig(
405 "NodeStore::sibling: parent is a leaf — invariant violated".into(),
406 ));
407 }
408 let parent = self.internal(parent_ref)?;
409 let n_raw = n.raw();
410 if parent.left.raw() == n_raw {
411 Ok(Some(parent.right))
412 } else if parent.right.raw() == n_raw {
413 Ok(Some(parent.left))
414 } else {
415 Err(RcfError::InvalidConfig(
416 "NodeStore::sibling: child not registered with parent".into(),
417 ))
418 }
419 }
420
421 pub fn internal_bbox(&self, n: NodeRef) -> RcfResult<&BoundingBox<D>> {
430 Ok(&self.internal(n)?.bbox)
431 }
432
433 pub fn set_parent(&mut self, n: NodeRef, parent: Option<NodeRef>) -> RcfResult<()> {
439 match self.view_mut(n)? {
440 NodeViewMut::Internal(i) => {
441 i.parent = parent;
442 }
443 NodeViewMut::Leaf(l) => {
444 l.parent = parent;
445 }
446 }
447 Ok(())
448 }
449
450 pub fn set_mass(&mut self, n: NodeRef, mass: u64) -> RcfResult<()> {
456 match self.view_mut(n)? {
457 NodeViewMut::Internal(i) => {
458 i.mass = mass;
459 }
460 NodeViewMut::Leaf(l) => {
461 l.mass = mass;
462 }
463 }
464 Ok(())
465 }
466
467 pub fn set_internal_bbox(&mut self, n: NodeRef, bbox: BoundingBox<D>) -> RcfResult<()> {
474 self.internal_mut(n)?.bbox = bbox;
475 Ok(())
476 }
477
478 pub fn set_internal_children(
487 &mut self,
488 n: NodeRef,
489 new_left: NodeRef,
490 new_right: NodeRef,
491 ) -> RcfResult<()> {
492 let i = self.internal_mut(n)?;
493 i.left = new_left;
494 i.right = new_right;
495 Ok(())
496 }
497
498 pub fn set_internal_cut(&mut self, n: NodeRef, new_cut: Cut) -> RcfResult<()> {
506 self.internal_mut(n)?.cut = new_cut;
507 Ok(())
508 }
509}
510
511#[cfg(test)]
512#[allow(clippy::float_cmp)] mod tests {
514 use super::*;
515 use proptest::prelude::*;
516
517 fn unit_bbox<const D: usize>() -> BoundingBox<D> {
518 let mut b = BoundingBox::<D>::from_point(&vec![0.0; D]).unwrap();
519 b.extend(&vec![1.0; D]).unwrap();
520 b
521 }
522
523 #[test]
524 fn new_rejects_zero_capacity() {
525 assert!(matches!(
526 NodeStore::<2>::new(0).unwrap_err(),
527 RcfError::InvalidConfig(_)
528 ));
529 }
530
531 #[test]
532 fn new_rejects_capacity_above_max() {
533 assert!(matches!(
535 NodeStore::<2>::new(u32::MAX).unwrap_err(),
536 RcfError::InvalidConfig(_)
537 ));
538 }
539
540 #[test]
541 fn new_starts_empty() {
542 let s = NodeStore::<2>::new(8).unwrap();
543 assert_eq!(s.capacity(), 8);
544 assert_eq!(s.live_count(), 0);
545 assert_eq!(s.live_internal_count(), 0);
546 assert_eq!(s.live_leaf_count(), 0);
547 }
548
549 #[test]
550 fn add_leaf_increments_live() {
551 let mut s = NodeStore::<2>::new(4).unwrap();
552 let l = s.add_leaf(7, None, 1).unwrap();
553 assert!(l.is_leaf());
554 assert_eq!(s.live_leaf_count(), 1);
555 assert_eq!(s.live_internal_count(), 0);
556 }
557
558 #[test]
559 fn add_internal_increments_live() {
560 let mut s = NodeStore::<2>::new(4).unwrap();
561 let l1 = s.add_leaf(0, None, 1).unwrap();
562 let l2 = s.add_leaf(1, None, 1).unwrap();
563 let cut = Cut::new(0, 0.5);
564 let i = s
565 .add_internal(cut, unit_bbox::<2>(), l1, l2, None, 2)
566 .unwrap();
567 assert!(i.is_internal());
568 assert_eq!(s.live_internal_count(), 1);
569 assert_eq!(s.live_leaf_count(), 2);
570 assert_eq!(s.live_count(), 3);
571 }
572
573 #[test]
574 fn add_leaf_capacity_exhausted() {
575 let mut s = NodeStore::<2>::new(2).unwrap();
576 s.add_leaf(0, None, 1).unwrap();
577 s.add_leaf(1, None, 1).unwrap();
578 assert!(matches!(
579 s.add_leaf(2, None, 1).unwrap_err(),
580 RcfError::InvalidConfig(_)
581 ));
582 }
583
584 #[test]
585 fn add_internal_capacity_exhausted() {
586 let mut s = NodeStore::<2>::new(1).unwrap();
587 let l1 = s.add_leaf(0, None, 1).unwrap();
588 let l2 = s.add_leaf(1, None, 1);
589 assert!(matches!(l2.unwrap_err(), RcfError::InvalidConfig(_)));
591 let i = s
592 .add_internal(Cut::new(0, 0.0), unit_bbox::<2>(), l1, l1, None, 1)
593 .unwrap();
594 assert!(
595 s.add_internal(Cut::new(0, 0.0), unit_bbox::<2>(), i, i, None, 1)
596 .is_err()
597 );
598 }
599
600 #[test]
601 fn delete_frees_slot_for_reuse() {
602 let mut s = NodeStore::<2>::new(2).unwrap();
603 let l = s.add_leaf(7, None, 1).unwrap();
604 let l_idx = l.index();
605 s.delete(l).unwrap();
606 assert_eq!(s.live_leaf_count(), 0);
607 let l2 = s.add_leaf(99, None, 1).unwrap();
609 assert_eq!(l2.index(), l_idx);
610 }
611
612 #[test]
613 fn delete_oob_index_rejected() {
614 let mut s = NodeStore::<2>::new(2).unwrap();
615 let bogus = NodeRef::leaf(99);
616 assert!(matches!(
617 s.delete(bogus).unwrap_err(),
618 RcfError::OutOfBounds { .. }
619 ));
620 }
621
622 #[test]
623 fn delete_double_free_rejected() {
624 let mut s = NodeStore::<2>::new(2).unwrap();
625 let l = s.add_leaf(0, None, 1).unwrap();
626 s.delete(l).unwrap();
627 assert!(matches!(
628 s.delete(l).unwrap_err(),
629 RcfError::OutOfBounds { .. }
630 ));
631 }
632
633 #[test]
634 fn leaf_returns_inserted_value() {
635 let mut s = NodeStore::<2>::new(2).unwrap();
636 let l = s.add_leaf(42, None, 1).unwrap();
637 let data = s.leaf(l).unwrap();
638 assert_eq!(data.point_idx, 42);
639 assert_eq!(data.mass, 1);
640 }
641
642 #[test]
643 fn leaf_mut_allows_inplace_update() {
644 let mut s = NodeStore::<2>::new(2).unwrap();
645 let l = s.add_leaf(1, None, 1).unwrap();
646 s.leaf_mut(l).unwrap().mass = 5;
647 assert_eq!(s.view(l).unwrap().mass(), 5);
648 }
649
650 #[test]
651 fn view_oob_returns_err() {
652 let s = NodeStore::<2>::new(2).unwrap();
653 assert!(matches!(
654 s.view(NodeRef::leaf(7)).unwrap_err(),
655 RcfError::OutOfBounds { .. }
656 ));
657 }
658
659 #[test]
660 fn parent_and_sibling_lookup() {
661 let mut s = NodeStore::<2>::new(4).unwrap();
662 let l1 = s.add_leaf(0, None, 1).unwrap();
663 let l2 = s.add_leaf(1, None, 1).unwrap();
664 let i = s
665 .add_internal(Cut::new(0, 0.5), unit_bbox::<2>(), l1, l2, None, 2)
666 .unwrap();
667 s.set_parent(l1, Some(i)).unwrap();
668 s.set_parent(l2, Some(i)).unwrap();
669
670 assert_eq!(s.parent(l1).unwrap(), Some(i));
671 assert_eq!(s.parent(i).unwrap(), None);
672 assert_eq!(s.sibling(l1).unwrap(), Some(l2));
673 assert_eq!(s.sibling(l2).unwrap(), Some(l1));
674 assert_eq!(s.sibling(i).unwrap(), None);
676 }
677
678 #[test]
679 fn sibling_orphan_detected() {
680 let mut s = NodeStore::<2>::new(4).unwrap();
681 let real_l = s.add_leaf(0, None, 1).unwrap();
682 let fake_l = s.add_leaf(1, None, 1).unwrap();
683 let other = s.add_leaf(2, None, 1).unwrap();
684 let i = s
685 .add_internal(Cut::new(0, 0.5), unit_bbox::<2>(), real_l, other, None, 2)
686 .unwrap();
687 s.set_parent(fake_l, Some(i)).unwrap();
689 assert!(matches!(
690 s.sibling(fake_l).unwrap_err(),
691 RcfError::InvalidConfig(_)
692 ));
693 }
694
695 #[test]
696 fn sibling_parent_is_leaf_rejected() {
697 let mut s = NodeStore::<2>::new(4).unwrap();
698 let leaf_parent = s.add_leaf(0, None, 1).unwrap();
699 let child = s.add_leaf(1, None, 1).unwrap();
700 s.set_parent(child, Some(leaf_parent)).unwrap();
702 assert!(matches!(
703 s.sibling(child).unwrap_err(),
704 RcfError::InvalidConfig(_)
705 ));
706 }
707
708 #[test]
709 fn internal_bbox_returns_cached_box() {
710 let mut s = NodeStore::<2>::new(2).unwrap();
711 let l1 = s.add_leaf(0, None, 1).unwrap();
712 let l2 = s.add_leaf(1, None, 1).unwrap();
713 let bbox = unit_bbox::<2>();
714 let i = s
715 .add_internal(Cut::new(0, 0.5), bbox.clone(), l1, l2, None, 2)
716 .unwrap();
717 assert_eq!(s.internal_bbox(i).unwrap(), &bbox);
718 }
719
720 #[test]
721 fn internal_bbox_on_leaf_rejected() {
722 let mut s = NodeStore::<2>::new(2).unwrap();
723 let l = s.add_leaf(0, None, 1).unwrap();
724 assert!(matches!(
725 s.internal_bbox(l).unwrap_err(),
726 RcfError::InvalidConfig(_)
727 ));
728 }
729
730 #[test]
731 fn set_mass_updates_leaf_and_internal() {
732 let mut s = NodeStore::<2>::new(2).unwrap();
733 let l = s.add_leaf(0, None, 1).unwrap();
734 s.set_mass(l, 9).unwrap();
735 assert_eq!(s.view(l).unwrap().mass(), 9);
736 }
737
738 #[test]
739 fn set_internal_bbox_replaces_cached() {
740 let mut s = NodeStore::<2>::new(2).unwrap();
741 let l1 = s.add_leaf(0, None, 1).unwrap();
742 let l2 = s.add_leaf(1, None, 1).unwrap();
743 let i = s
744 .add_internal(Cut::new(0, 0.5), unit_bbox::<2>(), l1, l2, None, 2)
745 .unwrap();
746 let mut new_bbox = BoundingBox::<2>::from_point(&[0.0, 0.0]).unwrap();
747 new_bbox.extend(&[10.0, 10.0]).unwrap();
748 s.set_internal_bbox(i, new_bbox.clone()).unwrap();
749 assert_eq!(s.internal_bbox(i).unwrap(), &new_bbox);
750 }
751
752 #[test]
753 fn set_internal_children_swaps_refs() {
754 let mut s = NodeStore::<2>::new(4).unwrap();
755 let l1 = s.add_leaf(0, None, 1).unwrap();
756 let l2 = s.add_leaf(1, None, 1).unwrap();
757 let l3 = s.add_leaf(2, None, 1).unwrap();
758 let i = s
759 .add_internal(Cut::new(0, 0.5), unit_bbox::<2>(), l1, l2, None, 2)
760 .unwrap();
761 s.set_internal_children(i, l1, l3).unwrap();
762 let data = s.internal(i).unwrap();
763 assert_eq!(data.left, l1);
764 assert_eq!(data.right, l3);
765 }
766
767 #[test]
768 fn set_internal_cut_replaces_cut() {
769 let mut s = NodeStore::<2>::new(4).unwrap();
770 let l1 = s.add_leaf(0, None, 1).unwrap();
771 let l2 = s.add_leaf(1, None, 1).unwrap();
772 let i = s
773 .add_internal(Cut::new(0, 0.5), unit_bbox::<2>(), l1, l2, None, 2)
774 .unwrap();
775 s.set_internal_cut(i, Cut::new(1, 9.0)).unwrap();
776 let data = s.internal(i).unwrap();
777 assert_eq!(data.cut.dim(), 1);
778 assert_eq!(data.cut.value(), 9.0);
779 }
780
781 #[test]
782 fn setters_reject_oob_or_wrong_kind() {
783 let mut s = NodeStore::<2>::new(2).unwrap();
784 let l = s.add_leaf(0, None, 1).unwrap();
785 assert!(matches!(
786 s.set_internal_bbox(l, unit_bbox::<2>()).unwrap_err(),
787 RcfError::InvalidConfig(_)
788 ));
789 assert!(matches!(
790 s.set_internal_children(l, l, l).unwrap_err(),
791 RcfError::InvalidConfig(_)
792 ));
793 assert!(matches!(
794 s.set_internal_cut(l, Cut::new(0, 0.0)).unwrap_err(),
795 RcfError::InvalidConfig(_)
796 ));
797 assert!(matches!(
798 s.set_parent(NodeRef::leaf(99), None).unwrap_err(),
799 RcfError::OutOfBounds { .. }
800 ));
801 assert!(matches!(
802 s.set_mass(NodeRef::leaf(99), 1).unwrap_err(),
803 RcfError::OutOfBounds { .. }
804 ));
805 }
806
807 proptest! {
811 #![proptest_config(ProptestConfig { cases: 64, ..ProptestConfig::default() })]
812 #[test]
813 fn invariants_under_random_ops(ops in proptest::collection::vec(0u32..20, 0..200)) {
814 const CAP: u32 = 16;
815 let mut s = NodeStore::<2>::new(CAP).unwrap();
816 let mut live_leaves: Vec<NodeRef> = Vec::new();
817 let mut live_internals: Vec<NodeRef> = Vec::new();
818
819 for op in ops {
820 match op % 4 {
821 0 => {
822 if let Ok(r) = s.add_leaf(0, None, 1) {
823 prop_assert!(!live_leaves.iter().any(|x| x.raw() == r.raw()));
825 live_leaves.push(r);
826 }
827 }
828 1 => {
829 if live_leaves.len() >= 2 {
830 let l1 = live_leaves[live_leaves.len() - 1];
831 let l2 = live_leaves[live_leaves.len() - 2];
832 if let Ok(r) = s.add_internal(
833 Cut::new(0, 0.0), unit_bbox::<2>(), l1, l2, None, 2,
834 ) {
835 prop_assert!(!live_internals.iter().any(|x| x.raw() == r.raw()));
836 live_internals.push(r);
837 }
838 }
839 }
840 2 => {
841 if let Some(l) = live_leaves.pop() {
842 s.delete(l).unwrap();
843 }
844 }
845 _ => {
846 if let Some(i) = live_internals.pop() {
847 s.delete(i).unwrap();
848 }
849 }
850 }
851 prop_assert_eq!(s.live_leaf_count(), live_leaves.len());
852 prop_assert_eq!(s.live_internal_count(), live_internals.len());
853 prop_assert_eq!(s.live_count(), live_leaves.len() + live_internals.len());
854 }
855 }
856 }
857}