4 files changed, 76 insertions, 18 deletions

diff --git a/src/cube.c b/src/cube.c
index 27d54b4..a6b733e 100644
--- a/src/cube.c
+++ b/src/cube.c
@@ -1,5 +1,6 @@
 #include <inttypes.h>
 #include <stdbool.h>
+#include <stdlib.h> /* TODO: Used only for malloc, remove if possible */
 #include <string.h>
 
 #ifdef DEBUG
diff --git a/src/cube_avx2.h b/src/cube_avx2.h
index 42fb9d1..8d1c940 100644
--- a/src/cube_avx2.h
+++ b/src/cube_avx2.h
@@ -23,9 +23,12 @@ _static_inline int64_t coord_fast_co(cube_fast_t);
 _static_inline int64_t coord_fast_csep(cube_fast_t);
 _static_inline int64_t coord_fast_cocsep(cube_fast_t);
 _static_inline int64_t coord_fast_eo(cube_fast_t);
-_static_inline void set_eo_fast(cube_fast_t *, int64_t);
 _static_inline int64_t coord_fast_esep(cube_fast_t);
 
+_static_inline void copy_corners_fast(cube_fast_t *, cube_fast_t);
+_static_inline void copy_edges_fast(cube_fast_t *, cube_fast_t);
+_static_inline void set_eo_fast(cube_fast_t *, int64_t);
+
 _static_inline cube_fast_t
 fastcube(
         uint8_t c_ufr,
@@ -199,12 +202,6 @@ coord_fast_eo(cube_fast_t c)
         return mask >> 17;
 }
 
-_static_inline void
-set_eo_fast(cube_fast_t *c, int64_t eo)
-{
-        /* TODO */
-}
-
 _static_inline int64_t
 coord_fast_esep(cube_fast_t c)
 {
@@ -234,3 +231,40 @@ coord_fast_esep(cube_fast_t c)
 
         return ret1 * 70 + ret2;
 }
+
+_static_inline void
+copy_corners_fast(cube_fast_t *dest, cube_fast_t src)
+{
+        *dest = _mm256_blend_epi32(*dest, src, 0x0F);
+}
+
+_static_inline void
+copy_edges_fast(cube_fast_t *dest, cube_fast_t src)
+{
+        *dest = _mm256_blend_epi32(*dest, src, 0xF0);
+}
+
+_static_inline void
+set_eo_fast(cube_fast_t *cube, int64_t eo)
+{
+        int64_t eo12, eotop, eobot;
+        __m256i veo;
+
+        eo12 = (eo << 1) + (_mm_popcnt_u64(eo) % 2);
+        eotop = (eo12 & (1 << 11)) << 17 |
+                (eo12 & (1 << 10)) << 10 |
+                (eo12 & (1 << 9)) << 3 |
+                (eo12 & (1 << 8)) >> 4;
+        eobot = (eo12 & (1 << 7)) << 53 |
+                (eo12 & (1 << 6)) << 46 |
+                (eo12 & (1 << 5)) << 39 |
+                (eo12 & (1 << 4)) << 32 |
+                (eo12 & (1 << 3)) << 25 |
+                (eo12 & (1 << 2)) << 18 |
+                (eo12 & (1 << 1)) << 11 |
+                (eo12 & 1) << 4;
+        veo = _mm256_set_epi64x(eotop, eobot, 0, 0);
+
+        *cube = _mm256_andnot_si256(_eo_avx2, *cube);
+        *cube = _mm256_or_si256(*cube, veo);
+}
diff --git a/src/cube_portable.h b/src/cube_portable.h
index fcd1d55..125c300 100644
--- a/src/cube_portable.h
+++ b/src/cube_portable.h
@@ -17,9 +17,12 @@ _static_inline int64_t coord_fast_co(cube_fast_t);
 _static_inline int64_t coord_fast_csep(cube_fast_t);
 _static_inline int64_t coord_fast_cocsep(cube_fast_t);
 _static_inline int64_t coord_fast_eo(cube_fast_t);
-_static_inline void set_eo_fast(cube_fast_t *, int64_t eo);
 _static_inline int64_t coord_fast_esep(cube_fast_t);
 
+_static_inline void copy_corners_fast(cube_fast_t *, cube_fast_t);
+_static_inline void copy_edges_fast(cube_fast_t *, cube_fast_t);
+_static_inline void set_eo_fast(cube_fast_t *, int64_t);
+
 _static_inline cube_fast_t
 fastcube(
         uint8_t c_ufr,
@@ -189,12 +192,6 @@ coord_fast_eo(cube_fast_t c)
         return ret;
 }
 
-_static_inline void
-_set_eo_fast(cube_fast_t *cube, int64_t eo)
-{
-        /* TODO */
-}
-
 /*
 We encode the edge separation as a number from 0 to C(12,4)*C(8,4).
 It can be seen as the composition of two "subset index" coordinates.
@@ -229,3 +226,28 @@ coord_fast_esep(cube_fast_t c)
 
         return ret1 * 70 + ret2;
 }
+
+_static_inline void
+copy_corners_fast(cube_fast_t *dest, cube_fast_t src)
+{
+        memcpy(&dest->corner, src.corner, sizeof(src.corner));
+}
+
+_static_inline void
+copy_edges_fast(cube_fast_t *dest, cube_fast_t src)
+{
+        memcpy(&dest->edge, src.edge, sizeof(src.edge));
+}
+
+_static_inline void
+set_eo_fast(cube_fast_t *cube, int64_t eo)
+{
+        int i, sum, flip;
+
+        for (sum = 0, i = 1; i < 12; i++, eo >>= 1) {
+                flip = eo % 2;
+                sum += flip;
+                cube->edge[i] = (cube->edge[i] & ~_eobit) | (_eobit * flip);
+        }
+        cube->edge[0] = (cube->edge[0] & ~_eobit) | (_eobit * (sum%2));
+}
diff --git a/src/solve_h48.h b/src/solve_h48.h
index e3e6de1..2fe1d69 100644
--- a/src/solve_h48.h
+++ b/src/solve_h48.h
@@ -88,16 +88,17 @@ invcoord_h48(
         cube_fast_t ret;
         int64_t coclass, ee, esep, eo;
 
+        DBG_ASSERT(h <= 11, cubetofast(zero),
+                "invcoord_h48: h must be between 0 and 11\n");
+
         coclass = i / H48_ESIZE;
         ee = i % H48_ESIZE;
         esep = ee >> h;
         eo = (ee & ((1<<h)-1)) << (11-h);
 
-/* TODO: implement set_stuff methods
-        ret.c = crep[coclass];
-        ret.e = erep[esep];
+        copy_corners_fast(&ret, crep[coclass]);
+        copy_edges_fast(&ret, erep[esep]);
         set_eo_fast(&ret, eo);
-*/
 
         return ret;
 }