Added A* solution algorithm

1 files changed, 182 insertions, 0 deletions

diff --git a/solution_algorithm.cpp b/solution_algorithm.cpp
new file mode 100644
index 0000000..0955534
--- /dev/null
+++ b/solution_algorithm.cpp
@@ -0,0 +1,182 @@
+#include "solution_algorithm.hpp"
+#include "puzzle.hpp"
+
+#include <memory>
+#include <vector>
+#include <algorithm>
+#include <stdio.h>
+#include <unistd.h>
+#include <FL/Fl.H>
+
+typedef std::vector<GameParams::coordinates> state_type;
+
+std::unique_ptr<ASearch> ASearch::Start(GameParams *gp)
+{
+    /*
+     * init_xy numbering of puzzles in the correct sequence
+     */
+    state_type init_xy(puzzle_pieces, GameParams::coordinates{});
+    std::for_each(gp->puzzles.begin(), gp->puzzles.end(),
+                              [&init_xy]
+                              (const std::unique_ptr<Puzzle>& p) {
+                                    init_xy[p->sequence_number] =
+                                                        {p->x(), p->y()};
+                              });
+    init_xy[puzzle_pieces-1] = {gp->empty_box.x, gp->empty_box.y};
+    Node initial = Node(init_xy);
+    state_type goal_xy;
+    for(GameParams::coordinates& c : gp->standard_puzzle_coordinates)
+        goal_xy.push_back(c);
+    Node goal = Node(goal_xy);
+    return std::unique_ptr<ASearch>(new ASearch(initial, goal, gp));
+}
+
+Node* ASearch::FindSolution()
+{
+    ApplyFairEvaluator(initial);
+    open_queue.push(std::shared_ptr<Node>(new Node(initial)));
+    while(!open_queue.empty()) {
+        std::shared_ptr<Node> best_node = open_queue.top();
+        open_queue.pop();
+#ifdef DEBUG
+        printf("Current: \n");
+        for(GameParams::coordinates c : best_node->state)
+            printf("%d %d ", c.x, c.y);
+        printf("VALUE: %d\n", best_node->evaluation);
+#endif
+        if(goal == *best_node) {
+            open_queue.push(best_node);
+            return best_node.get();
+        }
+        close_set.insert(best_node);
+        vect_node free_moves = ComputeFreeMoves(best_node);
+        for(auto child_node : free_moves) {
+            if(close_set.find(child_node) != close_set.end())
+                continue;
+            ApplyFairEvaluator(*child_node);
+#ifdef DEBUG
+            printf("Child\n");
+            for(GameParams::coordinates c : child_node->state)
+                printf("%d %d ", c.x, c.y);
+            printf("HASH %ld ", std::hash<std::shared_ptr<Node>>()(child_node));
+            printf("VALUE: %d\n", child_node->evaluation);
+#endif
+            auto& PQ_cont = GetPQContainer(open_queue);
+            auto found_pos = std::find_if(PQ_cont.begin(),
+                                          PQ_cont.end(),
+                                     [&child_node]
+                                     (const std::shared_ptr<Node>& n) {
+                                        return *n == *child_node;
+                                     });
+            /* Open_queue doesn't contain child node */
+            if(found_pos == PQ_cont.end())
+                open_queue.push(child_node);
+            else if(child_node->evaluation < (*found_pos)->evaluation) {
+                PQ_cont.erase(std::remove(PQ_cont.begin(), PQ_cont.end(),
+                              *found_pos));
+                /*
+                 * reorder priority queue
+                 */
+                std::make_heap(PQ_cont.begin(), PQ_cont.end(), Node::Comp());
+                open_queue.push(child_node);
+            }
+        }
+    }
+    return nullptr;
+}
+
+static int compute_manhattan_distance(GameParams::coordinates first,
+                                      GameParams::coordinates second)
+{
+    return std::abs(first.x - second.x) + abs(first.y - second.y);
+}
+
+/*
+ * g(n) + W(n)
+ * g(n) is path length
+ * W(n) is sum of Manhattan distances
+ */
+void ASearch::ApplyFairEvaluator(Node& n) const
+{
+    int sum_md = 0;
+    for(int i = 0; i < puzzle_pieces; ++i)
+        sum_md +=
+            compute_manhattan_distance(n.state[i],
+                                       gp->standard_puzzle_coordinates[i]);
+    n.evaluation = n.depth + sum_md;
+}
+
+
+class MovesCreator {
+    vect_node& free_moves;
+    GameParams::coordinates empty_box_coord;
+    state_type cur_state;
+
+public:
+    MovesCreator(vect_node& fm, GameParams::coordinates ebc,
+                 state_type cs)
+                : free_moves(fm), empty_box_coord(ebc), cur_state(cs)
+                {}
+    void operator()(const GameParams::coordinates& c) {
+        if(IsNearEmptyBox(c, empty_box_coord)) {
+            state_type next_state = cur_state;
+            auto found_pos =
+                std::find_if(next_state.begin(), next_state.end()-1,
+                             [c]
+                             (const GameParams::coordinates& next_coord) {
+                                return next_coord == c;
+                             });
+            std::swap(*found_pos, next_state[puzzle_pieces-1]);
+            free_moves.push_back(std::shared_ptr<Node>(new Node(next_state)));
+        }
+    }
+private:
+    bool IsNearEmptyBox(GameParams::coordinates cur_puzzle,
+                        GameParams::coordinates empty_box) const;
+};
+
+bool MovesCreator::IsNearEmptyBox(GameParams::coordinates cur_puzzle,
+                                  GameParams::coordinates empty_box) const
+{
+    return (empty_box.x-spacing-puzzle_size == cur_puzzle.x &&
+            empty_box.y == cur_puzzle.y) ||
+            (empty_box.x == cur_puzzle.x &&
+             empty_box.y-spacing-puzzle_size == cur_puzzle.y) ||
+            (empty_box.x+puzzle_size+spacing == cur_puzzle.x &&
+             empty_box.y == cur_puzzle.y) ||
+            (empty_box.x == cur_puzzle.x &&
+             empty_box.y+puzzle_size+spacing == cur_puzzle.y);
+}
+
+vect_node ASearch::ComputeFreeMoves(const std::shared_ptr<Node>& parent) const
+{
+    vect_node free_moves;
+    GameParams::coordinates empty_box_coord = parent->state[puzzle_pieces-1];
+    std::for_each(parent->state.begin(), parent->state.end()-1,
+                    MovesCreator(free_moves, empty_box_coord, parent->state));
+    std::for_each(free_moves.begin(), free_moves.end(),
+                                    [parent]
+                                    (std::shared_ptr<Node>& child) {
+                                        child->depth = parent->depth + 1;
+                                        child->parent_p = parent.get();
+                                    });
+    return free_moves;
+}
+
+void ASearch::ShowSolution(Node *goal)
+{
+    if(goal->parent_p == nullptr)
+        return;
+    ShowSolution(goal->parent_p);
+    for(int i = 0; i < puzzle_pieces-1; ++i) {
+        auto puzzle_pos = std::find_if(gp->puzzles.begin(), gp->puzzles.end(),
+                                            [i]
+                                            (const std::unique_ptr<Puzzle>& p) {
+                                                return p->sequence_number == i;
+                                            });
+        (*puzzle_pos)->position(goal->state[i].x, goal->state[i].y);
+        gp->win->redraw();
+        usleep(80000);
+        Fl::wait();
+    }
+}