<commit_before>// File created: 2012-11-08 11:26:46

#include <alloca.h>
#include <assert.h>
#include <string.h>

#define ABS(x) ((x) < 0 ? -(x) : (x))

typedef union r_elem {
   rtree    *branch_ptr;
   mushaabb *leaf_aabb_ptr;
} r_elem;

typedef struct r_insertee {
   R_DEPTH insert_depth;
   const mushbounds *bounds;
   r_elem elem;
} r_insertee;

////////////////////////////////////////// Iterator aux

// We use the given void* as a pointer to a buffer holding the path to the
// current iterator position. That is, a node and an index for each level of
// the tree but the last.
//
// The index is the index we descended into, so the following holds for all
// valid x and x+1:
//
//    path_nodes[x+1] == path_nodes[x]->branch_nodes[path_idxs[x]]

#define AUX_SIZEOF (sizeof(rtree*) + sizeof(R_IDX))

static inline void iter_aux_split(
   const mushboxen* boxen,
   void *a, rtree ***path_nodes, R_IDX **path_idxs)
{
   *path_nodes = (rtree**)a;
   *path_idxs  = (R_IDX*)((char*)a + boxen->max_depth * sizeof(rtree*));
}
size_t mushboxen_iter_aux_size(const mushboxen* boxen) {
   return boxen->max_depth * AUX_SIZEOF;
}

const size_t mushboxen_iter_aux_size_init = 16 * AUX_SIZEOF;

////////////////////////////////////////// Boxen main

bool mushboxen_init(mushboxen* boxen) {
   *boxen = (mushboxen){
      .max_depth  = 0,
      .count      = 0,
      .root.count = 0,
   };
   return true;
}

static void r_free(rtree* node) {
   if (node->count >= 0)
      for (R_IDX i = 0; i < node->count; ++i)
         free(node->leaf_aabbs[i].data);
   else {
      for (R_IDX i = 0, count = -node->count; i < count; ++i) {
         r_free(node->branch_nodes[i]);
         free(node->branch_nodes[i]);
      }
   }
}
void mushboxen_free(mushboxen* boxen) {
   r_free(&boxen->root);
}

static bool r_copy(rtree* copy, const rtree* orig) {
   memcpy(copy, orig, sizeof *orig);
   if (copy->count >= 0) {
      for (R_IDX i = 0; i < copy->count; ++i) {
         mushaabb *box = &copy->leaf_aabbs[i];
         const mushcell *orig_data = box->data;
         box->data = malloc(box->size * sizeof *box->data);
         if (!box->data) {
            while (i--)
               free(copy->leaf_aabbs[i].data);
            return false;
         }
         memcpy(box->data, orig_data, box->size * sizeof *box->data);
      }
   } else {
      for (R_IDX i = 0, count = -copy->count; i < count; ++i) {
         copy->branch_nodes[i] = malloc(sizeof *copy->branch_nodes[i]);
         if (!copy->branch_nodes[i]
          || !r_copy(copy->branch_nodes[i], orig->branch_nodes[i]))
         {
            for (R_IDX j = 0; j <= i; ++j)
               free(copy->branch_nodes[j]);
            return false;
         }
      }
   }
   return true;
}
bool mushboxen_copy(mushboxen* copy, const mushboxen* boxen) {
   memcpy(copy, boxen, sizeof *boxen);
   return r_copy(&copy->root, &boxen->root);
}

size_t mushboxen_count(const mushboxen* boxen) { return boxen->count; }

static mushaabb* r_get(const rtree* node, mushcoords pos) {
   for (R_IDX i = 0, count = ABS(node->count); i < count; ++i) {
      if (!mushbounds_contains(&node->bounds[i], pos))
         continue;
      if (node->count >= 0)
         return (mushaabb*)&node->leaf_aabbs[i];

      mushaabb *aabb = r_get(node->branch_nodes[i], pos);
      if (aabb)
         return aabb;
   }
   return NULL;
}
mushaabb* mushboxen_get(const mushboxen* boxen, mushcoords pos) {
   return r_get(&boxen->root, pos);
}

static mushboxen_iter r_get_iter(
   const rtree* node, R_DEPTH depth, mushcoords pos,
   const mushboxen* boxen, void* aux)
{
   rtree **path_nodes;
   R_IDX  *path_idxs;
   iter_aux_split(boxen, aux, &path_nodes, &path_idxs);

   for (R_IDX i = 0, count = ABS(node->count); i < count; ++i) {
      if (!mushbounds_contains(&node->bounds[i], pos))
         continue;
      if (node->count >= 0)
         return (mushboxen_iter){ (rtree*)node, i, depth, aux };

      mushboxen_iter iter =
         r_get_iter(node->branch_nodes[i], depth + 1, pos, boxen, aux);
      if (!mushboxen_iter_is_null(iter)) {
         path_nodes[depth] = (rtree*)node;
         path_idxs [depth] = i;
         return iter;
      }
   }
   return mushboxen_iter_null;
}
mushboxen_iter mushboxen_get_iter(
   const mushboxen* boxen, mushcoords pos, void* aux)
{
   return r_get_iter(&boxen->root, 0, pos, boxen, aux);
}

static bool r_contains_bounds(const rtree* node, const mushbounds* bs) {
   for (R_IDX i = 0, count = ABS(node->count); i < count; ++i) {
      if (!mushbounds_contains_bounds(&node->bounds[i], bs))
         continue;
      if (node->count >= 0 || r_contains_bounds(node->branch_nodes[i], bs))
         return true;
   }
   return false;
}
bool mushboxen_contains_bounds(const mushboxen* boxen, const mushbounds* bs) {
   return r_contains_bounds(&boxen->root, bs);
}

void mushboxen_loosen_bounds(const mushboxen* boxen, mushbounds* bounds) {
   for (R_IDX i = 0; i < ABS(boxen->root.count); ++i)
      mushbounds_expand_to_cover(bounds, &boxen->root.bounds[i]);
}

static void r_flatten_node(bool is_leaf, rtree* node, R_IDX count) {
   for (R_IDX i = 0, j = 0, remaining = count; remaining; ++i) {
      if (is_leaf ? !node->leaf_aabbs[i].data : !node->branch_nodes[i])
         continue;

      while (is_leaf ? !!node->leaf_aabbs[j].data : !!node->branch_nodes[j])
         if (++j == count)
            return;

      if (i >= j) {
         // i points to a nonnull box while j points to a null slot earlier on.
         // Move i to j.
         node->bounds[j] = node->bounds[i];
         if (is_leaf) {
            node->leaf_aabbs[j] = node->leaf_aabbs[i];
            node->leaf_aabbs[i].data = NULL;
         } else {
            node->branch_nodes[j] = node->branch_nodes[i];
            node->branch_nodes[i] = NULL;
         }
      }
      --remaining;
   }
}

