From dbe8817d953a04373102b015bdb2c83d17e39a4e Mon Sep 17 00:00:00 2001
From: Patrick Pelissier <patrick.pelissier@gmail.com>
Date: Fri, 27 Jul 2018 06:19:54 +0200
Subject: [PATCH] Add bitset_clz

---
 m-bitset.h           | 38 ++++++++++++++++++++++++++++++++------
 tests/test-mbitset.c | 36 ++++++++++++++++++++++++++++++++++++
 2 files changed, 68 insertions(+), 6 deletions(-)

diff --git a/m-bitset.h b/m-bitset.h
index a39fa0f1..fb065dcf 100644
--- a/m-bitset.h
+++ b/m-bitset.h
@@ -25,12 +25,13 @@
 #ifndef MSTARLIB_BITSET_H
 #define MSTARLIB_BITSET_H
 
+#include <stdint.h>
 #include "m-core.h"
 
 /********************************** INTERNAL ************************************/
 
 // Define a limb of a bitset
-typedef unsigned long bitset_limb;
+typedef uint64_t bitset_limb;
 #define BITSET_LIMB_BIT (sizeof(bitset_limb) * CHAR_BIT)
 
 typedef struct bitset_s {
@@ -633,22 +634,47 @@ bitset_not(bitset_t dest)
 }
 
 static inline size_t
-bitset_hash(const bitset_t dest)
+bitset_hash(const bitset_t set)
 {
-  BITSETI_CONTRACT(dest);
-  size_t s = dest->size;
+  BITSETI_CONTRACT(set);
+  size_t s = set->size;
   size_t n = s / BITSET_LIMB_BIT;
   size_t m = s % BITSET_LIMB_BIT;
   M_HASH_DECL(hash);
   for(size_t i = 0 ; i < n; i++)
-    M_HASH_UP(hash, dest->ptr[i]);
+    M_HASH_UP(hash, set->ptr[i]);
   if (m) {
     size_t mask = (((bitset_limb)1) << m) - 1;
-    M_HASH_UP(hash, (dest->ptr[n] & mask));
+    M_HASH_UP(hash, (set->ptr[n] & mask));
   }
   return M_HASH_FINAL (hash);
 }
 
+static inline size_t
+bitset_clz(const bitset_t set)
+{
+  BITSETI_CONTRACT(set);
+  size_t s = set->size;
+  assert (s > 0);
+  size_t n = (s -1) / BITSET_LIMB_BIT;
+  size_t m = s % BITSET_LIMB_BIT;
+  bitset_limb limb = set->ptr[n];
+  if (m) {
+    size_t mask = (((bitset_limb)1) << m) - 1;
+    limb &= mask;
+  } else {
+    m = BITSET_LIMB_BIT;
+  }
+  s = 0;
+  while (limb == 0 && n > 0) {
+    s += m;
+    limb = set->ptr[--n];
+    m = BITSET_LIMB_BIT;
+  }
+  s += m_core_clz64(limb) - (BITSET_LIMB_BIT - m);
+  return s;
+}
+
 #define BITSET_OPLIST                                                   \
   (INIT(bitset_init)                                                    \
    ,INIT_SET(bitset_init_set)                                           \
diff --git a/tests/test-mbitset.c b/tests/test-mbitset.c
index 3d78e9a7..9586a24f 100644
--- a/tests/test-mbitset.c
+++ b/tests/test-mbitset.c
@@ -313,6 +313,41 @@ static void test_let(void)
   }
 }
 
+static void test_clz(void)
+{
+  M_LET(s1, bitset_t) {
+    bitset_push_back(s1, false);
+    assert (bitset_clz(s1) == 1);
+    bitset_push_back(s1, false);
+    assert (bitset_clz(s1) == 2);
+    bitset_set_at(s1, 0, true);
+    assert (bitset_clz(s1) == 1);
+    bitset_set_at(s1, 1, true);
+    assert (bitset_clz(s1) == 0);
+    for(int i = 0; i < 62; i++)
+      bitset_push_back(s1, false);
+    assert (bitset_clz(s1) == 62);
+    bitset_set_at(s1, 63, true);
+    assert (bitset_clz(s1) == 0);
+    bitset_push_back(s1, false);
+    assert (bitset_clz(s1) == 1);
+    bitset_set_at(s1, 64, true);
+    assert (bitset_clz(s1) == 0);
+    for(size_t i = 0; i < 620; i++) {
+      bitset_push_back(s1, false);
+      assert (bitset_clz(s1) == i+1);
+    }
+    bitset_set_at(s1, 64+620, true);
+    assert (bitset_clz(s1) == 0);
+    for(size_t i = 1; i < 64+1+620; i++) {
+      bitset_set_at(s1, i, false);
+    }
+    assert (bitset_clz(s1) == 64+620);
+    bitset_set_at(s1, 0, false);
+    assert (bitset_clz(s1) == 64+620+1);
+  }
+}
+
 int main(void)
 {
   test1();
@@ -320,5 +355,6 @@ int main(void)
   test_str();
   test_logic();
   test_let();
+  test_clz();
   exit(0);
 }