1 files changed, 341 insertions, 284 deletions

diff --git a/src/common/tree.h b/src/common/tree.h
index 18faa4a48..28370e343 100644
--- a/src/common/tree.h
+++ b/src/common/tree.h
@@ -43,294 +43,265 @@
  * The maximum height of a red-black tree is 2lg (n+1).
  */
 
-#include "common/assert.h"
+namespace Common::freebsd {
 
-namespace Common {
+enum class RBColor {
+    RB_BLACK = 0,
+    RB_RED = 1,
+};
+
+#pragma pack(push, 4)
 template <typename T>
-class RBHead {
+class RBEntry {
 public:
-    [[nodiscard]] T* Root() {
-        return rbh_root;
-    }
+    constexpr RBEntry() = default;
 
-    [[nodiscard]] const T* Root() const {
-        return rbh_root;
+    [[nodiscard]] constexpr T* Left() {
+        return m_rbe_left;
     }
-
-    void SetRoot(T* root) {
-        rbh_root = root;
+    [[nodiscard]] constexpr const T* Left() const {
+        return m_rbe_left;
     }
 
-    [[nodiscard]] bool IsEmpty() const {
-        return Root() == nullptr;
+    constexpr void SetLeft(T* e) {
+        m_rbe_left = e;
     }
 
-private:
-    T* rbh_root = nullptr;
-};
-
-enum class EntryColor {
-    Black,
-    Red,
-};
-
-template <typename T>
-class RBEntry {
-public:
-    [[nodiscard]] T* Left() {
-        return rbe_left;
+    [[nodiscard]] constexpr T* Right() {
+        return m_rbe_right;
     }
-
-    [[nodiscard]] const T* Left() const {
-        return rbe_left;
+    [[nodiscard]] constexpr const T* Right() const {
+        return m_rbe_right;
     }
 
-    void SetLeft(T* left) {
-        rbe_left = left;
+    constexpr void SetRight(T* e) {
+        m_rbe_right = e;
     }
 
-    [[nodiscard]] T* Right() {
-        return rbe_right;
+    [[nodiscard]] constexpr T* Parent() {
+        return m_rbe_parent;
     }
-
-    [[nodiscard]] const T* Right() const {
-        return rbe_right;
+    [[nodiscard]] constexpr const T* Parent() const {
+        return m_rbe_parent;
     }
 
-    void SetRight(T* right) {
-        rbe_right = right;
+    constexpr void SetParent(T* e) {
+        m_rbe_parent = e;
     }
 
-    [[nodiscard]] T* Parent() {
-        return rbe_parent;
+    [[nodiscard]] constexpr bool IsBlack() const {
+        return m_rbe_color == RBColor::RB_BLACK;
     }
-
-    [[nodiscard]] const T* Parent() const {
-        return rbe_parent;
+    [[nodiscard]] constexpr bool IsRed() const {
+        return m_rbe_color == RBColor::RB_RED;
     }
-
-    void SetParent(T* parent) {
-        rbe_parent = parent;
+    [[nodiscard]] constexpr RBColor Color() const {
+        return m_rbe_color;
     }
 
-    [[nodiscard]] bool IsBlack() const {
-        return rbe_color == EntryColor::Black;
+    constexpr void SetColor(RBColor c) {
+        m_rbe_color = c;
     }
 
-    [[nodiscard]] bool IsRed() const {
-        return rbe_color == EntryColor::Red;
-    }
+private:
+    T* m_rbe_left{};
+    T* m_rbe_right{};
+    T* m_rbe_parent{};
+    RBColor m_rbe_color{RBColor::RB_BLACK};
+};
+#pragma pack(pop)
 
-    [[nodiscard]] EntryColor Color() const {
-        return rbe_color;
-    }
+template <typename T>
+struct CheckRBEntry {
+    static constexpr bool value = false;
+};
+template <typename T>
+struct CheckRBEntry<RBEntry<T>> {
+    static constexpr bool value = true;
+};
 
-    void SetColor(EntryColor color) {
-        rbe_color = color;
-    }
+template <typename T>
+concept IsRBEntry = CheckRBEntry<T>::value;
 
+template <typename T>
+concept HasRBEntry = requires(T& t, const T& ct) {
+    { t.GetRBEntry() } -> std::same_as<RBEntry<T>&>;
+    { ct.GetRBEntry() } -> std::same_as<const RBEntry<T>&>;
+};
+
+template <typename T>
+requires HasRBEntry<T>
+class RBHead {
 private:
-    T* rbe_left = nullptr;
-    T* rbe_right = nullptr;
-    T* rbe_parent = nullptr;
-    EntryColor rbe_color{};
+    T* m_rbh_root = nullptr;
+
+public:
+    [[nodiscard]] constexpr T* Root() {
+        return m_rbh_root;
+    }
+    [[nodiscard]] constexpr const T* Root() const {
+        return m_rbh_root;
+    }
+    constexpr void SetRoot(T* root) {
+        m_rbh_root = root;
+    }
+
+    [[nodiscard]] constexpr bool IsEmpty() const {
+        return this->Root() == nullptr;
+    }
 };
 
-template <typename Node>
-[[nodiscard]] RBEntry<Node>& RB_ENTRY(Node* node) {
-    return node->GetEntry();
+template <typename T>
+requires HasRBEntry<T>
+[[nodiscard]] constexpr RBEntry<T>& RB_ENTRY(T* t) {
+    return t->GetRBEntry();
 }
-
-template <typename Node>
-[[nodiscard]] const RBEntry<Node>& RB_ENTRY(const Node* node) {
-    return node->GetEntry();
+template <typename T>
+requires HasRBEntry<T>
+[[nodiscard]] constexpr const RBEntry<T>& RB_ENTRY(const T* t) {
+    return t->GetRBEntry();
 }
 
-template <typename Node>
-[[nodiscard]] Node* RB_PARENT(Node* node) {
-    return RB_ENTRY(node).Parent();
+template <typename T>
+requires HasRBEntry<T>
+[[nodiscard]] constexpr T* RB_LEFT(T* t) {
+    return RB_ENTRY(t).Left();
 }
-
-template <typename Node>
-[[nodiscard]] const Node* RB_PARENT(const Node* node) {
-    return RB_ENTRY(node).Parent();
+template <typename T>
+requires HasRBEntry<T>
+[[nodiscard]] constexpr const T* RB_LEFT(const T* t) {
+    return RB_ENTRY(t).Left();
 }
 
-template <typename Node>
-void RB_SET_PARENT(Node* node, Node* parent) {
-    return RB_ENTRY(node).SetParent(parent);
+template <typename T>
+requires HasRBEntry<T>
+[[nodiscard]] constexpr T* RB_RIGHT(T* t) {
+    return RB_ENTRY(t).Right();
 }
-
-template <typename Node>
-[[nodiscard]] Node* RB_LEFT(Node* node) {
-    return RB_ENTRY(node).Left();
+template <typename T>
+requires HasRBEntry<T>
+[[nodiscard]] constexpr const T* RB_RIGHT(const T* t) {
+    return RB_ENTRY(t).Right();
 }
 
-template <typename Node>
-[[nodiscard]] const Node* RB_LEFT(const Node* node) {
-    return RB_ENTRY(node).Left();
+template <typename T>
+requires HasRBEntry<T>
+[[nodiscard]] constexpr T* RB_PARENT(T* t) {
+    return RB_ENTRY(t).Parent();
 }
-
-template <typename Node>
-void RB_SET_LEFT(Node* node, Node* left) {
-    return RB_ENTRY(node).SetLeft(left);
+template <typename T>
+requires HasRBEntry<T>
+[[nodiscard]] constexpr const T* RB_PARENT(const T* t) {
+    return RB_ENTRY(t).Parent();
 }
 
-template <typename Node>
-[[nodiscard]] Node* RB_RIGHT(Node* node) {
-    return RB_ENTRY(node).Right();
+template <typename T>
+requires HasRBEntry<T>
+constexpr void RB_SET_LEFT(T* t, T* e) {
+    RB_ENTRY(t).SetLeft(e);
 }
-
-template <typename Node>
-[[nodiscard]] const Node* RB_RIGHT(const Node* node) {
-    return RB_ENTRY(node).Right();
+template <typename T>
+requires HasRBEntry<T>
+constexpr void RB_SET_RIGHT(T* t, T* e) {
+    RB_ENTRY(t).SetRight(e);
 }
-
-template <typename Node>
-void RB_SET_RIGHT(Node* node, Node* right) {
-    return RB_ENTRY(node).SetRight(right);
+template <typename T>
+requires HasRBEntry<T>
+constexpr void RB_SET_PARENT(T* t, T* e) {
+    RB_ENTRY(t).SetParent(e);
 }
 
-template <typename Node>
-[[nodiscard]] bool RB_IS_BLACK(const Node* node) {
-    return RB_ENTRY(node).IsBlack();
+template <typename T>
+requires HasRBEntry<T>
+[[nodiscard]] constexpr bool RB_IS_BLACK(const T* t) {
+    return RB_ENTRY(t).IsBlack();
 }
-
-template <typename Node>
-[[nodiscard]] bool RB_IS_RED(const Node* node) {
-    return RB_ENTRY(node).IsRed();
+template <typename T>
+requires HasRBEntry<T>
+[[nodiscard]] constexpr bool RB_IS_RED(const T* t) {
+    return RB_ENTRY(t).IsRed();
 }
 
-template <typename Node>
-[[nodiscard]] EntryColor RB_COLOR(const Node* node) {
-    return RB_ENTRY(node).Color();
+template <typename T>
+requires HasRBEntry<T>
+[[nodiscard]] constexpr RBColor RB_COLOR(const T* t) {
+    return RB_ENTRY(t).Color();
 }
 
-template <typename Node>
-void RB_SET_COLOR(Node* node, EntryColor color) {
-    return RB_ENTRY(node).SetColor(color);
+template <typename T>
+requires HasRBEntry<T>
+constexpr void RB_SET_COLOR(T* t, RBColor c) {
+    RB_ENTRY(t).SetColor(c);
 }
 
-template <typename Node>
-void RB_SET(Node* node, Node* parent) {
-    auto& entry = RB_ENTRY(node);
-    entry.SetParent(parent);
-    entry.SetLeft(nullptr);
-    entry.SetRight(nullptr);
-    entry.SetColor(EntryColor::Red);
+template <typename T>
+requires HasRBEntry<T>
+constexpr void RB_SET(T* elm, T* parent) {
+    auto& rb_entry = RB_ENTRY(elm);
+    rb_entry.SetParent(parent);
+    rb_entry.SetLeft(nullptr);
+    rb_entry.SetRight(nullptr);
+    rb_entry.SetColor(RBColor::RB_RED);
 }
 
-template <typename Node>
-void RB_SET_BLACKRED(Node* black, Node* red) {
-    RB_SET_COLOR(black, EntryColor::Black);
-    RB_SET_COLOR(red, EntryColor::Red);
+template <typename T>
+requires HasRBEntry<T>
+constexpr void RB_SET_BLACKRED(T* black, T* red) {
+    RB_SET_COLOR(black, RBColor::RB_BLACK);
+    RB_SET_COLOR(red, RBColor::RB_RED);
 }
 
-template <typename Node>
-void RB_ROTATE_LEFT(RBHead<Node>* head, Node* elm, Node*& tmp) {
+template <typename T>
+requires HasRBEntry<T>
+constexpr void RB_ROTATE_LEFT(RBHead<T>& head, T* elm, T*& tmp) {
     tmp = RB_RIGHT(elm);
-    RB_SET_RIGHT(elm, RB_LEFT(tmp));
-    if (RB_RIGHT(elm) != nullptr) {
+    if (RB_SET_RIGHT(elm, RB_LEFT(tmp)); RB_RIGHT(elm) != nullptr) {
         RB_SET_PARENT(RB_LEFT(tmp), elm);
     }
 
-    RB_SET_PARENT(tmp, RB_PARENT(elm));
-    if (RB_PARENT(tmp) != nullptr) {
+    if (RB_SET_PARENT(tmp, RB_PARENT(elm)); RB_PARENT(tmp) != nullptr) {
         if (elm == RB_LEFT(RB_PARENT(elm))) {
             RB_SET_LEFT(RB_PARENT(elm), tmp);
         } else {
             RB_SET_RIGHT(RB_PARENT(elm), tmp);
         }
     } else {
-        head->SetRoot(tmp);
+        head.SetRoot(tmp);
     }
 
     RB_SET_LEFT(tmp, elm);
     RB_SET_PARENT(elm, tmp);
 }
 
-template <typename Node>
-void RB_ROTATE_RIGHT(RBHead<Node>* head, Node* elm, Node*& tmp) {
+template <typename T>
+requires HasRBEntry<T>
+constexpr void RB_ROTATE_RIGHT(RBHead<T>& head, T* elm, T*& tmp) {
     tmp = RB_LEFT(elm);
-    RB_SET_LEFT(elm, RB_RIGHT(tmp));
-    if (RB_LEFT(elm) != nullptr) {
+    if (RB_SET_LEFT(elm, RB_RIGHT(tmp)); RB_LEFT(elm) != nullptr) {
         RB_SET_PARENT(RB_RIGHT(tmp), elm);
     }
 
-    RB_SET_PARENT(tmp, RB_PARENT(elm));
-    if (RB_PARENT(tmp) != nullptr) {
+    if (RB_SET_PARENT(tmp, RB_PARENT(elm)); RB_PARENT(tmp) != nullptr) {
         if (elm == RB_LEFT(RB_PARENT(elm))) {
             RB_SET_LEFT(RB_PARENT(elm), tmp);
         } else {
             RB_SET_RIGHT(RB_PARENT(elm), tmp);
         }
     } else {
-        head->SetRoot(tmp);
+        head.SetRoot(tmp);
     }
 
     RB_SET_RIGHT(tmp, elm);
     RB_SET_PARENT(elm, tmp);
 }
 
-template <typename Node>
-void RB_INSERT_COLOR(RBHead<Node>* head, Node* elm) {
-    Node* parent = nullptr;
-    Node* tmp = nullptr;
-
-    while ((parent = RB_PARENT(elm)) != nullptr && RB_IS_RED(parent)) {
-        Node* gparent = RB_PARENT(parent);
-        if (parent == RB_LEFT(gparent)) {
-            tmp = RB_RIGHT(gparent);
-            if (tmp && RB_IS_RED(tmp)) {
-                RB_SET_COLOR(tmp, EntryColor::Black);
-                RB_SET_BLACKRED(parent, gparent);
-                elm = gparent;
-                continue;
-            }
-
-            if (RB_RIGHT(parent) == elm) {
-                RB_ROTATE_LEFT(head, parent, tmp);
-                tmp = parent;
-                parent = elm;
-                elm = tmp;
-            }
-
-            RB_SET_BLACKRED(parent, gparent);
-            RB_ROTATE_RIGHT(head, gparent, tmp);
-        } else {
-            tmp = RB_LEFT(gparent);
-            if (tmp && RB_IS_RED(tmp)) {
-                RB_SET_COLOR(tmp, EntryColor::Black);
-                RB_SET_BLACKRED(parent, gparent);
-                elm = gparent;
-                continue;
-            }
-
-            if (RB_LEFT(parent) == elm) {
-                RB_ROTATE_RIGHT(head, parent, tmp);
-                tmp = parent;
-                parent = elm;
-                elm = tmp;
-            }
-
-            RB_SET_BLACKRED(parent, gparent);
-            RB_ROTATE_LEFT(head, gparent, tmp);
-        }
-    }
-
-    RB_SET_COLOR(head->Root(), EntryColor::Black);
-}
-
-template <typename Node>
-void RB_REMOVE_COLOR(RBHead<Node>* head, Node* parent, Node* elm) {
-    Node* tmp;
-    while ((elm == nullptr || RB_IS_BLACK(elm)) && elm != head->Root() && parent != nullptr) {
+template <typename T>
+requires HasRBEntry<T>
+constexpr void RB_REMOVE_COLOR(RBHead<T>& head, T* parent, T* elm) {
+    T* tmp;
+    while ((elm == nullptr || RB_IS_BLACK(elm)) && elm != head.Root()) {
         if (RB_LEFT(parent) == elm) {
             tmp = RB_RIGHT(parent);
-            if (!tmp) {
-                ASSERT_MSG(false, "tmp is invalid!");
-                break;
-            }
             if (RB_IS_RED(tmp)) {
                 RB_SET_BLACKRED(tmp, parent);
                 RB_ROTATE_LEFT(head, parent, tmp);
@@ -339,29 +310,29 @@ void RB_REMOVE_COLOR(RBHead<Node>* head, Node* parent, Node* elm) {
 
             if ((RB_LEFT(tmp) == nullptr || RB_IS_BLACK(RB_LEFT(tmp))) &&
                 (RB_RIGHT(tmp) == nullptr || RB_IS_BLACK(RB_RIGHT(tmp)))) {
-                RB_SET_COLOR(tmp, EntryColor::Red);
+                RB_SET_COLOR(tmp, RBColor::RB_RED);
                 elm = parent;
                 parent = RB_PARENT(elm);
             } else {
                 if (RB_RIGHT(tmp) == nullptr || RB_IS_BLACK(RB_RIGHT(tmp))) {
-                    Node* oleft;
+                    T* oleft;
                     if ((oleft = RB_LEFT(tmp)) != nullptr) {
-                        RB_SET_COLOR(oleft, EntryColor::Black);
+                        RB_SET_COLOR(oleft, RBColor::RB_BLACK);
                     }
 
-                    RB_SET_COLOR(tmp, EntryColor::Red);
+                    RB_SET_COLOR(tmp, RBColor::RB_RED);
                     RB_ROTATE_RIGHT(head, tmp, oleft);
                     tmp = RB_RIGHT(parent);
                 }
 
                 RB_SET_COLOR(tmp, RB_COLOR(parent));
-                RB_SET_COLOR(parent, EntryColor::Black);
+                RB_SET_COLOR(parent, RBColor::RB_BLACK);
                 if (RB_RIGHT(tmp)) {
-                    RB_SET_COLOR(RB_RIGHT(tmp), EntryColor::Black);
+                    RB_SET_COLOR(RB_RIGHT(tmp), RBColor::RB_BLACK);
                 }
 
                 RB_ROTATE_LEFT(head, parent, tmp);
-                elm = head->Root();
+                elm = head.Root();
                 break;
             }
         } else {
@@ -372,68 +343,56 @@ void RB_REMOVE_COLOR(RBHead<Node>* head, Node* parent, Node* elm) {
                 tmp = RB_LEFT(parent);
             }
 
-            if (!tmp) {
-                ASSERT_MSG(false, "tmp is invalid!");
-                break;
-            }
-
             if ((RB_LEFT(tmp) == nullptr || RB_IS_BLACK(RB_LEFT(tmp))) &&
                 (RB_RIGHT(tmp) == nullptr || RB_IS_BLACK(RB_RIGHT(tmp)))) {
-                RB_SET_COLOR(tmp, EntryColor::Red);
+                RB_SET_COLOR(tmp, RBColor::RB_RED);
                 elm = parent;
                 parent = RB_PARENT(elm);
             } else {
                 if (RB_LEFT(tmp) == nullptr || RB_IS_BLACK(RB_LEFT(tmp))) {
-                    Node* oright;
+                    T* oright;
                     if ((oright = RB_RIGHT(tmp)) != nullptr) {
-                        RB_SET_COLOR(oright, EntryColor::Black);
+                        RB_SET_COLOR(oright, RBColor::RB_BLACK);
                     }
 
-                    RB_SET_COLOR(tmp, EntryColor::Red);
+                    RB_SET_COLOR(tmp, RBColor::RB_RED);
                     RB_ROTATE_LEFT(head, tmp, oright);
                     tmp = RB_LEFT(parent);
                 }
 
                 RB_SET_COLOR(tmp, RB_COLOR(parent));
-                RB_SET_COLOR(parent, EntryColor::Black);
+                RB_SET_COLOR(parent, RBColor::RB_BLACK);
 
                 if (RB_LEFT(tmp)) {
-                    RB_SET_COLOR(RB_LEFT(tmp), EntryColor::Black);
+                    RB_SET_COLOR(RB_LEFT(tmp), RBColor::RB_BLACK);
                 }
 
                 RB_ROTATE_RIGHT(head, parent, tmp);
-                elm = head->Root();
+                elm = head.Root();
                 break;
             }
         }
     }
 
     if (elm) {
-        RB_SET_COLOR(elm, EntryColor::Black);
+        RB_SET_COLOR(elm, RBColor::RB_BLACK);
     }
 }
 
-template <typename Node>
-Node* RB_REMOVE(RBHead<Node>* head, Node* elm) {
-    Node* child = nullptr;
-    Node* parent = nullptr;
-    Node* old = elm;
-    EntryColor color{};
-
-    const auto finalize = [&] {
-        if (color == EntryColor::Black) {
-            RB_REMOVE_COLOR(head, parent, child);
-        }
-
-        return old;
-    };
+template <typename T>
+requires HasRBEntry<T>
+constexpr T* RB_REMOVE(RBHead<T>& head, T* elm) {
+    T* child = nullptr;
+    T* parent = nullptr;
+    T* old = elm;
+    RBColor color = RBColor::RB_BLACK;
 
     if (RB_LEFT(elm) == nullptr) {
         child = RB_RIGHT(elm);
     } else if (RB_RIGHT(elm) == nullptr) {
         child = RB_LEFT(elm);
     } else {
-        Node* left;
+        T* left;
         elm = RB_RIGHT(elm);
         while ((left = RB_LEFT(elm)) != nullptr) {
             elm = left;
@@ -446,6 +405,7 @@ Node* RB_REMOVE(RBHead<Node>* head, Node* elm) {
         if (child) {
             RB_SET_PARENT(child, parent);
         }
+
         if (parent) {
             if (RB_LEFT(parent) == elm) {
                 RB_SET_LEFT(parent, child);
@@ -453,14 +413,14 @@ Node* RB_REMOVE(RBHead<Node>* head, Node* elm) {
                 RB_SET_RIGHT(parent, child);
             }
         } else {
-            head->SetRoot(child);
+            head.SetRoot(child);
         }
 
         if (RB_PARENT(elm) == old) {
             parent = elm;
         }
 
-        elm->SetEntry(old->GetEntry());
+        elm->SetRBEntry(old->GetRBEntry());
 
         if (RB_PARENT(old)) {
             if (RB_LEFT(RB_PARENT(old)) == old) {
@@ -469,17 +429,24 @@ Node* RB_REMOVE(RBHead<Node>* head, Node* elm) {
                 RB_SET_RIGHT(RB_PARENT(old), elm);
             }
         } else {
-            head->SetRoot(elm);
+            head.SetRoot(elm);
         }
+
         RB_SET_PARENT(RB_LEFT(old), elm);
+
         if (RB_RIGHT(old)) {
             RB_SET_PARENT(RB_RIGHT(old), elm);
         }
+
         if (parent) {
             left = parent;
         }
 
-        return finalize();
+        if (color == RBColor::RB_BLACK) {
+            RB_REMOVE_COLOR(head, parent, child);
+        }
+
+        return old;
     }
 
     parent = RB_PARENT(elm);
@@ -495,17 +462,69 @@ Node* RB_REMOVE(RBHead<Node>* head, Node* elm) {
             RB_SET_RIGHT(parent, child);
         }
     } else {
-        head->SetRoot(child);
+        head.SetRoot(child);
+    }
+
+    if (color == RBColor::RB_BLACK) {
+        RB_REMOVE_COLOR(head, parent, child);
+    }
+
+    return old;
+}
+
+template <typename T>
+requires HasRBEntry<T>
+constexpr void RB_INSERT_COLOR(RBHead<T>& head, T* elm) {
+    T *parent = nullptr, *tmp = nullptr;
+    while ((parent = RB_PARENT(elm)) != nullptr && RB_IS_RED(parent)) {
+        T* gparent = RB_PARENT(parent);
+        if (parent == RB_LEFT(gparent)) {
+            tmp = RB_RIGHT(gparent);
+            if (tmp && RB_IS_RED(tmp)) {
+                RB_SET_COLOR(tmp, RBColor::RB_BLACK);
+                RB_SET_BLACKRED(parent, gparent);
+                elm = gparent;
+                continue;
+            }
+
+            if (RB_RIGHT(parent) == elm) {
+                RB_ROTATE_LEFT(head, parent, tmp);
+                tmp = parent;
+                parent = elm;
+                elm = tmp;
+            }
+
+            RB_SET_BLACKRED(parent, gparent);
+            RB_ROTATE_RIGHT(head, gparent, tmp);
+        } else {
+            tmp = RB_LEFT(gparent);
+            if (tmp && RB_IS_RED(tmp)) {
+                RB_SET_COLOR(tmp, RBColor::RB_BLACK);
+                RB_SET_BLACKRED(parent, gparent);
+                elm = gparent;
+                continue;
+            }
+
+            if (RB_LEFT(parent) == elm) {
+                RB_ROTATE_RIGHT(head, parent, tmp);
+                tmp = parent;
+                parent = elm;
+                elm = tmp;
+            }
+
+            RB_SET_BLACKRED(parent, gparent);
+            RB_ROTATE_LEFT(head, gparent, tmp);
+        }
     }
 
-    return finalize();
+    RB_SET_COLOR(head.Root(), RBColor::RB_BLACK);
 }
 
-// Inserts a node into the RB tree
-template <typename Node, typename CompareFunction>
-Node* RB_INSERT(RBHead<Node>* head, Node* elm, CompareFunction cmp) {
-    Node* parent = nullptr;
-    Node* tmp = head->Root();
+template <typename T, typename Compare>
+requires HasRBEntry<T>
+constexpr T* RB_INSERT(RBHead<T>& head, T* elm, Compare cmp) {
+    T* parent = nullptr;
+    T* tmp = head.Root();
     int comp = 0;
 
     while (tmp) {
@@ -529,17 +548,17 @@ Node* RB_INSERT(RBHead<Node>* head, Node* elm, CompareFunction cmp) {
             RB_SET_RIGHT(parent, elm);
         }
     } else {
-        head->SetRoot(elm);
+        head.SetRoot(elm);
     }
 
     RB_INSERT_COLOR(head, elm);
     return nullptr;
 }
 
-// Finds the node with the same key as elm
-template <typename Node, typename CompareFunction>
-Node* RB_FIND(RBHead<Node>* head, Node* elm, CompareFunction cmp) {
-    Node* tmp = head->Root();
+template <typename T, typename Compare>
+requires HasRBEntry<T>
+constexpr T* RB_FIND(RBHead<T>& head, T* elm, Compare cmp) {
+    T* tmp = head.Root();
 
     while (tmp) {
         const int comp = cmp(elm, tmp);
@@ -555,11 +574,11 @@ Node* RB_FIND(RBHead<Node>* head, Node* elm, CompareFunction cmp) {
     return nullptr;
 }
 
-// Finds the first node greater than or equal to the search key
-template <typename Node, typename CompareFunction>
-Node* RB_NFIND(RBHead<Node>* head, Node* elm, CompareFunction cmp) {
-    Node* tmp = head->Root();
-    Node* res = nullptr;
+template <typename T, typename Compare>
+requires HasRBEntry<T>
+constexpr T* RB_NFIND(RBHead<T>& head, T* elm, Compare cmp) {
+    T* tmp = head.Root();
+    T* res = nullptr;
 
     while (tmp) {
         const int comp = cmp(elm, tmp);
@@ -576,13 +595,13 @@ Node* RB_NFIND(RBHead<Node>* head, Node* elm, CompareFunction cmp) {
     return res;
 }
 
-// Finds the node with the same key as lelm
-template <typename Node, typename CompareFunction>
-Node* RB_FIND_LIGHT(RBHead<Node>* head, const void* lelm, CompareFunction lcmp) {
-    Node* tmp = head->Root();
+template <typename T, typename U, typename Compare>
+requires HasRBEntry<T>
+constexpr T* RB_FIND_KEY(RBHead<T>& head, const U& key, Compare cmp) {
+    T* tmp = head.Root();
 
     while (tmp) {
-        const int comp = lcmp(lelm, tmp);
+        const int comp = cmp(key, tmp);
         if (comp < 0) {
             tmp = RB_LEFT(tmp);
         } else if (comp > 0) {
@@ -595,14 +614,14 @@ Node* RB_FIND_LIGHT(RBHead<Node>* head, const void* lelm, CompareFunction lcmp)
     return nullptr;
 }
 
-// Finds the first node greater than or equal to the search key
-template <typename Node, typename CompareFunction>
-Node* RB_NFIND_LIGHT(RBHead<Node>* head, const void* lelm, CompareFunction lcmp) {
-    Node* tmp = head->Root();
-    Node* res = nullptr;
+template <typename T, typename U, typename Compare>
+requires HasRBEntry<T>
+constexpr T* RB_NFIND_KEY(RBHead<T>& head, const U& key, Compare cmp) {
+    T* tmp = head.Root();
+    T* res = nullptr;
 
     while (tmp) {
-        const int comp = lcmp(lelm, tmp);
+        const int comp = cmp(key, tmp);
         if (comp < 0) {
             res = tmp;
             tmp = RB_LEFT(tmp);
@@ -616,8 +635,43 @@ Node* RB_NFIND_LIGHT(RBHead<Node>* head, const void* lelm, CompareFunction lcmp)
     return res;
 }
 
-template <typename Node>
-Node* RB_NEXT(Node* elm) {
+template <typename T, typename Compare>
+requires HasRBEntry<T>
+constexpr T* RB_FIND_EXISTING(RBHead<T>& head, T* elm, Compare cmp) {
+    T* tmp = head.Root();
+
+    while (true) {
+        const int comp = cmp(elm, tmp);
+        if (comp < 0) {
+            tmp = RB_LEFT(tmp);
+        } else if (comp > 0) {
+            tmp = RB_RIGHT(tmp);
+        } else {
+            return tmp;
+        }
+    }
+}
+
+template <typename T, typename U, typename Compare>
+requires HasRBEntry<T>
+constexpr T* RB_FIND_EXISTING_KEY(RBHead<T>& head, const U& key, Compare cmp) {
+    T* tmp = head.Root();
+
+    while (true) {
+        const int comp = cmp(key, tmp);
+        if (comp < 0) {
+            tmp = RB_LEFT(tmp);
+        } else if (comp > 0) {
+            tmp = RB_RIGHT(tmp);
+        } else {
+            return tmp;
+        }
+    }
+}
+
+template <typename T>
+requires HasRBEntry<T>
+constexpr T* RB_NEXT(T* elm) {
     if (RB_RIGHT(elm)) {
         elm = RB_RIGHT(elm);
         while (RB_LEFT(elm)) {
@@ -636,8 +690,9 @@ Node* RB_NEXT(Node* elm) {
     return elm;
 }
 
-template <typename Node>
-Node* RB_PREV(Node* elm) {
+template <typename T>
+requires HasRBEntry<T>
+constexpr T* RB_PREV(T* elm) {
     if (RB_LEFT(elm)) {
         elm = RB_LEFT(elm);
         while (RB_RIGHT(elm)) {
@@ -656,30 +711,32 @@ Node* RB_PREV(Node* elm) {
     return elm;
 }
 
-template <typename Node>
-Node* RB_MINMAX(RBHead<Node>* head, bool is_min) {
-    Node* tmp = head->Root();
-    Node* parent = nullptr;
+template <typename T>
+requires HasRBEntry<T>
+constexpr T* RB_MIN(RBHead<T>& head) {
+    T* tmp = head.Root();
+    T* parent = nullptr;
 
     while (tmp) {
         parent = tmp;
-        if (is_min) {
-            tmp = RB_LEFT(tmp);
-        } else {
-            tmp = RB_RIGHT(tmp);
-        }
+        tmp = RB_LEFT(tmp);
     }
 
     return parent;
 }
 
-template <typename Node>
-Node* RB_MIN(RBHead<Node>* head) {
-    return RB_MINMAX(head, true);
-}
+template <typename T>
+requires HasRBEntry<T>
+constexpr T* RB_MAX(RBHead<T>& head) {
+    T* tmp = head.Root();
+    T* parent = nullptr;
 
-template <typename Node>
-Node* RB_MAX(RBHead<Node>* head) {
-    return RB_MINMAX(head, false);
+    while (tmp) {
+        parent = tmp;
+        tmp = RB_RIGHT(tmp);
+    }
+
+    return parent;
 }
-} // namespace Common
+
+} // namespace Common::freebsd