// The new node will be T-below the old node.
static mushboxen_iter r_split_node(
   rtree *old, rtree *new, mushbounds* pold_cover, mushbounds* pnew_cover,
   const mushbounds* elem_bounds, r_elem elem)
{
   assert (ABS(old->count) == R_BRANCHING_FACTOR);

   const bool is_leaf = old->count >= 0;

#ifdef R_SPLIT_QUADRATIC
   // The classic quadratic algorithm from Guttman 1984: For each pair of
   // bounds, see how much "wasted" space remains in a box that encompasses
   // them both. Split the pair that wastes the most space into separate nodes.
   //
   // And our addition: make sure to preserve T-ordering. The way we do it
   // makes this actually cubic.

   // The transitive closure of T-aboveness. We need this because of the
   // following kind of situation:
   //
   // +----+
   // | A  |   +---+
   // |   +----|  +---+
   // +---| B  |C | D |
   //     +----|  +---+
   //          +---+
   //
   // Here A is T-below B which is T-below C which is T-below D.
   //
   // For example, if we put C in the new node, that directly also forces B
   // into the new node. But forcing B into the new node indirectly forces A
   // into the new node as well. We won't know that unless we look at
   // transitive T-aboveness from C.
   //
   // Putting C into the new node forces D to be either left of C in the new
   // node or anywhere in the old node. For this reason we preserve element
   // order in assignments: we do new[i] = old[i] instead of something like
   // new[n++] = old[i], and then flatten things at the end.
   //
   // Symmetrically, putting B into the old node, forces C and transitively D
   // into the old node. A is forced to be either right of B in the old node or
   // anywhere in the new node.
   //
   // tabove[i][j / TABOVE_BITS] & 1 << j % TABOVE_BITS) will be true if old[i]
   // is T-above old[j], or if old[i] is T-above some old[k] (...) which is
   // T-above old[j].
   //
   // We require one extra bit in TABOVE_BITS_LEN because we want to know which
   // boxes are T-above elem as well.
   const ptrdiff_t
      TABOVE_BITS      = CHAR_BIT * sizeof(uint_fast32_t),
      TABOVE_BITS_LEN = (R_BRANCHING_FACTOR+1 + TABOVE_BITS - 1) / TABOVE_BITS;

   uint_fast32_t tabove[R_BRANCHING_FACTOR][TABOVE_BITS_LEN];

   static const uint_fast32_t TABOVE_ONE = 1;

   #define IS_TABOVE(I, J) \
      (tabove[I][J / TABOVE_BITS] & TABOVE_ONE << J % TABOVE_BITS)

   #define SET_TABOVE(I, J) \
      (tabove[I][J / TABOVE_BITS] |= TABOVE_ONE << J % TABOVE_BITS)

   // Zero init.
   for (R_IDX i = 0; i < R_BRANCHING_FACTOR + 1; ++i)
      for (R_IDX j = 0; j < TABOVE_BITS_LEN; ++j)
         tabove[i][j] = 0;

   // Direct T-aboveness.
   for (R_IDX i = 0; i < R_BRANCHING_FACTOR; ++i) {
      // For convenience, we say that each box is T-above itself. This allows
      // NEXT_UNASS to avoid a comparison.
      SET_TABOVE(i, i);

      for (R_IDX j = i + 1; j < R_BRANCHING_FACTOR; ++j)
         if (mushbounds_overlaps(&old->bounds[i], &old->bounds[j]))
            SET_TABOVE(i, j);

      if (mushbounds_overlaps(&old->bounds[i], elem_bounds))
         SET_TABOVE(i, R_BRANCHING_FACTOR);
   }

   // The transitive closure.
   for (R_IDX i = 0; i < R_BRANCHING_FACTOR; ++i)
      for (R_IDX j = i + 1; j < R_BRANCHING_FACTOR; ++j)
         if (IS_TABOVE(i, j))
            for (R_IDX k = j + 1; k < R_BRANCHING_FACTOR + 1; ++k)
               tabove[i][k / TABOVE_BITS] |=
                  tabove[j][k / TABOVE_BITS] & TABOVE_ONE << k % TABOVE_BITS;

   // A value of R_BRANCHING_FACTOR refers to elem.
   //
   // This solution definitely works, since the number of boxes transitively
   // T-below max_waste_j is bounded from above by the number of valid indices
   // after max_waste_j plus one. (See the inequality in the loop below.) As
   // can be shown:
   //
   //    R_BRANCHING_FACTOR + 1 - #(T-below max_waste_j)
   // >= R_BRANCHING_FACTOR + 1 - (#(valid indices after max_waste_j]) + 1)
   // == R_BRANCHING_FACTOR + 1 - ((R_BRANCHING_FACTOR - 1 - max_waste_j) + 1)
   // == R_BRANCHING_FACTOR + 1 - (R_BRANCHING_FACTOR - max_waste_j)
   // == 1 + max_waste_j
   //
   // We want this to be >= R_MIN_XS_PER_NODE and different from max_waste_i,
   // so set max_waste_j = R_MIN_XS_PER_NODE.
   size_t max_waste = 0;
   R_IDX max_waste_i = 0,
         max_waste_j = R_MIN_XS_PER_NODE;

   for (R_IDX i = 0; i < R_BRANCHING_FACTOR; ++i) {
      const mushbounds *bounds_i = &old->bounds[i];
      const size_t size_i = is_leaf ? old->leaf_aabbs[i].size
                                    : mushbounds_size(bounds_i);

      bool i_tabove_checked = false;

      for (R_IDX j = i + 1; j < R_BRANCHING_FACTOR; ++j) {
         const mushbounds *bounds_j = &old->bounds[j];
         const size_t size_j = is_leaf ? old->leaf_aabbs[j].size
                                       : mushbounds_size(bounds_j);

         mushbounds bounds = *bounds_i;
         mushbounds_expand_to_cover(&bounds, bounds_j);

         const size_t together = mushbounds_clamped_size(&bounds),
                      waste    = together - size_i - size_j;
         if (waste <= max_waste)
            continue;

         // While this (putting i in old and j in new) is a better solution, it
         // may violate the R_MIN_XS_PER_NODE invariant when we take T-ordering
         // into consideration.
         //
         // This forces all boxes T-above i into the old node and all boxes
         // T-below j into the new node. So make sure that the following holds:
         //
         //    R_BRANCHING_FACTOR + 1 - #(T-below j) - 1 >= R_MIN_XS_PER_NODE
         //
         // I.e. that at least R_MIN_XS_PER_NODE are guaranteed to be available
         // for the old node. The +1 is due to the elem we're placing and the
         // -1 is due to j itself. They cancel each other out, leaving us with:
         //
         //    R_BRANCHING_FACTOR - #(T-below j) >= R_MIN_XS_PER_NODE
         //
         // Similarly, at least R_MIN_XS_PER_NODE need to be available for the
         // new node:
         //
         //    R_BRANCHING_FACTOR - #(T-above i) >= R_MIN_XS_PER_NODE
         //
         // To simplify the check, we transform it a bit. We want to make sure
         // the following never becomes true:
         //
         //    R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE - #(T-related) < 0
         if (!i_tabove_checked) {
            R_IDX check = R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE;
            for (R_IDX k = 0; k < i; ++k)
               if (IS_TABOVE(k, i) && !check--)
                   goto next_i;
            i_tabove_checked = true;
         }

         // There are R_BRANCHING_FACTOR - j - 1 possible boxes T-below j, plus
         // one for elem.
         //
         // In other words:
         //
         //    #(T-below j) <= R_BRANCHING_FACTOR - j
         //
         // Meanwhile, we want to ensure:
         //
         //    #(T-below j) <= R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE
         //
         // So if the following holds, this is guaranteed:
         //
         //    R_BRANCHING_FACTOR - j <= R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE
         //
         // I.e.:
         //
         //    - j <= - R_MIN_XS_PER_NODE
         //      j >=   R_MIN_XS_PER_NODE
         if (j < R_MIN_XS_PER_NODE) {
            R_IDX check = R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE;
            for (R_IDX k = j + 1; k <= R_BRANCHING_FACTOR; ++k)
               if (IS_TABOVE(j, k) && !check--)
                  goto next_j;
         }
         max_waste   = waste;
         max_waste_i = i;
         max_waste_j = j;
next_j:;
      }

      // elem also has a bound to be included. No boxes are T-below it, so we
      // don't need to worry about that.
      mushbounds bounds = *bounds_i;
      mushbounds_expand_to_cover(&bounds, elem_bounds);
      const size_t waste = mushbounds_clamped_size(&bounds)
                         - size_i
                         - (is_leaf ? elem.leaf_aabb_ptr->size
                                    : mushbounds_size(elem_bounds));
      if (waste <= max_waste)
         continue;
      if (!i_tabove_checked) {
         R_IDX check = R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE;
         for (R_IDX k = 0; k < i; ++k)
            if (IS_TABOVE(k, i) && !check--)
                goto next_i;
      }
      max_waste   = waste;
      max_waste_i = i;
      max_waste_j = R_BRANCHING_FACTOR;
next_i:;
   }

   bool assigned_elem = false,
        elem_in_old;

   // Assign max_waste_j to the new node.
   new->count = 1;
   mushbounds new_cover;
   if (max_waste_j == R_BRANCHING_FACTOR) {
      // elem must come last, so don't actually write it yet.
      new_cover = *elem_bounds;
      assigned_elem = true;
      elem_in_old   = false;
   } else {
      new->bounds[max_waste_j] = new_cover = old->bounds[max_waste_j];

      if (is_leaf) {
         new->leaf_aabbs[max_waste_j] = old->leaf_aabbs[max_waste_j];

         // Mark it as placed.
         old->leaf_aabbs[max_waste_j].data = NULL;
      } else {
         new->branch_nodes[max_waste_j] = old->branch_nodes[max_waste_j];
         old->branch_nodes[max_waste_j] = NULL;
      }

      // Assign all boxes transitively T-below max_waste_j to the new node as
      // well.
      for (R_IDX i = max_waste_j + 1; i < R_BRANCHING_FACTOR; ++i) {
         if (!IS_TABOVE(max_waste_j, i))
            continue;

         new->bounds[i] = old->bounds[i];

         if (is_leaf) {
            new->leaf_aabbs[i] = old->leaf_aabbs[i];
            old->leaf_aabbs[i].data = NULL;
         } else {
            new->branch_nodes[i] = old->branch_nodes[i];
            old->branch_nodes[i] = NULL;
         }

         mushbounds_expand_to_cover(&new_cover, &new->bounds[i]);
         ++new->count;
      }
      if (IS_TABOVE(max_waste_j, R_BRANCHING_FACTOR)) {
         assigned_elem = true;
         elem_in_old = false;
         mushbounds_expand_to_cover(&new_cover, elem_bounds);
         ++new->count;
      }
   }

   // Assign max_waste_i to the old node.
   //
   // At this point we start subverting T-aboveness, using it also to mark
   // boxes assigned to the old node.
   old->count = 1;
   mushbounds old_cover;
   if (max_waste_i == R_BRANCHING_FACTOR) {
      old_cover = *elem_bounds;
      assigned_elem = true;
      elem_in_old   = true;
   } else {
      old_cover = old->bounds[max_waste_i];
      if (is_leaf)
         new->leaf_aabbs[max_waste_i].data = NULL;
      else
         new->branch_nodes[max_waste_i] = NULL;
   }

   // Assign all boxes transitively T-above max_waste_i as well.
   for (R_IDX i = 0; i < max_waste_i; ++i) {
      if (!IS_TABOVE(i, max_waste_i))
         continue;
      SET_TABOVE(i, max_waste_i);
      mushbounds_expand_to_cover(&old_cover, &old->bounds[i]);
      ++old->count;

      if (is_leaf)
         new->leaf_aabbs[i].data = NULL;
      else
         new->branch_nodes[i] = NULL;
   }

   size_t old_cover_size = mushbounds_clamped_size(&old_cover),
          new_cover_size = mushbounds_clamped_size(&new_cover);

   for (R_IDX first_unass = 0, remaining;
        (remaining = R_BRANCHING_FACTOR - new->count - old->count + 1);)
   {
      // Move the given index to an unassigned box: one which is nonnull and
      // not T-above max_waste_i.
      #define NEXT_UNASS(i) \
         while (i < R_BRANCHING_FACTOR \
             && ((is_leaf ? !old->leaf_aabbs[i].data : !old->branch_nodes[i]) \
              || IS_TABOVE(i, max_waste_i))) \
            ++i;

      NEXT_UNASS(first_unass);

      assert ((first_unass < R_BRANCHING_FACTOR || !assigned_elem)
           && "need to assign more, but nothing left");

      if (old->count + remaining == R_MIN_XS_PER_NODE) {
         // Assign the rest to old.
         if (!assigned_elem) {
            assigned_elem = true;
            elem_in_old   = true;
            mushbounds_expand_to_cover(&old_cover, elem_bounds);
            ++old->count;
         }
         for (R_IDX i = first_unass; i < R_BRANCHING_FACTOR;) {
            mushbounds_expand_to_cover(&old_cover, &old->bounds[i]);
            ++old->count;
            if (is_leaf)
               new->leaf_aabbs[i].data = NULL;
            else
               new->branch_nodes[i] = NULL;
            ++i;
            NEXT_UNASS(i);
         }
         break;
      }

      if (new->count + remaining == R_MIN_XS_PER_NODE) {
         // Assign the rest to new.
         if (!assigned_elem) {
            assigned_elem = true;
            elem_in_old   = false;
            mushbounds_expand_to_cover(&new_cover, elem_bounds);
            ++new->count;
         }
         for (R_IDX i = first_unass; i < R_BRANCHING_FACTOR;) {
            new->bounds[i] = old->bounds[i];
            if (is_leaf) {
               new->leaf_aabbs[i] = old->leaf_aabbs[i];
               old->leaf_aabbs[i].data = NULL;
            } else {
               new->branch_nodes[i] = old->branch_nodes[i];
               old->branch_nodes[i] = NULL;
            }
            mushbounds_expand_to_cover(&new_cover, &new->bounds[i]);
            ++new->count;
            ++i;
            NEXT_UNASS(i);
         }
         break;
      }

      // Select the next box to place: take the one with the highest preference
      // for either node, i.e. with the highest difference between how much it
      // increases the size of each node's covering bounds. Place it in the
      // node whose covering bounds will be enlarged the least.
      //
      // That is, if T-ordering is not taken into account. Since T-ordering
      // forces some placements, if placing a box in one node would force the
      // other one's count below R_MIN_XS_PER_NODE, we have no choice but to
      // put that box in the other node.

      size_t            picked_ddiff = 0;
      R_IDX             picked_idx;
      r_elem            picked_elem;
      const mushbounds *picked_bounds;
      bool              picked_old;
      mushbounds        picked_cover;
      size_t            picked_cover_size;

      #define TRY_PICK_NEXT(BOUNDS, IDX, ELEM) do { \
         mushbounds old_cover2 = old_cover, \
                    new_cover2 = new_cover; \
         mushbounds_expand_to_cover(&old_cover2, BOUNDS); \
         mushbounds_expand_to_cover(&new_cover2, BOUNDS); \
         const size_t old_cover2_size = mushbounds_clamped_size(&old_cover2), \
                      new_cover2_size = mushbounds_clamped_size(&new_cover2), \
                      old_diff        = old_cover2_size - old_cover_size, \
                      new_diff        = new_cover2_size - new_cover_size, \
                      ddiff = ABS((ptrdiff_t)new_diff - (ptrdiff_t)old_diff); \
         \
         if (ddiff > picked_ddiff || !picked_ddiff) { \
            picked_ddiff      = ddiff; \
            picked_idx        = IDX; \
            picked_elem       = ELEM; \
            picked_bounds     = BOUNDS; \
            picked_old        = old_diff <= new_diff; \
            picked_cover      = picked_old ? old_cover2 : new_cover2; \
            picked_cover_size = \
               picked_old ? old_cover2_size : new_cover2_size; \
         } \
      } while (0)

      for (R_IDX i = first_unass; i < R_BRANCHING_FACTOR;) {
         const r_elem elem_i =
            is_leaf ? (r_elem){ .leaf_aabb_ptr = &old->leaf_aabbs  [i] }
                    : (r_elem){ .branch_ptr    =  old->branch_nodes[i] };

         R_IDX tabove_forced = 0,
               tbelow_forced = 0;
         for (R_IDX j = first_unass; j < R_BRANCHING_FACTOR;) {
            if (IS_TABOVE(j, i))
               ++tabove_forced;
            if (IS_TABOVE(i, j))
               ++tbelow_forced;
            ++j;
            NEXT_UNASS(j);
         }
         if (!assigned_elem && IS_TABOVE(i, R_BRANCHING_FACTOR))
            ++tbelow_forced;

         // If the number of unassigned boxes transitively T-above this one
         // would force new->count too low, this must be placed in new.
         if (new->count + remaining - tabove_forced - 1 < R_MIN_XS_PER_NODE) {
            picked_idx        = i;
            picked_elem       = elem_i;
            picked_bounds     = &old->bounds[i];
            picked_cover      = new_cover;
            picked_cover_size = 0;
            mushbounds_expand_to_cover(&picked_cover, picked_bounds);
            goto forced_new;
         }

         // And similarly, if the number of unassigned boxes transitively
         // T-below this one would force old->count too low, this must be
         // placed in old.
         if (old->count + remaining - tbelow_forced - 1 < R_MIN_XS_PER_NODE) {
            // Be sneaky and only write those variables which are used in the
            // picked_old case.
            picked_idx        = i;
            picked_cover      = old_cover;
            picked_cover_size = 0;
            mushbounds_expand_to_cover(&picked_cover, &old->bounds[i]);
            goto forced_old;
         }

         TRY_PICK_NEXT(&old->bounds[i], i, elem_i);
         ++i;
         NEXT_UNASS(i);
      }
      if (!assigned_elem) {
         // elem can only force things that are T-above it.
         R_IDX tabove_forced = 0;
         for (R_IDX j = first_unass; j < R_BRANCHING_FACTOR;) {
            if (IS_TABOVE(j, R_BRANCHING_FACTOR))
               ++tabove_forced;
            ++j;
            NEXT_UNASS(j);
         }

         if (new->count + remaining - tabove_forced - 1 < R_MIN_XS_PER_NODE) {
            picked_idx        = R_BRANCHING_FACTOR;
            picked_elem       = elem;
            picked_bounds     = elem_bounds;
            picked_cover      = new_cover;
            picked_cover_size = 0;
            mushbounds_expand_to_cover(&picked_cover, picked_bounds);
            goto forced_elem_new;
         }

         TRY_PICK_NEXT(elem_bounds, R_BRANCHING_FACTOR, elem);
      }

      if (picked_old) {
         if (picked_idx == R_BRANCHING_FACTOR) {
            assigned_elem = true;
            elem_in_old   = true;
         } else {
forced_old:
            SET_TABOVE(picked_idx, max_waste_i);

            if (is_leaf)
               new->leaf_aabbs[picked_idx].data = NULL;
            else
               new->branch_nodes[picked_idx] = NULL;
         }
         ++old->count;
         old_cover = picked_cover;

         // Putting this one in old also means that all boxes transitively
         // T-above it must stay in old.
         for (R_IDX i = first_unass; i < picked_idx;) {
            if (IS_TABOVE(i, picked_idx)) {
               SET_TABOVE(i, max_waste_i);

               if (is_leaf)
                  new->leaf_aabbs[i].data = NULL;
               else
                  new->branch_nodes[i] = NULL;

               mushbounds_expand_to_cover(&old_cover, &old->bounds[i]);
               ++old->count;

               // Unset this so that we know we have to recompute it below.
               picked_cover_size = 0;
            }
            ++i;
            NEXT_UNASS(i);
         }
         old_cover_size =
            picked_cover_size ? picked_cover_size
                              : mushbounds_clamped_size(&old_cover);
      } else {
         if (picked_idx == R_BRANCHING_FACTOR) {
forced_elem_new:
            assigned_elem = true;
            elem_in_old   = false;
         } else {
forced_new:
            new->bounds[picked_idx] = *picked_bounds;
            if (is_leaf) {
               new->leaf_aabbs[picked_idx] = *picked_elem.leaf_aabb_ptr;
               old->leaf_aabbs[picked_idx].data = NULL;
            } else {
               new->branch_nodes[picked_idx] = picked_elem.branch_ptr;
               old->branch_nodes[picked_idx] = NULL;
            }
         }
         ++new->count;
         new_cover = picked_cover;

         // Putting this one in new also means that all boxes transitively
         // T-below it must be put in new.
         R_IDX i = picked_idx + 1;
         NEXT_UNASS(i);
         while (i < R_BRANCHING_FACTOR) {
            if (IS_TABOVE(picked_idx, i)) {
               new->bounds[i] = old->bounds[i];
               if (is_leaf) {
                  new->leaf_aabbs[i] = old->leaf_aabbs[i];
                  old->leaf_aabbs[i].data = NULL;
               } else {
                  new->branch_nodes[i] = old->branch_nodes[i];
                  old->branch_nodes[i] = NULL;
               }
               mushbounds_expand_to_cover(&new_cover, &old->bounds[i]);
               ++new->count;
               picked_cover_size = 0;
            }
            ++i;
            NEXT_UNASS(i);
         }
         if (!assigned_elem && IS_TABOVE(picked_idx, R_BRANCHING_FACTOR)) {
            assigned_elem = true;
            elem_in_old   = false;
            mushbounds_expand_to_cover(&new_cover, elem_bounds);
            ++new->count;
            picked_cover_size = 0;
         }
         new_cover_size =
            picked_cover_size ? picked_cover_size
                              : mushbounds_clamped_size(&new_cover);
      }
      #undef NEXT_UNASS
      #undef TRY_PICK_NEXT
   }

   assert (old->count - 1 + new->count == R_BRANCHING_FACTOR);
   assert (assigned_elem);
   assert (old->count >= R_MIN_XS_PER_NODE);
   assert (new->count >= R_MIN_XS_PER_NODE);

   *pold_cover = old_cover;
   *pnew_cover = new_cover;

   // Both nodes still have nullified entries interspersed among their real
   // entries, so move everything in them to the beginning.
   r_flatten_node(is_leaf, old, old->count -  elem_in_old);
   r_flatten_node(is_leaf, new, new->count - !elem_in_old);

   // Place elem wherever it belongs. Don't increment the node's count: that's
   // already been done.

   mushboxen_iter iter;

   iter.node = elem_in_old ? old : new;
   iter.idx  = iter.node->count - 1;
   iter.node->bounds[iter.idx] = *elem_bounds;
   if (is_leaf)
      iter.node->leaf_aabbs[iter.idx] = *elem.leaf_aabb_ptr;
   else
      iter.node->branch_nodes[iter.idx] = elem.branch_ptr;

