1 files changed, 241 insertions, 0 deletions
diff --git a/src/Ryujinx.Horizon/Sdk/Ngc/Detail/BpNode.cs b/src/Ryujinx.Horizon/Sdk/Ngc/Detail/BpNode.cs
new file mode 100644
index 00000000..6884cddd
--- /dev/null
+++ b/src/Ryujinx.Horizon/Sdk/Ngc/Detail/BpNode.cs
@@ -0,0 +1,241 @@
+namespace Ryujinx.Horizon.Sdk.Ngc.Detail
+{
+    class BpNode
+    {
+        private readonly Set _set = new();
+        private SparseSet _sparseSet;
+        private BpNode _nextNode;
+
+        public Set Set => _set;
+
+        public bool Import(ref BinaryReader reader)
+        {
+            if (!_set.Import(ref reader))
+            {
+                return false;
+            }
+
+            if (!reader.Read(out byte hasNext))
+            {
+                return false;
+            }
+
+            if (hasNext == 0)
+            {
+                return true;
+            }
+
+            _sparseSet = new();
+            _nextNode = new();
+
+            return _sparseSet.Import(ref reader) && _nextNode.Import(ref reader);
+        }
+
+        public int FindOpen(int index)
+        {
+            uint membershipBits = _set.BitVector.Array[index / Set.BitsPerWord];
+
+            int wordBitOffset = index % Set.BitsPerWord;
+            int unsetBits = 1;
+
+            for (int bit = wordBitOffset - 1; bit >= 0; bit--)
+            {
+                if (((membershipBits >> bit) & 1) != 0)
+                {
+                    if (--unsetBits == 0)
+                    {
+                        return (index & ~(Set.BitsPerWord - 1)) | bit;
+                    }
+                }
+                else
+                {
+                    unsetBits++;
+                }
+            }
+
+            int plainIndex = _sparseSet.Rank1(index);
+            if (plainIndex == 0)
+            {
+                return -1;
+            }
+
+            int newIndex = index;
+
+            if (!_sparseSet.Has(index))
+            {
+                if (plainIndex == 0 || _nextNode == null)
+                {
+                    return -1;
+                }
+
+                newIndex = _sparseSet.Select1(plainIndex);
+                if (newIndex < 0)
+                {
+                    return -1;
+                }
+            }
+            else
+            {
+                plainIndex--;
+            }
+
+            int openIndex = _nextNode.FindOpen(plainIndex);
+            if (openIndex < 0)
+            {
+                return -1;
+            }
+
+            int openSparseIndex = _sparseSet.Select1(openIndex);
+            if (openSparseIndex < 0)
+            {
+                return -1;
+            }
+
+            if (newIndex != index)
+            {
+                unsetBits = 1;
+
+                for (int bit = newIndex % Set.BitsPerWord - 1; bit > wordBitOffset; bit--)
+                {
+                    unsetBits += ((membershipBits >> bit) & 1) != 0 ? -1 : 1;
+                }
+
+                int bestCandidate = -1;
+
+                membershipBits = _set.BitVector.Array[openSparseIndex / Set.BitsPerWord];
+
+                for (int bit = openSparseIndex % Set.BitsPerWord + 1; bit < Set.BitsPerWord; bit++)
+                {
+                    if (unsetBits - 1 == 0)
+                    {
+                        bestCandidate = bit;
+                    }
+
+                    unsetBits += ((membershipBits >> bit) & 1) != 0 ? -1 : 1;
+                }
+
+                return (openSparseIndex & ~(Set.BitsPerWord - 1)) | bestCandidate;
+            }
+            else
+            {
+                return openSparseIndex;
+            }
+        }
+
+        public int Enclose(int index)
+        {
+            uint membershipBits = _set.BitVector.Array[index / Set.BitsPerWord];
+
+            int unsetBits = 1;
+
+            for (int bit = index % Set.BitsPerWord - 1; bit >= 0; bit--)
+            {
+                if (((membershipBits >> bit) & 1) != 0)
+                {
+                    if (--unsetBits == 0)
+                    {
+                        return (index & ~(Set.BitsPerWord - 1)) + bit;
+                    }
+                }
+                else
+                {
+                    unsetBits++;
+                }
+            }
+
+            int setBits = 2;
+
+            for (int bit = index % Set.BitsPerWord + 1; bit < Set.BitsPerWord; bit++)
+            {
+                if (((membershipBits >> bit) & 1) != 0)
+                {
+                    setBits++;
+                }
+                else
+                {
+                    if (--setBits == 0)
+                    {
+                        return FindOpen((index & ~(Set.BitsPerWord - 1)) + bit);
+                    }
+                }
+            }
+
+            int newIndex = index;
+
+            if (!_sparseSet.Has(index))
+            {
+                newIndex = _sparseSet.Select1(_sparseSet.Rank1(index));
+                if (newIndex < 0)
+                {
+                    return -1;
+                }
+            }
+
+            if (!_set.Has(newIndex))
+            {
+                newIndex = FindOpen(newIndex);
+                if (newIndex < 0)
+                {
+                    return -1;
+                }
+            }
+            else
+            {
+                newIndex = _nextNode.Enclose(_sparseSet.Rank1(newIndex) - 1);
+                if (newIndex < 0)
+                {
+                    return -1;
+                }
+
+                newIndex = _sparseSet.Select1(newIndex);
+            }
+
+            int nearestIndex = _sparseSet.Select1(_sparseSet.Rank1(newIndex));
+            if (nearestIndex < 0)
+            {
+                return -1;
+            }
+
+            setBits = 0;
+
+            membershipBits = _set.BitVector.Array[newIndex / Set.BitsPerWord];
+
+            if ((newIndex / Set.BitsPerWord) == (nearestIndex / Set.BitsPerWord))
+            {
+                for (int bit = nearestIndex % Set.BitsPerWord - 1; bit >= newIndex % Set.BitsPerWord; bit--)
+                {
+                    if (((membershipBits >> bit) & 1) != 0)
+                    {
+                        if (++setBits > 0)
+                        {
+                            return (newIndex & ~(Set.BitsPerWord - 1)) + bit;
+                        }
+                    }
+                    else
+                    {
+                        setBits--;
+                    }
+                }
+            }
+            else
+            {
+                for (int bit = Set.BitsPerWord - 1; bit >= newIndex % Set.BitsPerWord; bit--)
+                {
+                    if (((membershipBits >> bit) & 1) != 0)
+                    {
+                        if (++setBits > 0)
+                        {
+                            return (newIndex & ~(Set.BitsPerWord - 1)) + bit;
+                        }
+                    }
+                    else
+                    {
+                        setBits--;
+                    }
+                }
+            }
+
+            return -1;
+        }
+    }
+}