From c8aa6818c740ef4caefc9a505d3b3635e2d56752 Mon Sep 17 00:00:00 2001
From: Asriel Camora <asriel@camora.dev>
Date: Tue, 4 Jul 2023 07:29:35 +0200
Subject: [PATCH] Float CAS intrinsics & move BMI2 intrinsic

---
 Solver/Crafty/ActionSet.cs  | 38 ++-----------------
 Solver/Crafty/Intrinsics.cs | 73 +++++++++++++++++++++++++++++++++++++
 2 files changed, 76 insertions(+), 35 deletions(-)

diff --git a/Solver/Crafty/ActionSet.cs b/Solver/Crafty/ActionSet.cs
index 9a67112..acca247 100644
--- a/Solver/Crafty/ActionSet.cs
+++ b/Solver/Crafty/ActionSet.cs
@@ -2,7 +2,6 @@ using Craftimizer.Simulator.Actions;
 using System.Diagnostics.Contracts;
 using System.Numerics;
 using System.Runtime.CompilerServices;
-using System.Runtime.Intrinsics.X86;
 
 namespace Craftimizer.Solver.Crafty;
 
@@ -10,38 +9,6 @@ public struct ActionSet
 {
     private uint bits;
 
-    [MethodImpl(MethodImplOptions.AggressiveInlining)]
-    private static int NthBitSet(uint value, int n)
-    {
-        if (Bmi2.IsSupported)
-            return BitOperations.TrailingZeroCount(Bmi2.ParallelBitDeposit(1u << n, value));
-
-        var mask = 0x0000FFFFu;
-        var size = 16;
-        var _base = 0;
-
-        if (n++ >= BitOperations.PopCount(value))
-            return 32;
-
-        while (size > 0)
-        {
-            var count = BitOperations.PopCount(value & mask);
-            if (n > count)
-            {
-                _base += size;
-                size >>= 1;
-                mask |= mask << size;
-            }
-            else
-            {
-                size >>= 1;
-                mask >>= size;
-            }
-        }
-
-        return _base;
-    }
-
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
     private static int FromAction(ActionType action) => Simulator.AcceptedActionsLUT[(byte)action];
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
@@ -57,13 +24,14 @@ public struct ActionSet
     public readonly bool HasAction(ActionType action) => (bits & (1u << (FromAction(action) + 1))) != 0;
     [Pure]
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
-    public readonly ActionType ElementAt(int index) => ToAction(NthBitSet(bits, index) - 1);
+    public readonly ActionType ElementAt(int index) => ToAction(Intrinsics.NthBitSet(bits, index) - 1);
 
     [Pure]
     public readonly int Count => BitOperations.PopCount(bits);
 
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
-    public readonly ActionType SelectRandom(Random random) => ElementAt(random.Next(Count));
+    //public readonly ActionType SelectRandom(Random random) => ElementAt(random.Next(Count));
+    public readonly ActionType SelectRandom(Random random) => First();
 
     [Pure]
     [MethodImpl(MethodImplOptions.AggressiveInlining)]
diff --git a/Solver/Crafty/Intrinsics.cs b/Solver/Crafty/Intrinsics.cs
index 8772854..7894ae3 100644
--- a/Solver/Crafty/Intrinsics.cs
+++ b/Solver/Crafty/Intrinsics.cs
@@ -61,4 +61,77 @@ internal static class Intrinsics
         Avx2.IsSupported ?
         HMaxIndexAVX2(v, len) :
         HMaxIndexScalar(v, len);
+
+    [Pure]
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    private static int NthBitSetScalar(uint value, int n)
+    {
+        var mask = 0x0000FFFFu;
+        var size = 16;
+        var _base = 0;
+
+        if (n++ >= BitOperations.PopCount(value))
+            return 32;
+
+        while (size > 0)
+        {
+            var count = BitOperations.PopCount(value & mask);
+            if (n > count)
+            {
+                _base += size;
+                size >>= 1;
+                mask |= mask << size;
+            }
+            else
+            {
+                size >>= 1;
+                mask >>= size;
+            }
+        }
+
+        return _base;
+    }
+
+    [Pure]
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    private static int NthBitSetBMI2(uint value, int n) =>
+        BitOperations.TrailingZeroCount(Bmi2.ParallelBitDeposit(1u << n, value));
+
+    [Pure]
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static int NthBitSet(uint value, int n)
+    {
+        // TODO: debug
+        if (n >= BitOperations.PopCount(value))
+            throw new ArgumentException(null, nameof(value));
+
+        return Bmi2.IsSupported ?
+            NthBitSetBMI2(value, n) :
+            NthBitSetScalar(value, n);
+    }
+
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static void CASMax(ref float location, float newValue)
+    {
+        float snapshot;
+        do
+        {
+            snapshot = location;
+            if (snapshot >= newValue) return;
+        } while (Interlocked.CompareExchange(ref location, newValue, snapshot) != snapshot);
+    }
+
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
+    public static void CASAdd(ref float location, float value)
+    {
+        float snapshot;
+        float newValue;
+        do
+        {
+            snapshot = location;
+            newValue = snapshot + value;
+        }
+        while (Interlocked.CompareExchange(ref location, newValue, snapshot) != snapshot);
+    }
 }