Greatly accelerated bitset consecutive allocation

src/main/java/me/cortex/voxy/common/util/HierarchicalBitSet.java

@@ -1,5 +1,7 @@
 package me.cortex.voxy.common.util;
 
+import java.util.Random;
+
 public class HierarchicalBitSet {
     public static final int SET_FULL = -1;
     private final int limit;
@@ -74,45 +76,60 @@ public class HierarchicalBitSet {
 
     //Returns the next free index from idx
     private int findNextFree(int idx) {
-        int pos = Long.numberOfTrailingZeros((~this.A)|((1L<<(idx>>18))-1));
-        return 0;
+        int pos;
+        do {
+            pos = Long.numberOfTrailingZeros((~this.A) & -(1L << (idx >> 18)));
+            idx = Math.max(pos << 18, idx);
+
+            pos = Long.numberOfTrailingZeros((~this.B[idx >> 18]) & -(1L << ((idx >> 12) & 0x3F)));
+            idx = Math.max((pos + ((idx >> 18) << 6)) << 12, idx);
+            if (pos == 64) continue;//Try again
+
+            pos = Long.numberOfTrailingZeros((~this.C[idx >> 12]) & -(1L << ((idx >> 6) & 0x3F)));
+            idx = Math.max((pos + ((idx >> 12) << 6)) << 6, idx);
+            if (pos == 64) continue;//Try again
+
+            pos = Long.numberOfTrailingZeros(((~this.D[idx >> 6]) & -(1L << (idx & 0x3F))));
+            idx = Math.max(pos + ((idx >> 6) << 6), idx);
+        } while (pos == 64);
+        //TODO: fixme: this is due to the fact of the acceleration structure
+        return idx;
     }
 
 
     //TODO: FIXME: THIS IS SLOW AS SHIT
     public int allocateNextConsecutiveCounted(int count) {
+        if (count > 64) {
+            throw new IllegalStateException("Count to large for current implementation which has fastpath");
+        }
         if (this.A==-1) {
             return -1;
         }
         if (this.cnt+count>this.limit) {
             return -2;//Limit reached
         }
-        //TODO:FIXME DONT DO THIS, do a faster search
-
-        int i = 0;
+        long chkMsk = ((1L<<count)-1);
+        int i = this.findNextFree(0);
         while (true) {
-            boolean isFree = true;
-            for (int j = 0; j < count; j++) {
-                if (this.isSet(i+j)) {
-                    isFree = false;
-                    break;
-                }
+            long fusedValue = this.D[i>>6]>>>(i&63);
+            if (64-(i&63) < count) {
+                fusedValue |= this.D[(i>>6)+1] << (64-(i&63));
             }
 
-            if (isFree) {
-                for (int j = 0; j < count; j++) {
-                    this.set(j + i);
-                }
-                return i;
-            } else {
-                i++;//THIS IS SLOW BUT WORKS
-                /* TODO: FIX AND FINISH OPTIMIZATION
-                i +=
-                while (this.D[i>>6] == -1) {
-                    i++;
-                }
-                 */
+            if ((fusedValue&chkMsk) != 0) {
+                //Space does not contain enough empty value
+                i += Long.numberOfTrailingZeros(fusedValue);//Skip as much as possible (i.e. skip to the next 1 bit)
+                i = this.findNextFree(i);
+
+                continue;
             }
+
+            //TODO: optimize this laziness
+            // (can  do it by first setting/updating the lower D index and propagating, then the upper D index (if it has/needs one))
+            for (int j = 0; j < count; j++) {
+                this.set(j + i);
+            }
+            return i;
         }
     }
 
@@ -141,4 +158,34 @@ public class HierarchicalBitSet {
     public boolean isSet(int idx) {
         return (this.D[idx>>6]&(1L<<(idx&0x3f)))!=0;
     }
+
+
+    public static void main(String[] args) {
+        var h = new HierarchicalBitSet();
+        for (int i = 0; i < 64*32; i++) {
+            h.set(i);
+        }
+        h.set(0);
+        {
+            int i = 0;
+            while (i<64*32) {
+                int j = h.findNextFree(i);
+                if (h.isSet(j)) {
+                    throw new IllegalStateException();
+                }
+                for (int k = i; k < j; k++) {
+                    if (!h.isSet(k)) {
+                        throw new IllegalStateException();
+                    }
+                }
+                i = j + 1;
+            }
+        }
+        var r = new Random(0);
+        for (int i = 0; i < 500; i++) {
+            h.free(r.nextInt(64*32));
+        }
+
+        h.allocateNextConsecutiveCounted(10);
+    }
 }