#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();
    }
}