Add support for seqlock-based entity managers

Change-Id: I8298b3a43d4c48a4880aa75b53a1a9ac133968b0
diff --git a/base/EntityManager.h b/base/EntityManager.h
index d176114..e969a8d 100644
--- a/base/EntityManager.h
+++ b/base/EntityManager.h
@@ -97,6 +97,13 @@
 
     EntityManager() : EntityManager(0) { }
 
+    EntityManager(size_t initialItems) :
+        mEntries(initialItems),
+        mFirstFreeIndex(0),
+        mLiveEntries(0) {
+        reserve(initialItems);
+    }
+
     ~EntityManager() { clear(); }
 
     struct EntityEntry {
@@ -108,12 +115,16 @@
         Item item;
     };
 
-    void clear() {
-        mEntries.clear();
+	void clear() {
+		reserve(mEntries.size());
         mFirstFreeIndex = 0;
         mLiveEntries = 0;
     }
 
+    size_t nextFreeIndex() const {
+        return mFirstFreeIndex;
+    }
+
     EntityHandle add(const Item& item, size_t type) {
 
         if (!type) return INVALID_ENTITY_HANDLE;
@@ -289,13 +300,28 @@
     }
 
     Item* get(EntityHandle h) {
+        EM_DBG("get 0x%llx", (unsigned long long)h);
+        size_t index = getHandleIndex(h);
+        if (index >= mEntries.size()) {
+            return nullptr;
+        }
+
+        auto& entry = mEntries[index];
+        if (entry.liveGeneration != getHandleGeneration(h)) {
+            return nullptr;
+        }
+
+        return &entry.item;
+    }
+
+    const Item* get_const(EntityHandle h) const {
         size_t index = getHandleIndex(h);
         if (index >= mEntries.size()) return nullptr;
 
-        auto& entry = mEntries[index];
-        if (entry.liveGeneration != getHandleGeneration(h)) return nullptr;
+        const auto& entry = mEntries.data() + index;
+        if (entry->liveGeneration != getHandleGeneration(h)) return nullptr;
 
-        return &entry.item;
+        return &entry->item;
     }
 
     bool isLive(EntityHandle h) const {
@@ -341,10 +367,22 @@
     }
 
 private:
-    EntityManager(size_t initialItems) :
-        mEntries(initialItems),
-        mFirstFreeIndex(0),
-        mLiveEntries(0) { }
+    void reserve(size_t count) {
+        if (mEntries.size() < count) {
+            mEntries.resize(count);
+        }
+        for (size_t i = 0; i < count; ++i) {
+            mEntries[i].handle = makeHandle(i, 0, 1);
+            mEntries[i].nextFreeIndex = i + 1;
+            ++mEntries[i].liveGeneration;
+            if ((mEntries[i].liveGeneration == 0) ||
+                    (mEntries[i].liveGeneration == (1ULL << generationBits))) {
+                mEntries[i].liveGeneration = 1;
+            }
+            EM_DBG("new un-init entry: index %zu nextFree %zu",
+                    i, i + 1);
+        }
+    }
 
     std::vector<EntityEntry> mEntries;
     size_t mFirstFreeIndex;