Initial implementation of IndexedBinaryHeap sift functions

src/backing/indexed/binary_heap.rs

@@ -1,7 +1,7 @@
 use std::{collections::HashMap, hash::Hash};
 
 use crate::backing::{
-    containers::{Pair, SiftResult},
+    containers::{Pair, SiftError, SiftResult},
     indexed::IndexedBacking,
 };
 
@@ -24,11 +24,78 @@ impl<D: Hash + Eq + Clone + Send + Sync, P: PartialOrd + Clone + Send + Sync>
     }
 
     fn sift_up(&mut self, i: usize) -> SiftResult {
-        todo!()
+        print!("{i}");
+        if i == 0 {
+            // Base case, at root so nothing to do
+            Ok(())
+        } else if let Some(child) = self.data.get(i).cloned() {
+            let parent_index = (i - 1) / 2;
+            // Check if the heap property is violated
+            if child < self.data[parent_index] {
+                (self.data[i], self.data[parent_index]) =
+                    (self.data[parent_index].clone(), self.data[i].clone());
+                self.indices.insert(self.data[i].clone().data(), i).unwrap();
+                self.indices
+                    .insert(self.data[parent_index].clone().data(), parent_index)
+                    .unwrap();
+                self.sift_up(parent_index)
+            } else {
+                // Sift complete
+                Ok(())
+            }
+        } else {
+            // Tried to sift a non-existent index
+            Err(SiftError::new(i, self.data.len()))
+        }
     }
 
     fn sift_down(&mut self, i: usize) -> SiftResult {
-        todo!()
+        print!("{i}");
+        if i > self.data.len() {
+            // Tried to sift a non-existent index
+            Err(SiftError::new(i, self.data.len()))
+        } else {
+            if let Some(first_child) = self.data.get(i * 2 + 1).cloned() {
+                let smaller_child_index;
+                let smaller_child;
+
+                // Find the smallest child and its index
+                if let Some(second_child) = self.data.get(i * 2 + 2).cloned() {
+                    // Both children, use the smaller one
+                    if first_child < second_child {
+                        smaller_child = first_child;
+                        smaller_child_index = i * 2 + 1;
+                    } else {
+                        smaller_child = second_child;
+                        smaller_child_index = i * 2 + 2;
+                    }
+                } else {
+                    // Only one child, no choice
+                    smaller_child = first_child;
+                    smaller_child_index = i * 2 + 1;
+                }
+
+                if smaller_child < self.data[i] {
+                    // Swap parent with child
+                    self.data[smaller_child_index] = self.data[i].clone();
+                    self.indices.insert(
+                        self.data[smaller_child_index].clone().data(),
+                        smaller_child_index,
+                    );
+                    self.data[i] = smaller_child.clone();
+                    self.indices.insert(smaller_child.data(), i);
+
+                    // Repeat process with child
+                    self.sift_down(smaller_child_index)
+                } else {
+                    // Heap property satisfied, we're done
+                    Ok(())
+                }
+            } else {
+                // Base case, no children so nothing to do
+                Ok(())
+            }
+        }
     }
 
     fn delete_pair(&mut self, i: usize) -> Option<Pair<D, P>> {