Added esep inverse coord

3 files changed, 59 insertions, 5 deletions

diff --git a/TODO.txt b/TODO.txt
index affddec..30d0247 100644
--- a/TODO.txt
+++ b/TODO.txt
@@ -1,7 +1,7 @@
 In progress: go back to nissy-style BFS for eosep data computation
         - (done) compute selfsim and cocsep representatives
         - (done) implement set_eo_fast for invcoord_h48
-        - for invcoord_h48 remove erep, implement inverse esep coord
+        - (done) for invcoord_h48 remove erep, implement inverse esep coord
         - add unit tests for invcoord_h48 (compute tables, if needed)
         - change to BFS
 
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;
 }
+
diff --git a/src/cube_portable.h b/src/cube_portable.h
index 24c39e8..cb4d8fc 100644
--- a/src/cube_portable.h
+++ b/src/cube_portable.h
@@ -256,6 +256,31 @@ 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 ret;
+        int64_t i, j, k, l, s, v, w, is1, set1, set2;
+        uint8_t bit2, bit1;
+        uint8_t slice[3] = {0};
+
+        ret = cubetofast(solved);
+        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;
+
+                ret.edge[i] = (slice[s]++) | (s << 2);
+        }
+
+        return ret;
 }