libBLOC

diff --git a/tree/tree_add.c b/tree/tree_add.c
new file mode 100644
index 0000000..67397ba
--- /dev/null
+++ b/tree/tree_add.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (C) 2023 Barry
+ *
+ * This file is part of Barry's Little Objects in C Library (libBLOC).
+ *
+ * libBLOC is free software: you can redistribute it and/or modify it under the
+ * terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * libBLOC is distributed in the hope that it will be useful, but WITHOUT ANY
+ * WARRANTY; without even the implied warranty of MERCHANTIBILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
+ * details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * libBLOC.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include <BLOC/object.h>
+#include <BLOC/tree.h>
+#include "../assert.h"
+#include "../object.h"
+#include "tree.h"
+
+/* Add a node to a tree recursively */
+static struct TreeNode *
+_tree_add_r(Tree *t, struct TreeNode *parent, struct TreeNode *child)
+{
+	/* Add to correct side */
+	if (t->compare(parent->header.obj, child->header.obj) > 0) {
+		if (!parent->left) {
+			parent->left = child;
+
+			child->header.prev = parent->header.prev;
+			child->header.next = &parent->header;
+			parent->header.prev = &child->header;
+			if (child->header.prev)
+				child->header.prev->next = &child->header;
+			else
+				t->header.start = &child->header;
+		} else {
+			parent->left = _tree_add_r(t, parent->left, child);
+		}
+	} else {
+		if (!parent->right) {
+			parent->right = child;
+
+			child->header.next = parent->header.next;
+			child->header.prev = &parent->header;
+			parent->header.next = &child->header;
+			if (child->header.next)
+				child->header.next->prev = &child->header;
+			else
+				t->header.end = &child->header;
+		} else {
+			parent->right = _tree_add_r(t, parent->right, child);
+		}
+	}
+
+	/* Balance */
+	if (_tree_count(parent->right) > _tree_count(parent->left) + 1)
+		return _tree_rotate_left(parent);
+	if (_tree_count(parent->left) > _tree_count(parent->right) + 1)
+		return _tree_rotate_right(parent);
+	return parent;
+}
+
+/* Add an object to a tree */
+void
+tree_add(Tree *tree, void *obj)
+{
+	ASSERT(tree);
+	ASSERT(obj);
+	struct ObjectHeader *header = object_header(obj);
+	ASSERT(header->magic == OBJECT_MAGIC);
+	if (tree->type && tree->type != header->type)
+		return;
+
+	struct TreeNode *node;
+	node = malloc(sizeof(struct TreeNode));
+	node->left = node->right = NULL;
+	node->header.prev = node->header.next = NULL;
+	node->header.obj = obj_get(obj);
+
+	obj_lock(tree);
+	tree->header.entries++;
+	if (!tree->root) {
+		tree->root = node;
+		tree->header.start = tree->header.end = &node->header;
+	} else {
+		tree->root = _tree_add_r(tree, tree->root, node);
+	}
+	obj_unlock(tree);
+}