Working algorithm, its faster tan light!

6 files changed, 213 insertions, 13 deletions

diff --git a/Makefile b/Makefile
index 38f7853..cafcbf0 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,6 @@
 CXX = g++
-CXXFLAGS = -std=c++14 -g -Wall -Wshadow -Wextra -fsanitize=address -fsanitize=undefined
+# CXXFLAGS = -std=c++14 -g -Wall -Wshadow -Wextra -fsanitize=address -fsanitize=undefined
+CXXFLAGS = -std=c++14 -O3
 
 C_FILES = $(wildcard src/*.cpp)
 H_FILES = $(wildcard src/*.h)
diff --git a/scrambles.txt b/scrambles.txt
new file mode 100644
index 0000000..43a8f60
--- /dev/null
+++ b/scrambles.txt
@@ -0,0 +1,5 @@
+L R' F' U2 D2 R2 F D U' B2 U' R B R L B2 U R' F' L2 U' L2 D B R2
+B U2 D L2 F D2 U2 R' U' R2 D' F L' D B D' L2 U2 R2 U' B' F2 D2 F2 U2
+D R' B R2 B' L2 F2 D2 L2 R' B2 L D2 U2 L R2 D L R2 U2 F2 U' D' B U
+B2 F2 D2 R2 D2 U' L2 R2 F2 D2 F L R' B L2 R' B R' F' R2 L' B2 R2 U' B2
+D' B2 R' D' L' R' B2 U L' D' U2 R' F U2 L2 U2 F' D' B2 F' U' L D U L'
diff --git a/src/cube.h b/src/cube.h
index c050d22..ccc83d2 100644
--- a/src/cube.h
+++ b/src/cube.h
@@ -158,8 +158,7 @@ static const rot_f rot_func[] = {
   rotation_d,
   rotation_f,
   rotation_b,
-  rotation_rp,
-  rotation_lp,
+  rotation_rp, rotation_lp,
   rotation_up,
   rotation_dp,
   rotation_fp,
@@ -184,6 +183,13 @@ static void perform_rotations(cube_t *cube, rotations rots[]) {
   }
 }
 
+static rotations reverse_rotation(rotations rot) {
+  int ret = (int)rot;
+  if ((int)rot < 6) ret = (int)rot + 6;
+  if ((int)rot >= 6 && rot < 12) ret = (int)rot - 6;
+  return (rotations)ret;
+}
+
 /******************************************************************************/
 /* Parsing rotations from input ***********************************************/
 
@@ -222,7 +228,7 @@ static rotations* parse_scramble(char **str) {
   ssize_t i = 0;
   char *rot_str = str[0];
   while (rot_str) {
-	printf("token: %s\n", rot_str);
+	// printf("token: %s\n", rot_str);
 	rot_str = str[++sz];
   }
 
@@ -372,7 +378,7 @@ static void dump_cube_grid(cube_t *cube) {
 
 typedef uint32_t rot_cnt_t;
 #define build_rot_cnt(rot, cnt) (rot_cnt_t)(((uint32_t)(rot) << 16)|(cnt))
-#define get_rot(rot_cnt) (((uint32_t)(rot_cnt))>>16)
+#define get_rot(rot_cnt) (rotations)(((uint32_t)(rot_cnt))>>16)
 #define get_cnt(rot_cnt) (uint32_t)((rot_cnt) & 0xffff)
 
 KHASH_INIT(cube, cube_t, rot_cnt_t, 1, kh_cube_hash_func, kh_cube_hash_equal)
diff --git a/src/main.cpp b/src/main.cpp
index b66e3be..3144b0b 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,5 +1,41 @@
 #include "solver.hpp"
+#include <iostream>
+#include <time.h>       /* clock_t, clock, CLOCKS_PER_SEC */
+#include <math.h>       /* sqrt */
 
 int main() {
   Solver solver(6);
+  cube_t cube;
+  int i;
+  char *buf = NULL;
+  size_t sz;
+  clock_t timer;
+
+  while (getline(&buf, &sz, stdin) != -1) {
+	init_cube(&cube);
+	if (sz == 0) break;
+	printf("> %s\n", buf);
+	rotate_from_str(&cube, buf);		
+
+	dump_cube_grid(&cube);
+	timer = clock();
+	auto result = solver.solve(cube);
+	timer = clock() - timer;
+	std::cerr << "Time: " << (float)timer/CLOCKS_PER_SEC << "\n"; 
+
+
+	for (auto rot: result) {
+	  std::cerr << rot << " ";
+	}
+	std::cerr << "\n";
+
+	for (auto rot: result) {
+	  perform_rotation(&cube, rot);
+	}
+	dump_cube_grid(&cube);
+  }
+
+  if (buf)
+	free(buf);
+
 }
diff --git a/src/solver.cpp b/src/solver.cpp
index 98e8a13..d05771a 100644
--- a/src/solver.cpp
+++ b/src/solver.cpp
@@ -1,5 +1,7 @@
 #include <queue>
+// #include <priority_queue>
 #include <iostream>
+#include <algorithm>
 #include "solver.hpp"
 
 Solver::Solver(uint32_t pruning_tab_depth) {
@@ -9,6 +11,11 @@ Solver::Solver(uint32_t pruning_tab_depth) {
   generate_pruning_table(pruning_tab_depth);
 }
 
+Solver::~Solver() {
+  kh_destroy_cube(pruning_table);
+  kh_destroy_cube(visited_states);
+}
+
 void Solver::generate_pruning_table(uint32_t depth) {
   std::cerr << "Generating pruning table...\n";
   cube_t initial, cube, rotated;
@@ -28,13 +35,9 @@ void Solver::generate_pruning_table(uint32_t depth) {
 	cube = q.front();
 	q.pop();
 
-	// std::cerr << "processing cube:\n";
-	// dump_cube_grid(&cube);
-
 	k = kh_get_cube(pruning_table, cube);
 	rot_cnt = kh_val(pruning_table, k);
 	cnt = get_cnt(rot_cnt);
-	// std::cerr << cnt << "\n";
 	if (cnt >= depth) 
 	  continue;
 	
@@ -47,11 +50,154 @@ void Solver::generate_pruning_table(uint32_t depth) {
 		k = kh_put_cube(pruning_table, rotated, &ret);	
 		kh_val(pruning_table, k) = build_rot_cnt(rot, cnt+1);
 		q.push(rotated);
-
-		// printf("Cube cnt: %d\n", cnt+1);
-		// dump_cube_grid(&rotated);
 	  }
 	}
   }
+
   std::cerr << "Generated\n";
 }
+
+std::vector<rotations> Solver::get_solve_pruning(cube_t cube) {
+  khiter_t k;
+  cube_t initial;
+  rotations rot, rev_rot;
+  std::vector<rotations> result(0);
+  init_cube(&initial);
+
+  k = kh_get_cube(pruning_table, cube);
+  if (k == kh_end(pruning_table)) {
+	return result;
+  }
+
+  while (!kh_cube_hash_equal(cube, initial)) {
+	rot = get_rot(kh_val(pruning_table, k));
+	rev_rot = reverse_rotation(rot);	
+	result.push_back(rev_rot);
+
+	perform_rotation(&cube, rev_rot);
+	k = kh_get_cube(pruning_table, cube);
+  }
+
+  return result;
+}
+
+#define TILE_VAL(i) (face>>((i)<<2)&0xf)
+#define CMP_TILES(i, j) (uint32_t)(!!(TILE_VAL(i)==TILE_VAL(j)))
+
+/* Count number of neibourgh tiles of the same color */
+__attribute__((optimize("unroll-loops")))
+heur_t Solver::heuristic(const cube_t cube, const uint16_t moves_cnt) {
+  heur_t value = 0;
+  int i, j;
+  face_t face;
+  for (i = 0; i < 6; i++) {
+	face = cube.faces[i];
+	for (j = 0; j < 7; j++)
+	  value += CMP_TILES(j,j+1);
+	value += CMP_TILES(7,0);
+	for (j = 0; j < 4; j++)
+	  value += (TILE_VAL((j<<1)+1)==i);
+  }
+
+  /* This is absolutely random */
+  return value * 2 - moves_cnt * 3;
+}
+
+typedef std::pair<cube_t, heur_t> qentry_t;
+class Comp {
+public:
+  bool operator()(const qentry_t &e1, const qentry_t &e2) {
+	return e1.second < e2.second;
+  }
+};
+
+std::vector<rotations> Solver::retrieve_solution(cube_t cube) {
+  std::vector<rotations> result;
+  rot_cnt_t rot_cnt;
+  rotations rot, rev_rot;
+  khiter_t k;
+
+  k = kh_get_cube(visited_states, cube);
+  assert(k != kh_end(visited_states));
+  rot_cnt = kh_val(visited_states, k);
+
+  while ((rot = get_rot(rot_cnt)) != NULL_ROT) {
+	rev_rot = reverse_rotation(rot); 
+	result.push_back(rot);
+	perform_rotation(&cube, rev_rot);
+
+	k = kh_get_cube(visited_states, cube);
+	rot_cnt = kh_val(visited_states, k);
+  }
+
+
+  std::reverse(result.begin(), result.end());
+  return result;
+}
+
+std::vector<rotations> Solver::solve(const cube_t cube) {
+  cube_t state = cube;
+  cube_t new_state;
+  khiter_t k;
+  rot_cnt_t rot_cnt;
+  uint16_t cur_cnt;
+  int rot, ret;
+  bool found_solution = false;
+  std::priority_queue<qentry_t, std::vector<qentry_t>, Comp> pq;
+  
+  kh_clear_cube(visited_states);
+  k = kh_put_cube(visited_states, state, &ret);
+  kh_val(visited_states, k) = build_rot_cnt(NULL_ROT, 0);
+
+  pq.push({state, heuristic(cube, 0)});
+
+  while (!found_solution) {
+	auto cur = pq.top();
+	pq.pop();
+	state = cur.first;
+	// std::cerr << "Current state:\n";
+	// dump_cube_grid(&state);
+
+	k = kh_get_cube(visited_states, state);
+	cur_cnt = get_cnt(kh_val(visited_states, k));
+
+	for (rot = ROT_R; rot < NULL_ROT; rot++) {
+	  new_state = state;
+	  perform_rotation(&new_state, (rotations)rot);
+
+	  k = kh_get_cube(pruning_table, new_state);
+	  /* Found the cube in pruning table! We got it :D */
+	  if (k != kh_end(pruning_table)) {
+		state = new_state;
+		found_solution = true;
+		k = kh_put_cube(visited_states, new_state, &ret);
+		kh_val(visited_states, k) = build_rot_cnt(rot, cur_cnt+1);
+		break;
+	  }
+
+	  k = kh_get_cube(visited_states, new_state);
+
+	  /* This state was already visited.
+	   * Maybe we found a shorter path? */
+	  if (k != kh_end(visited_states)) {
+		rot_cnt = kh_val(visited_states, k);
+		if (get_cnt(rot_cnt) > cur_cnt + 1) {
+		  kh_val(visited_states, k) = build_rot_cnt(rot, cur_cnt+1);
+		}
+	  }
+	  /* This is a new unvisited state! */
+	  else {
+		k = kh_put_cube(visited_states, new_state, &ret);
+		kh_val(visited_states, k) = build_rot_cnt(rot, cur_cnt+1);
+		pq.push({new_state, heuristic(new_state, cur_cnt+1)});
+	  }
+	}
+  }
+
+  std::cerr << "Found solution!\n" << "\n";
+  auto res = retrieve_solution(state);
+  auto prun_res = get_solve_pruning(state);
+  //std::cerr << prun_res.size() << "\n";
+  res.insert(res.end(), prun_res.begin(), prun_res.end()); 
+  return res;
+}
diff --git a/src/solver.hpp b/src/solver.hpp
index 8c51b56..a41152e 100644
--- a/src/solver.hpp
+++ b/src/solver.hpp
@@ -5,15 +5,21 @@
 
 #include "cube.h"
 
+typedef int32_t heur_t;
+
 class Solver {
 private:
   kh_cube_t *pruning_table;
   kh_cube_t *visited_states;
 
   void generate_pruning_table(uint32_t depth);
+  std::vector<rotations> retrieve_solution(cube_t cube);
+  std::vector<rotations> get_solve_pruning(cube_t cube);
 public:
+  heur_t heuristic(const cube_t cube, const uint16_t moves_cnt);
   Solver(uint32_t pruning_tab_depth=0);
-  std::vector<rotations> solve(cube_t *cube);
+  ~Solver();
+  std::vector<rotations> solve(const cube_t cube);
 };
 
 #endif /* _ALGO_HPP_ */