Add to_string to KDTree for debug printing

kdtree.h

@@ -1,5 +1,6 @@
 #include <algorithm>
 #include <glm/glm.hpp>
+#include <string>
 #include <vector>
 
 // Forward declarations
@@ -42,6 +43,12 @@ class KDTree {
 
     ~KDTree() = default;
 
+    std::string to_string() {
+        std::string str = "";
+        to_string_recurse(str, root, 0);
+        return str;
+    }
+
   private:
     Node *root;
 
@@ -87,8 +94,7 @@ class KDTree {
         std::nth_element(points.begin(), points.begin() + middle, points.end(),
                          get_point_comparator(axis));
 
-        Point *median;
-        float pivot;
+        Point *median = points[middle];
 
         // TODO: This copies. Can we split the vector into two without copying?
         std::vector<Point *> left_of_median(points.begin(), points.begin() + middle);
@@ -98,4 +104,17 @@ class KDTree {
         return new Node(axis, median, build(left_of_median, depth + 1),
                         build(right_of_median, depth + 1));
     }
+
+    void to_string_recurse(std::string &str, Node *node, int depth) {
+        if (node == nullptr) { return; }
+
+        Point *point = node->point;
+
+        str += std::string(depth, '-') + std::to_string(point->coordinates[0]) + ", " +
+               std::to_string(point->coordinates[1]) + ", " +
+               std::to_string(point->coordinates[2]) + "\n";
+
+        to_string_recurse(str, node->left, depth + 1);
+        to_string_recurse(str, node->right, depth + 1);
+    }
 };