Added esep inverse coord

1 files changed, 31 insertions, 2 deletions

diff --git a/src/cube_avx2.h b/src/cube_avx2.h
index 30b0c0f..d39da94 100644
--- a/src/cube_avx2.h
+++ b/src/cube_avx2.h
@@ -273,6 +273,35 @@ set_eo_fast(cube_fast_t *cube, int64_t eo)
 _static_inline cube_fast_t
 invcoord_fast_esep(int64_t esep)
 {
-        /* TODO */
-        return cubetofast(zero);
+        cube_fast_t eee, ret;
+        int64_t i, j, k, l, s, v, w, is1, set1, set2;
+        uint8_t bit2, bit1, mem[32];
+        uint8_t slice[3] = {0};
+
+        set1 = esep % 70;
+        set2 = esep / 70;
+
+        for (i = 0, j = 0, k = 4, l = 4; i < 12; i++) {
+                v = binomial[11-i][k];
+                w = binomial[7-j][l];
+                bit2 = set2 >= v;
+                bit1 = set1 >= w;
+                is1 = (1 - bit2) * bit1;
+
+                set2 -= bit2 * v;
+                k -= bit2;
+                set1 -= is1 * w;
+                l -= is1;
+                j += (1-bit2);
+                s = 2*bit2 + (1-bit2)*bit1;
+
+                mem[i+16] = (slice[s]++) | (s << 2);
+        }
+
+        ret = cubetofast(solved);
+        eee = _mm256_loadu_si256((__m256i_u *)&mem);
+        copy_edges_fast(&ret, eee);
+
+        return ret;
 }
+