DragonECS-Graphs/src/Internal/SparseMatrix.cs

using System;
using System.Data.SqlTypes;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using TValue = System.Int32;

namespace DCFApixels.DragonECS.Graphs.Internal
{
    public sealed unsafe class SparseMatrix
    {
        public const int MIN_CAPACITY_BITS_OFFSET = 4;
        public const int MIN_CAPACITY = 1 << MIN_CAPACITY_BITS_OFFSET;

        private const int CHAIN_LENGTH_THRESHOLD = 5;
        private const float CHAIN_LENGTH_THRESHOLD_CAPCITY_THRESHOLD = 0.65f;

        private int* _buckets;
        private Entry* _entries;
        private int _capacity;
        private int _count_Threshold;

        private int _count;

        private int _freeList;
        private int _freeCount;

        private int _modBitMask;

        #region Properties
        public int Count
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get { return _count; }
        }
        public int Capacity
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get { return _capacity; }
        }
        #endregion

        #region Constructors
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SparseMatrix(int minCapacity = MIN_CAPACITY)
        {
            minCapacity = NormalizeCapacity(minCapacity);
            _buckets = UnmanagedArrayUtility.New<int>(minCapacity);
            _entries = UnmanagedArrayUtility.NewAndInit<Entry>(minCapacity);
            for (int i = 0; i < minCapacity; i++)
            {
                _buckets[i] = -1;
            }
            _modBitMask = (minCapacity - 1) & 0x7FFFFFFF;

            _count = 0;
            _freeList = 0;
            _freeCount = 0;

            SetCapacity(minCapacity);
        }
        ~SparseMatrix()
        {
            UnmanagedArrayUtility.Free(_buckets);
            UnmanagedArrayUtility.Free(_entries);
        }
        #endregion

        #region Add/TryAdd/Set
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Add(int x, int y, TValue value)
        {
            unchecked
            {
                long key = KeyUtility.FromXY(x, y);
#if DEBUG
                if (FindEntry(key) >= 0)
                {
                    Throw.ArgumentException("Has(x, y) is true");
                }
#endif
                int hash = IntHashes.hashes[y] ^ x;
                AddInternal(key, hash, value);
            }
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool TryAdd(int x, int y, TValue value)
        {
            unchecked
            {
                long key = KeyUtility.FromXY(x, y);
                int hash = IntHashes.hashes[y] ^ x;
                if (FindEntry(x, y) >= 0)
                {
                    return false;
                }
                AddInternal(key, hash, value);
                return true;
            }
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Set(int x, int y, TValue value)
        {
            unchecked
            {
                long key = KeyUtility.FromXY(x, y);
                int hash = IntHashes.hashes[y] ^ x;
                int targetBucket = hash & _modBitMask;

                for (int i = _buckets[targetBucket]; i >= 0; i = _entries[i].next)
                {
                    if (_entries[i].hash == hash && _entries[i].key == key)
                    {
                        _entries[i].value = value;
                        return;
                    }
                }
                AddInternal(key, hash, value);
            }
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private void AddInternal(long key, int hash, int value)
        {
            unchecked
            {
                int targetBucket = hash & _modBitMask;
                int index;
                if (_freeCount == 0)
                {
                    if (_count > _count_Threshold)
                    { 
                        Resize();
                        // обновляем под новое значение _modBitMask
                        targetBucket = hash & _modBitMask;
                    }
                    //index = Interlocked.Increment(ref _count);
                    index = _count++;
                }
                else
                {
                    index = _freeList;
                    _freeList = _entries[index].next;
                    _freeCount--;
                }

#if DEBUG
                if (_freeCount < 0) { Throw.UndefinedException(); }
#endif

                ref int basket = ref _buckets[targetBucket];
                ref Entry entry = ref _entries[index];

                entry.hash = hash;
                entry.next = basket;
                entry.key = key;
                entry.value = value;
                basket = index;
            }
        }
        #endregion

        #region FindEntry/Has
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private int FindEntry(int x, int y)
        {
            long key = KeyUtility.FromXY(x, y);
            int hash = IntHashes.hashes[y] ^ x;
            for (int i = _buckets[hash & _modBitMask]; i >= 0; i = _entries[i].next)
            {
                if (_entries[i].hash == hash && _entries[i].key == key)
                {
                    return i;
                }
            }
            return -1;
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool HasKey(int x, int y)
        {
            return FindEntry(x, y) >= 0;
        }
        #endregion

        #region GetValue/TryGetValue
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public TValue GetValue(int x, int y)
        {
            int index = FindEntry(x, y);
#if DEBUG
            if (index < 0) { Throw.KeyNotFound(); }
#endif
            return _entries[index].value;
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool TryGetValue(int x, int y, out TValue value)
        {
            int index = FindEntry(x, y);
            if (index < 0)
            {
                value = default;
                return false;
            }
            value = _entries[index].value;
            return true;
        }
        #endregion

        #region TryDel
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public bool TryDel(int x, int y)
        {
            long key = KeyUtility.FromXY(x, y);
            int hash = IntHashes.hashes[y] ^ x;
            int targetBucket = hash & _modBitMask;
            ref int basket = ref _buckets[targetBucket];

            int last = -1;
            for (int i = basket; i >= 0; last = i, i = _entries[i].next)
            {
                if (_entries[i].hash == hash && _entries[i].key == key)
                {
                    if (last < 0)
                    {
                        basket = _entries[i].next;
                    }
                    else
                    {
                        _entries[last].next = _entries[i].next;
                    }
                    _entries[i].next = _freeList;
                    _entries[i].key = default; //Key.Null;
                    _entries[i].value = default;
                    _freeList = i;
                    _freeCount++;
                    return true;
                }
            }
            return false;
        }
        #endregion

        #region Clear
        public void Clear()
        {
            if (_count > 0)
            {
                for (int i = 0; i < _capacity; i++)
                {
                    _buckets[i] = -1;
                }
                for (int i = 0; i < _capacity; i++)
                {
                    _entries[i] = default;
                }
                _count = 0;
            }
        }
        #endregion

        #region Resize
        [MethodImpl(MethodImplOptions.NoInlining)]
        private void Resize()
        {
            int newSize = _capacity << 1;
            Console.WriteLine($"Resize {newSize}");
            _modBitMask = (newSize - 1) & 0x7FFFFFFF;

            //newBuckets create and ini
            int* newBuckets = UnmanagedArrayUtility.New<int>(newSize);
            for (int i = 0; i < newSize; i++)
            {
                newBuckets[i] = -1;
            }
            //END newBuckets create and ini

            Entry* newEntries = UnmanagedArrayUtility.ResizeAndInit<Entry>(_entries, _capacity, newSize);
            for (int i = 0; i < _count; i++)
            {
                if (newEntries[i].key >= 0)
                {
                    ref Entry entry = ref newEntries[i];
                    ref int basket = ref newBuckets[entry.hash & _modBitMask];
                    entry.next = basket;
                    basket = i;
                }
            }

            UnmanagedArrayUtility.Free(_buckets);
            _buckets = newBuckets;
            _entries = newEntries;
            
            SetCapacity(newSize);
        }

        private void SetCapacity(int newSize)
        {
            _capacity = newSize;
            _count_Threshold = (int)(_capacity * CHAIN_LENGTH_THRESHOLD_CAPCITY_THRESHOLD);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static int NormalizeCapacity(int capacity)
        {
            int result = MIN_CAPACITY;
            while (result < capacity) { result <<= 1; }
            return result;
        }
        #endregion

        #region Utils
        [StructLayout(LayoutKind.Sequential, Pack = 4)]
        private struct Entry
        {
            public int next;        // Index of next entry, -1 if last
            public long key;
            public int hash;
            public TValue value;
            public override string ToString() { return key == 0 ? "NULL" : $"{key} {value}"; }
        }

        public static class KeyUtility
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            public static long FromXY(int x, int y)
            {
                unchecked
                {
                    return ((long)x << 32) | (long)y;
                }
            }
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            private static int Mixing(int v)
            {
                unchecked
                {
                    int i = v;
                    v *= 3571;
                    
                    v ^= v << 13;
                    v ^= v >> 17;
                    v ^= v << 5;
                    //v = (v >> 28) + v ^ i;
                    return v;
                }
            }
        }
        #endregion
    }


    public static unsafe class IntHashes
    {
        public static int* hashes = null;
        public static int length = 0;
        public static void InitFor(int count)
        {
            unchecked
            {
                const decimal G1 = 1.6180339887498948482045868383m;
                const uint Q32_MAX = uint.MaxValue;
                const uint X1_Q32 = (uint)(1m / G1 * Q32_MAX) + 1;

                //count = GetHighBitNumber((uint)count) << 1;

                if (count <= length) { return; }

                if (hashes != null)
                {
                    UnmanagedArrayUtility.Free(hashes);
                }

                hashes = UnmanagedArrayUtility.New<int>(count);

                uint state = 3571U;

                for (int i = 0; i < count; i++)
                {
                    //state ^= state << 13;
                    //state ^= state >> 17;
                    //state ^= state << 5;

                    state = X1_Q32 * state;

                    //int v = Mixing(i);
                    hashes[i] = (int)state;
                }

                count = length;
            }
        }
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static int Mixing(int v)
        {
            unchecked
            {
                int i = v;
                v *= 3571;
                //if(i % 3 == 0)
                //{
                //    v ^= (v << 13) | 8191;
                //}
                //else
                //{
                //    v ^= (v << 13);
                //}
                v ^= (v << 13);
                v ^= v >> 17;
                //if (i % 5 == 0)
                //{
                //    v ^= (v << 5) | 15;
                //}
                //else
                //{
                //    v ^= (v << 5);
                //}
                v ^= (v << 5);
            
                //v = (v >> 28) + v ^ i;
                return v;


            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static int GetHighBitNumber(uint bits)
        {
            if (bits == 0)
            {
                return -1;
            }
            int bit = 0;
            if ((bits & 0xFFFF0000) != 0)
            {
                bits >>= 16;
                bit |= 16;
            }
            if ((bits & 0xFF00) != 0)
            {
                bits >>= 8;
                bit |= 8;
            }
            if ((bits & 0xF0) != 0)
            {
                bits >>= 4;
                bit |= 4;
            }
            if ((bits & 0xC) != 0)
            {
                bits >>= 2;
                bit |= 2;
            }
            if ((bits & 0x2) != 0)
            {
                bit |= 1;
            }
            return bit;
        }
    }
}