#ifdef MUSH_ENABLE_EXPENSIVE_DEBUGGING
   for (R_IDX i = 0; i < old->count; ++i) {
      assert (mushbounds_contains_bounds(pold_cover, &old->bounds[i]));
      if (is_leaf) assert (old->leaf_aabbs  [i].data != NULL);
      else         assert (old->branch_nodes[i]      != NULL);
   }
   for (R_IDX i = 0; i < new->count; ++i) {
      assert (mushbounds_contains_bounds(pnew_cover, &new->bounds[i]));
      if (is_leaf) assert (new->leaf_aabbs  [i].data != NULL);
      else         assert (new->branch_nodes[i]      != NULL);
   }
#endif

   if (!is_leaf) {
      old->count *= -1;
      new->count *= -1;
   }
   return iter;
   #undef IS_TABOVE
   #undef SET_TABOVE
#else
#error Only the quadratic split algorithm is implemented!
#endif
}

// FIXME: handle malloc failure, gracefully
static mushboxen_iter r_insert(
   r_insertee insertee, rtree *node, rtree **path_nodes, R_IDX *path_idxs,
   mushbounds *node_cover, rtree** split, mushbounds *split_cover)
{
   r_elem ins_here;
   const mushbounds *ins_here_bounds;
   mushbounds ins_here_bounds_st;

   R_IDX child_idx;

   mushboxen_iter iter;

   R_IDX abs_count = ABS(node->count);

   if (insertee.insert_depth == 0) {
      ins_here        = insertee.elem;
      ins_here_bounds = insertee.bounds;
   } else {
      // Select the child to recurse into: the one whose bounds are already
      // closest to insertee.bounds, resolving ties by size.
      //
      // But if any child overlaps with insertee.bounds, to preserve
      // T-ordering, the "rightmost" overlapping child is selected.

      size_t best_size = SIZE_MAX,
             best_diff = SIZE_MAX;

      for (R_IDX i = abs_count; i--;) {
         mushbounds bounds = node->bounds[i];
         if (mushbounds_overlaps(insertee.bounds, &bounds)) {
            child_idx = i;
            break;
         }

         const size_t bounds_size = mushbounds_size(&bounds);

         mushbounds_expand_to_cover(&bounds, insertee.bounds);
         const size_t exp_size = mushbounds_clamped_size(&bounds);

         const size_t diff = exp_size - bounds_size;
         if (diff < best_diff || (diff == best_diff && exp_size < best_size)) {
            best_diff = diff;
            best_size = exp_size;
            child_idx = i;
         }
      }

      --insertee.insert_depth;

      mushbounds child_cover;
      iter = r_insert(insertee, node->branch_nodes[child_idx],
                      path_nodes + 1, path_idxs + 1,
                      &child_cover, &ins_here.branch_ptr, &ins_here_bounds_st);
      if (mushboxen_iter_is_null(iter))
         return iter;

      ++iter.path_depth;

      // Write the relevant auxiliary info. Since we move the pointers on every
      // recursive call, no indexing is needed.
      *path_nodes = node;
      *path_idxs  = child_idx;

      // Reset to the insert depth of this call, since we still use it below.
      ++insertee.insert_depth;

      node->bounds[child_idx] = child_cover;

      if (!ins_here.branch_ptr) {
         // No split happened below: we're done.
         goto no_split_done;
      }
      ins_here_bounds = &ins_here_bounds_st;
   }

   if (abs_count == R_BRANCHING_FACTOR) {<commit_msg>boxen/r: simplify insert: remove useless temporary<commit_after>// File created: 2012-11-08 11:26:46

#include <alloca.h>
#include <assert.h>
#include <string.h>

#define ABS(x) ((x) < 0 ? -(x) : (x))

typedef union r_elem {
   rtree    *branch_ptr;
   mushaabb *leaf_aabb_ptr;
} r_elem;

typedef struct r_insertee {
   R_DEPTH insert_depth;
   const mushbounds *bounds;
   r_elem elem;
} r_insertee;

////////////////////////////////////////// Iterator aux

// We use the given void* as a pointer to a buffer holding the path to the
// current iterator position. That is, a node and an index for each level of
// the tree but the last.
//
// The index is the index we descended into, so the following holds for all
// valid x and x+1:
//
//    path_nodes[x+1] == path_nodes[x]->branch_nodes[path_idxs[x]]

#define AUX_SIZEOF (sizeof(rtree*) + sizeof(R_IDX))

static inline void iter_aux_split(
   const mushboxen* boxen,
   void *a, rtree ***path_nodes, R_IDX **path_idxs)
{
   *path_nodes = (rtree**)a;
   *path_idxs  = (R_IDX*)((char*)a + boxen->max_depth * sizeof(rtree*));
}
size_t mushboxen_iter_aux_size(const mushboxen* boxen) {
   return boxen->max_depth * AUX_SIZEOF;
}

const size_t mushboxen_iter_aux_size_init = 16 * AUX_SIZEOF;

////////////////////////////////////////// Boxen main

bool mushboxen_init(mushboxen* boxen) {
   *boxen = (mushboxen){
      .max_depth  = 0,
      .count      = 0,
      .root.count = 0,
   };
   return true;
}

static void r_free(rtree* node) {
   if (node->count >= 0)
      for (R_IDX i = 0; i < node->count; ++i)
         free(node->leaf_aabbs[i].data);
   else {
      for (R_IDX i = 0, count = -node->count; i < count; ++i) {
         r_free(node->branch_nodes[i]);
         free(node->branch_nodes[i]);
      }
   }
}
void mushboxen_free(mushboxen* boxen) {
   r_free(&boxen->root);
}

static bool r_copy(rtree* copy, const rtree* orig) {
   memcpy(copy, orig, sizeof *orig);
   if (copy->count >= 0) {
      for (R_IDX i = 0; i < copy->count; ++i) {
         mushaabb *box = &copy->leaf_aabbs[i];
         const mushcell *orig_data = box->data;
         box->data = malloc(box->size * sizeof *box->data);
         if (!box->data) {
            while (i--)
               free(copy->leaf_aabbs[i].data);
            return false;
         }
         memcpy(box->data, orig_data, box->size * sizeof *box->data);
      }
   } else {
      for (R_IDX i = 0, count = -copy->count; i < count; ++i) {
         copy->branch_nodes[i] = malloc(sizeof *copy->branch_nodes[i]);
         if (!copy->branch_nodes[i]
          || !r_copy(copy->branch_nodes[i], orig->branch_nodes[i]))
         {
            for (R_IDX j = 0; j <= i; ++j)
               free(copy->branch_nodes[j]);
            return false;
         }
      }
   }
   return true;
}
bool mushboxen_copy(mushboxen* copy, const mushboxen* boxen) {
   memcpy(copy, boxen, sizeof *boxen);
   return r_copy(&copy->root, &boxen->root);
}

size_t mushboxen_count(const mushboxen* boxen) { return boxen->count; }

static mushaabb* r_get(const rtree* node, mushcoords pos) {
   for (R_IDX i = 0, count = ABS(node->count); i < count; ++i) {
      if (!mushbounds_contains(&node->bounds[i], pos))
         continue;
      if (node->count >= 0)
         return (mushaabb*)&node->leaf_aabbs[i];

      mushaabb *aabb = r_get(node->branch_nodes[i], pos);
      if (aabb)
         return aabb;
   }
   return NULL;
}
mushaabb* mushboxen_get(const mushboxen* boxen, mushcoords pos) {
   return r_get(&boxen->root, pos);
}

static mushboxen_iter r_get_iter(
   const rtree* node, R_DEPTH depth, mushcoords pos,
   const mushboxen* boxen, void* aux)
{
   rtree **path_nodes;
   R_IDX  *path_idxs;
   iter_aux_split(boxen, aux, &path_nodes, &path_idxs);

   for (R_IDX i = 0, count = ABS(node->count); i < count; ++i) {
      if (!mushbounds_contains(&node->bounds[i], pos))
         continue;
      if (node->count >= 0)
         return (mushboxen_iter){ (rtree*)node, i, depth, aux };

      mushboxen_iter iter =
         r_get_iter(node->branch_nodes[i], depth + 1, pos, boxen, aux);
      if (!mushboxen_iter_is_null(iter)) {
         path_nodes[depth] = (rtree*)node;
         path_idxs [depth] = i;
         return iter;
      }
   }
   return mushboxen_iter_null;
}
mushboxen_iter mushboxen_get_iter(
   const mushboxen* boxen, mushcoords pos, void* aux)
{
   return r_get_iter(&boxen->root, 0, pos, boxen, aux);
}

static bool r_contains_bounds(const rtree* node, const mushbounds* bs) {
   for (R_IDX i = 0, count = ABS(node->count); i < count; ++i) {
      if (!mushbounds_contains_bounds(&node->bounds[i], bs))
         continue;
      if (node->count >= 0 || r_contains_bounds(node->branch_nodes[i], bs))
         return true;
   }
   return false;
}
bool mushboxen_contains_bounds(const mushboxen* boxen, const mushbounds* bs) {
   return r_contains_bounds(&boxen->root, bs);
}

void mushboxen_loosen_bounds(const mushboxen* boxen, mushbounds* bounds) {
   for (R_IDX i = 0; i < ABS(boxen->root.count); ++i)
      mushbounds_expand_to_cover(bounds, &boxen->root.bounds[i]);
}

static void r_flatten_node(bool is_leaf, rtree* node, R_IDX count) {
   for (R_IDX i = 0, j = 0, remaining = count; remaining; ++i) {
      if (is_leaf ? !node->leaf_aabbs[i].data : !node->branch_nodes[i])
         continue;

      while (is_leaf ? !!node->leaf_aabbs[j].data : !!node->branch_nodes[j])
         if (++j == count)
            return;

      if (i >= j) {
         // i points to a nonnull box while j points to a null slot earlier on.
         // Move i to j.
         node->bounds[j] = node->bounds[i];
         if (is_leaf) {
            node->leaf_aabbs[j] = node->leaf_aabbs[i];
            node->leaf_aabbs[i].data = NULL;
         } else {
            node->branch_nodes[j] = node->branch_nodes[i];
            node->branch_nodes[i] = NULL;
         }
      }
      --remaining;
   }
}

// The new node will be T-below the old node.
static mushboxen_iter r_split_node(
   rtree *old, rtree *new, mushbounds* pold_cover, mushbounds* pnew_cover,
   const mushbounds* elem_bounds, r_elem elem)
{
   assert (ABS(old->count) == R_BRANCHING_FACTOR);

   const bool is_leaf = old->count >= 0;

#ifdef R_SPLIT_QUADRATIC
   // The classic quadratic algorithm from Guttman 1984: For each pair of
   // bounds, see how much "wasted" space remains in a box that encompasses
   // them both. Split the pair that wastes the most space into separate nodes.
   //
   // And our addition: make sure to preserve T-ordering. The way we do it
   // makes this actually cubic.

   // The transitive closure of T-aboveness. We need this because of the
   // following kind of situation:
   //
   // +----+
   // | A  |   +---+
   // |   +----|  +---+
   // +---| B  |C | D |
   //     +----|  +---+
   //          +---+
   //
   // Here A is T-below B which is T-below C which is T-below D.
   //
   // For example, if we put C in the new node, that directly also forces B
   // into the new node. But forcing B into the new node indirectly forces A
   // into the new node as well. We won't know that unless we look at
   // transitive T-aboveness from C.
   //
   // Putting C into the new node forces D to be either left of C in the new
   // node or anywhere in the old node. For this reason we preserve element
   // order in assignments: we do new[i] = old[i] instead of something like
   // new[n++] = old[i], and then flatten things at the end.
   //
   // Symmetrically, putting B into the old node, forces C and transitively D
   // into the old node. A is forced to be either right of B in the old node or
   // anywhere in the new node.
   //
   // tabove[i][j / TABOVE_BITS] & 1 << j % TABOVE_BITS) will be true if old[i]
   // is T-above old[j], or if old[i] is T-above some old[k] (...) which is
   // T-above old[j].
   //
   // We require one extra bit in TABOVE_BITS_LEN because we want to know which
   // boxes are T-above elem as well.
   const ptrdiff_t
      TABOVE_BITS      = CHAR_BIT * sizeof(uint_fast32_t),
      TABOVE_BITS_LEN = (R_BRANCHING_FACTOR+1 + TABOVE_BITS - 1) / TABOVE_BITS;

   uint_fast32_t tabove[R_BRANCHING_FACTOR][TABOVE_BITS_LEN];

   static const uint_fast32_t TABOVE_ONE = 1;

   #define IS_TABOVE(I, J) \
      (tabove[I][J / TABOVE_BITS] & TABOVE_ONE << J % TABOVE_BITS)

   #define SET_TABOVE(I, J) \
      (tabove[I][J / TABOVE_BITS] |= TABOVE_ONE << J % TABOVE_BITS)

   // Zero init.
   for (R_IDX i = 0; i < R_BRANCHING_FACTOR + 1; ++i)
      for (R_IDX j = 0; j < TABOVE_BITS_LEN; ++j)
         tabove[i][j] = 0;

   // Direct T-aboveness.
   for (R_IDX i = 0; i < R_BRANCHING_FACTOR; ++i) {
      // For convenience, we say that each box is T-above itself. This allows
      // NEXT_UNASS to avoid a comparison.
      SET_TABOVE(i, i);

      for (R_IDX j = i + 1; j < R_BRANCHING_FACTOR; ++j)
         if (mushbounds_overlaps(&old->bounds[i], &old->bounds[j]))
            SET_TABOVE(i, j);

      if (mushbounds_overlaps(&old->bounds[i], elem_bounds))
         SET_TABOVE(i, R_BRANCHING_FACTOR);
   }

   // The transitive closure.
   for (R_IDX i = 0; i < R_BRANCHING_FACTOR; ++i)
      for (R_IDX j = i + 1; j < R_BRANCHING_FACTOR; ++j)
         if (IS_TABOVE(i, j))
            for (R_IDX k = j + 1; k < R_BRANCHING_FACTOR + 1; ++k)
               tabove[i][k / TABOVE_BITS] |=
                  tabove[j][k / TABOVE_BITS] & TABOVE_ONE << k % TABOVE_BITS;

   // A value of R_BRANCHING_FACTOR refers to elem.
   //
   // This solution definitely works, since the number of boxes transitively
   // T-below max_waste_j is bounded from above by the number of valid indices
   // after max_waste_j plus one. (See the inequality in the loop below.) As
   // can be shown:
   //
   //    R_BRANCHING_FACTOR + 1 - #(T-below max_waste_j)
   // >= R_BRANCHING_FACTOR + 1 - (#(valid indices after max_waste_j]) + 1)
   // == R_BRANCHING_FACTOR + 1 - ((R_BRANCHING_FACTOR - 1 - max_waste_j) + 1)
   // == R_BRANCHING_FACTOR + 1 - (R_BRANCHING_FACTOR - max_waste_j)
   // == 1 + max_waste_j
   //
   // We want this to be >= R_MIN_XS_PER_NODE and different from max_waste_i,
   // so set max_waste_j = R_MIN_XS_PER_NODE.
   size_t max_waste = 0;
   R_IDX max_waste_i = 0,
         max_waste_j = R_MIN_XS_PER_NODE;

   for (R_IDX i = 0; i < R_BRANCHING_FACTOR; ++i) {
      const mushbounds *bounds_i = &old->bounds[i];
      const size_t size_i = is_leaf ? old->leaf_aabbs[i].size
                                    : mushbounds_size(bounds_i);

      bool i_tabove_checked = false;

      for (R_IDX j = i + 1; j < R_BRANCHING_FACTOR; ++j) {
         const mushbounds *bounds_j = &old->bounds[j];
         const size_t size_j = is_leaf ? old->leaf_aabbs[j].size
                                       : mushbounds_size(bounds_j);

         mushbounds bounds = *bounds_i;
         mushbounds_expand_to_cover(&bounds, bounds_j);

         const size_t together = mushbounds_clamped_size(&bounds),
                      waste    = together - size_i - size_j;
         if (waste <= max_waste)
            continue;

         // While this (putting i in old and j in new) is a better solution, it
         // may violate the R_MIN_XS_PER_NODE invariant when we take T-ordering
         // into consideration.
         //
         // This forces all boxes T-above i into the old node and all boxes
         // T-below j into the new node. So make sure that the following holds:
         //
         //    R_BRANCHING_FACTOR + 1 - #(T-below j) - 1 >= R_MIN_XS_PER_NODE
         //
         // I.e. that at least R_MIN_XS_PER_NODE are guaranteed to be available
         // for the old node. The +1 is due to the elem we're placing and the
         // -1 is due to j itself. They cancel each other out, leaving us with:
         //
         //    R_BRANCHING_FACTOR - #(T-below j) >= R_MIN_XS_PER_NODE
         //
         // Similarly, at least R_MIN_XS_PER_NODE need to be available for the
         // new node:
         //
         //    R_BRANCHING_FACTOR - #(T-above i) >= R_MIN_XS_PER_NODE
         //
         // To simplify the check, we transform it a bit. We want to make sure
         // the following never becomes true:
         //
         //    R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE - #(T-related) < 0
         if (!i_tabove_checked) {
            R_IDX check = R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE;
            for (R_IDX k = 0; k < i; ++k)
               if (IS_TABOVE(k, i) && !check--)
                   goto next_i;
            i_tabove_checked = true;
         }

         // There are R_BRANCHING_FACTOR - j - 1 possible boxes T-below j, plus
         // one for elem.
         //
         // In other words:
         //
         //    #(T-below j) <= R_BRANCHING_FACTOR - j
         //
         // Meanwhile, we want to ensure:
         //
         //    #(T-below j) <= R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE
         //
         // So if the following holds, this is guaranteed:
         //
         //    R_BRANCHING_FACTOR - j <= R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE
         //
         // I.e.:
         //
         //    - j <= - R_MIN_XS_PER_NODE
         //      j >=   R_MIN_XS_PER_NODE
         if (j < R_MIN_XS_PER_NODE) {
            R_IDX check = R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE;
            for (R_IDX k = j + 1; k <= R_BRANCHING_FACTOR; ++k)
               if (IS_TABOVE(j, k) && !check--)
                  goto next_j;
         }
         max_waste   = waste;
         max_waste_i = i;
         max_waste_j = j;
next_j:;
      }

      // elem also has a bound to be included. No boxes are T-below it, so we
      // don't need to worry about that.
      mushbounds bounds = *bounds_i;
      mushbounds_expand_to_cover(&bounds, elem_bounds);
      const size_t waste = mushbounds_clamped_size(&bounds)
                         - size_i
                         - (is_leaf ? elem.leaf_aabb_ptr->size
                                    : mushbounds_size(elem_bounds));
      if (waste <= max_waste)
         continue;
      if (!i_tabove_checked) {
         R_IDX check = R_BRANCHING_FACTOR - R_MIN_XS_PER_NODE;
         for (R_IDX k = 0; k < i; ++k)
            if (IS_TABOVE(k, i) && !check--)
                goto next_i;
      }
      max_waste   = waste;
      max_waste_i = i;
      max_waste_j = R_BRANCHING_FACTOR;
next_i:;
   }

   bool assigned_elem = false,
        elem_in_old;

   // Assign max_waste_j to the new node.
   new->count = 1;
   mushbounds new_cover;
   if (max_waste_j == R_BRANCHING_FACTOR) {
      // elem must come last, so don't actually write it yet.
      new_cover = *elem_bounds;
      assigned_elem = true;
      elem_in_old   = false;
   } else {
      new->bounds[max_waste_j] = new_cover = old->bounds[max_waste_j];

      if (is_leaf) {
         new->leaf_aabbs[max_waste_j] = old->leaf_aabbs[max_waste_j];

         // Mark it as placed.
         old->leaf_aabbs[max_waste_j].data = NULL;
      } else {
         new->branch_nodes[max_waste_j] = old->branch_nodes[max_waste_j];
         old->branch_nodes[max_waste_j] = NULL;
      }

      // Assign all boxes transitively T-below max_waste_j to the new node as
      // well.
      for (R_IDX i = max_waste_j + 1; i < R_BRANCHING_FACTOR; ++i) {
         if (!IS_TABOVE(max_waste_j, i))
            continue;

         new->bounds[i] = old->bounds[i];

         if (is_leaf) {
            new->leaf_aabbs[i] = old->leaf_aabbs[i];
            old->leaf_aabbs[i].data = NULL;
         } else {
            new->branch_nodes[i] = old->branch_nodes[i];
            old->branch_nodes[i] = NULL;
         }

         mushbounds_expand_to_cover(&new_cover, &new->bounds[i]);
         ++new->count;
      }
      if (IS_TABOVE(max_waste_j, R_BRANCHING_FACTOR)) {
         assigned_elem = true;
         elem_in_old = false;
         mushbounds_expand_to_cover(&new_cover, elem_bounds);
         ++new->count;
      }
   }

   // Assign max_waste_i to the old node.
   //
   // At this point we start subverting T-aboveness, using it also to mark
   // boxes assigned to the old node.
   old->count = 1;
   mushbounds old_cover;
   if (max_waste_i == R_BRANCHING_FACTOR) {
      old_cover = *elem_bounds;
      assigned_elem = true;
      elem_in_old   = true;
   } else {
      old_cover = old->bounds[max_waste_i];
      if (is_leaf)
         new->leaf_aabbs[max_waste_i].data = NULL;
      else
         new->branch_nodes[max_waste_i] = NULL;
   }

   // Assign all boxes transitively T-above max_waste_i as well.
   for (R_IDX i = 0; i < max_waste_i; ++i) {
      if (!IS_TABOVE(i, max_waste_i))
         continue;
      SET_TABOVE(i, max_waste_i);
      mushbounds_expand_to_cover(&old_cover, &old->bounds[i]);
      ++old->count;

      if (is_leaf)
         new->leaf_aabbs[i].data = NULL;
      else
         new->branch_nodes[i] = NULL;
   }

   size_t old_cover_size = mushbounds_clamped_size(&old_cover),
          new_cover_size = mushbounds_clamped_size(&new_cover);

   for (R_IDX first_unass = 0, remaining;
        (remaining = R_BRANCHING_FACTOR - new->count - old->count + 1);)
   {
      // Move the given index to an unassigned box: one which is nonnull and
      // not T-above max_waste_i.
      #define NEXT_UNASS(i) \
         while (i < R_BRANCHING_FACTOR \
             && ((is_leaf ? !old->leaf_aabbs[i].data : !old->branch_nodes[i]) \
              || IS_TABOVE(i, max_waste_i))) \
            ++i;

      NEXT_UNASS(first_unass);

      assert ((first_unass < R_BRANCHING_FACTOR || !assigned_elem)
           && "need to assign more, but nothing left");

      if (old->count + remaining == R_MIN_XS_PER_NODE) {
         // Assign the rest to old.
         if (!assigned_elem) {
            assigned_elem = true;
            elem_in_old   = true;
            mushbounds_expand_to_cover(&old_cover, elem_bounds);
            ++old->count;
         }
         for (R_IDX i = first_unass; i < R_BRANCHING_FACTOR;) {
            mushbounds_expand_to_cover(&old_cover, &old->bounds[i]);
            ++old->count;
            if (is_leaf)
               new->leaf_aabbs[i].data = NULL;
            else
               new->branch_nodes[i] = NULL;
            ++i;
            NEXT_UNASS(i);
         }
         break;
      }

      if (new->count + remaining == R_MIN_XS_PER_NODE) {
         // Assign the rest to new.
         if (!assigned_elem) {
            assigned_elem = true;
            elem_in_old   = false;
            mushbounds_expand_to_cover(&new_cover, elem_bounds);
            ++new->count;
         }
         for (R_IDX i = first_unass; i < R_BRANCHING_FACTOR;) {
            new->bounds[i] = old->bounds[i];
            if (is_leaf) {
               new->leaf_aabbs[i] = old->leaf_aabbs[i];
               old->leaf_aabbs[i].data = NULL;
            } else {
               new->branch_nodes[i] = old->branch_nodes[i];
               old->branch_nodes[i] = NULL;
            }
            mushbounds_expand_to_cover(&new_cover, &new->bounds[i]);
            ++new->count;
            ++i;
            NEXT_UNASS(i);
         }
         break;
      }

      // Select the next box to place: take the one with the highest preference
      // for either node, i.e. with the highest difference between how much it
      // increases the size of each node's covering bounds. Place it in the
      // node whose covering bounds will be enlarged the least.
      //
      // That is, if T-ordering is not taken into account. Since T-ordering
      // forces some placements, if placing a box in one node would force the
      // other one's count below R_MIN_XS_PER_NODE, we have no choice but to
      // put that box in the other node.

      size_t            picked_ddiff = 0;
      R_IDX             picked_idx;
      r_elem            picked_elem;
      const mushbounds *picked_bounds;
      bool              picked_old;
      mushbounds        picked_cover;
      size_t            picked_cover_size;

      #define TRY_PICK_NEXT(BOUNDS, IDX, ELEM) do { \
         mushbounds old_cover2 = old_cover, \
                    new_cover2 = new_cover; \
         mushbounds_expand_to_cover(&old_cover2, BOUNDS); \
         mushbounds_expand_to_cover(&new_cover2, BOUNDS); \
         const size_t old_cover2_size = mushbounds_clamped_size(&old_cover2), \
                      new_cover2_size = mushbounds_clamped_size(&new_cover2), \
                      old_diff        = old_cover2_size - old_cover_size, \
                      new_diff        = new_cover2_size - new_cover_size, \
                      ddiff = ABS((ptrdiff_t)new_diff - (ptrdiff_t)old_diff); \
         \
         if (ddiff > picked_ddiff || !picked_ddiff) { \
            picked_ddiff      = ddiff; \
            picked_idx        = IDX; \
            picked_elem       = ELEM; \
            picked_bounds     = BOUNDS; \
            picked_old        = old_diff <= new_diff; \
            picked_cover      = picked_old ? old_cover2 : new_cover2; \
            picked_cover_size = \
               picked_old ? old_cover2_size : new_cover2_size; \
         } \
      } while (0)

      for (R_IDX i = first_unass; i < R_BRANCHING_FACTOR;) {
         const r_elem elem_i =
            is_leaf ? (r_elem){ .leaf_aabb_ptr = &old->leaf_aabbs  [i] }
                    : (r_elem){ .branch_ptr    =  old->branch_nodes[i] };

         R_IDX tabove_forced = 0,
               tbelow_forced = 0;
         for (R_IDX j = first_unass; j < R_BRANCHING_FACTOR;) {
            if (IS_TABOVE(j, i))
               ++tabove_forced;
            if (IS_TABOVE(i, j))
               ++tbelow_forced;
            ++j;
            NEXT_UNASS(j);
         }
         if (!assigned_elem && IS_TABOVE(i, R_BRANCHING_FACTOR))
            ++tbelow_forced;

         // If the number of unassigned boxes transitively T-above this one
         // would force new->count too low, this must be placed in new.
         if (new->count + remaining - tabove_forced - 1 < R_MIN_XS_PER_NODE) {
            picked_idx        = i;
            picked_elem       = elem_i;
            picked_bounds     = &old->bounds[i];
            picked_cover      = new_cover;
            picked_cover_size = 0;
            mushbounds_expand_to_cover(&picked_cover, picked_bounds);
            goto forced_new;
         }

         // And similarly, if the number of unassigned boxes transitively
         // T-below this one would force old->count too low, this must be
         // placed in old.
         if (old->count + remaining - tbelow_forced - 1 < R_MIN_XS_PER_NODE) {
            // Be sneaky and only write those variables which are used in the
            // picked_old case.
            picked_idx        = i;
            picked_cover      = old_cover;
            picked_cover_size = 0;
            mushbounds_expand_to_cover(&picked_cover, &old->bounds[i]);
            goto forced_old;
         }

         TRY_PICK_NEXT(&old->bounds[i], i, elem_i);
         ++i;
         NEXT_UNASS(i);
      }
      if (!assigned_elem) {
         // elem can only force things that are T-above it.
         R_IDX tabove_forced = 0;
         for (R_IDX j = first_unass; j < R_BRANCHING_FACTOR;) {
            if (IS_TABOVE(j, R_BRANCHING_FACTOR))
               ++tabove_forced;
            ++j;
            NEXT_UNASS(j);
         }

         if (new->count + remaining - tabove_forced - 1 < R_MIN_XS_PER_NODE) {
            picked_idx        = R_BRANCHING_FACTOR;
            picked_elem       = elem;
            picked_bounds     = elem_bounds;
            picked_cover      = new_cover;
            picked_cover_size = 0;
            mushbounds_expand_to_cover(&picked_cover, picked_bounds);
            goto forced_elem_new;
         }

         TRY_PICK_NEXT(elem_bounds, R_BRANCHING_FACTOR, elem);
      }

      if (picked_old) {
         if (picked_idx == R_BRANCHING_FACTOR) {
            assigned_elem = true;
            elem_in_old   = true;
         } else {
forced_old:
            SET_TABOVE(picked_idx, max_waste_i);

            if (is_leaf)
               new->leaf_aabbs[picked_idx].data = NULL;
            else
               new->branch_nodes[picked_idx] = NULL;
         }
         ++old->count;
         old_cover = picked_cover;

         // Putting this one in old also means that all boxes transitively
         // T-above it must stay in old.
         for (R_IDX i = first_unass; i < picked_idx;) {
            if (IS_TABOVE(i, picked_idx)) {
               SET_TABOVE(i, max_waste_i);

               if (is_leaf)
                  new->leaf_aabbs[i].data = NULL;
               else
                  new->branch_nodes[i] = NULL;

               mushbounds_expand_to_cover(&old_cover, &old->bounds[i]);
               ++old->count;

               // Unset this so that we know we have to recompute it below.
               picked_cover_size = 0;
            }
            ++i;
            NEXT_UNASS(i);
         }
         old_cover_size =
            picked_cover_size ? picked_cover_size
                              : mushbounds_clamped_size(&old_cover);
      } else {
         if (picked_idx == R_BRANCHING_FACTOR) {
forced_elem_new:
            assigned_elem = true;
            elem_in_old   = false;
         } else {
forced_new:
            new->bounds[picked_idx] = *picked_bounds;
            if (is_leaf) {
               new->leaf_aabbs[picked_idx] = *picked_elem.leaf_aabb_ptr;
               old->leaf_aabbs[picked_idx].data = NULL;
            } else {
               new->branch_nodes[picked_idx] = picked_elem.branch_ptr;
               old->branch_nodes[picked_idx] = NULL;
            }
         }
         ++new->count;
         new_cover = picked_cover;

         // Putting this one in new also means that all boxes transitively
         // T-below it must be put in new.
         R_IDX i = picked_idx + 1;
         NEXT_UNASS(i);
         while (i < R_BRANCHING_FACTOR) {
            if (IS_TABOVE(picked_idx, i)) {
               new->bounds[i] = old->bounds[i];
               if (is_leaf) {
                  new->leaf_aabbs[i] = old->leaf_aabbs[i];
                  old->leaf_aabbs[i].data = NULL;
               } else {
                  new->branch_nodes[i] = old->branch_nodes[i];
                  old->branch_nodes[i] = NULL;
               }
               mushbounds_expand_to_cover(&new_cover, &old->bounds[i]);
               ++new->count;
               picked_cover_size = 0;
            }
            ++i;
            NEXT_UNASS(i);
         }
         if (!assigned_elem && IS_TABOVE(picked_idx, R_BRANCHING_FACTOR)) {
            assigned_elem = true;
            elem_in_old   = false;
            mushbounds_expand_to_cover(&new_cover, elem_bounds);
            ++new->count;
            picked_cover_size = 0;
         }
         new_cover_size =
            picked_cover_size ? picked_cover_size
                              : mushbounds_clamped_size(&new_cover);
      }
      #undef NEXT_UNASS
      #undef TRY_PICK_NEXT
   }

   assert (old->count - 1 + new->count == R_BRANCHING_FACTOR);
   assert (assigned_elem);
   assert (old->count >= R_MIN_XS_PER_NODE);
   assert (new->count >= R_MIN_XS_PER_NODE);

   *pold_cover = old_cover;
   *pnew_cover = new_cover;

   // Both nodes still have nullified entries interspersed among their real
   // entries, so move everything in them to the beginning.
   r_flatten_node(is_leaf, old, old->count -  elem_in_old);
   r_flatten_node(is_leaf, new, new->count - !elem_in_old);

   // Place elem wherever it belongs. Don't increment the node's count: that's
   // already been done.

   mushboxen_iter iter;

   iter.node = elem_in_old ? old : new;
   iter.idx  = iter.node->count - 1;
   iter.node->bounds[iter.idx] = *elem_bounds;
   if (is_leaf)
      iter.node->leaf_aabbs[iter.idx] = *elem.leaf_aabb_ptr;
   else
      iter.node->branch_nodes[iter.idx] = elem.branch_ptr;

#ifdef MUSH_ENABLE_EXPENSIVE_DEBUGGING
   for (R_IDX i = 0; i < old->count; ++i) {
      assert (mushbounds_contains_bounds(pold_cover, &old->bounds[i]));
      if (is_leaf) assert (old->leaf_aabbs  [i].data != NULL);
      else         assert (old->branch_nodes[i]      != NULL);
   }
   for (R_IDX i = 0; i < new->count; ++i) {
      assert (mushbounds_contains_bounds(pnew_cover, &new->bounds[i]));
      if (is_leaf) assert (new->leaf_aabbs  [i].data != NULL);
      else         assert (new->branch_nodes[i]      != NULL);
   }
#endif

   if (!is_leaf) {
      old->count *= -1;
      new->count *= -1;
   }
   return iter;
   #undef IS_TABOVE
   #undef SET_TABOVE
#else
#error Only the quadratic split algorithm is implemented!
#endif
}

// FIXME: handle malloc failure, gracefully
static mushboxen_iter r_insert(
   r_insertee insertee, rtree *node, rtree **path_nodes, R_IDX *path_idxs,
   mushbounds *node_cover, rtree** split, mushbounds *split_cover)
{
   r_elem ins_here;
   const mushbounds *ins_here_bounds;
   mushbounds ins_here_bounds_st;

   R_IDX child_idx;

   mushboxen_iter iter;

   R_IDX abs_count = ABS(node->count);

   if (insertee.insert_depth == 0) {
      ins_here        = insertee.elem;
      ins_here_bounds = insertee.bounds;
   } else {
      // Select the child to recurse into: the one whose bounds are already
      // closest to insertee.bounds, resolving ties by size.
      //
      // But if any child overlaps with insertee.bounds, to preserve
      // T-ordering, the "rightmost" overlapping child is selected.

      size_t best_size = SIZE_MAX,
             best_diff = SIZE_MAX;

      for (R_IDX i = abs_count; i--;) {
         mushbounds bounds = node->bounds[i];
         if (mushbounds_overlaps(insertee.bounds, &bounds)) {
            child_idx = i;
            break;
         }

         const size_t bounds_size = mushbounds_size(&bounds);

         mushbounds_expand_to_cover(&bounds, insertee.bounds);
         const size_t exp_size = mushbounds_clamped_size(&bounds);

         const size_t diff = exp_size - bounds_size;
         if (diff < best_diff || (diff == best_diff && exp_size < best_size)) {
            best_diff = diff;
            best_size = exp_size;
            child_idx = i;
         }
      }

      --insertee.insert_depth;

      iter = r_insert(insertee, node->branch_nodes[child_idx],
                      path_nodes + 1, path_idxs + 1,
                      &node->bounds[child_idx],
                      &ins_here.branch_ptr, &ins_here_bounds_st);
      if (mushboxen_iter_is_null(iter))
         return iter;

      ++iter.path_depth;

      // Write the relevant auxiliary info. Since we move the pointers on every
      // recursive call, no indexing is needed.
      *path_nodes = node;
      *path_idxs  = child_idx;

      // Reset to the insert depth of this call, since we still use it below.
      ++insertee.insert_depth;

      if (!ins_here.branch_ptr) {
         // No split happened below: we're done.
         goto no_split_done;
      }
      ins_here_bounds = &ins_here_bounds_st;
   }

   if (abs_count == R_BRANCHING_FACTOR) {
{'fraction_non_alphanumeric': 0.11518899455863595, 'fraction_numerical': 0.004198085027368285, 'mean_word_length': 1.940975354955126, 'pattern_counts': {'":': 0, '<': 123, '<?xml version=': 0, '>': 401, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 8, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5464388', 'n_tokens_mistral': 21841, 'n_tokens_neox': 21166, 'n_words': 5129}