#include "pathfinder.h"

#include <map>
#include <queue>
#include <algorithm>
#include <set>

//#include "fiboheap.h"

PathFinder::PathFinder(){
}

std::vector<GraphNode*> PathFinder::a_star(GraphNode* start, GraphNode* goal, bool debug)
{
    // Check if priorityqueue sort value in right order.
    std::priority_queue<GraphNode*,std::vector<GraphNode*>,ComparePriority> frontier = std::priority_queue<GraphNode*,std::vector<GraphNode*>,ComparePriority>();
    std::map<GraphNode*, float> cost = std::map<GraphNode*, float>(); //cost of visited node
    std::map<GraphNode*, GraphNode*> pred = std::map<GraphNode*, GraphNode*>(); //pred of visited node

    std::set<GraphNode*> visited = std::set<GraphNode*>();

    // init frontier, cost, and pred with value for start
    frontier.push(start);
    cost.insert(std::pair<GraphNode*,float>(start,0));
    pred.insert(std::pair<GraphNode*,GraphNode*>(start,NULL));

    GraphNode* current;
    while(!frontier.empty()){
        //pick best element from frontier (with priority queue the best is in front)
        current = frontier.top();
        //current = pn->node.first;
        frontier.pop();
        if (visited.count(current) == 0)
            visited.insert(current);
        else
            continue;

        if(debug) std::cout << "Exploring node " << current->getValue() << std::endl;
        // goal reached, end of a-star
        if (current == goal){
            break;
        }

        // for all neighbours of current node
        for (GraphNode* next : current->getNeighbours()){
            float new_cost = cost[current] + current->cost(next);
            if(debug) std::cout << "\tExploring neighbours node " << next->getValue() << " with cost " << new_cost << std::endl;

            if ((cost.count(next) == 0) || (new_cost < cost[next])){
                // affect processed cost to next node in cost_map
                cost[next] = new_cost;

                // calcul priority of node with heuristic,and add it to frontier                
                float priority = new_cost + next->heuristic(goal);
                if(debug) std::cout << "\t\t Priority: " << priority << std::endl;
                next->setPriority(priority);
                visited.erase(next);
                frontier.push(next);
                // memorize predecessor for next
                pred[next] = current;
             }
        }
    }

    // reconstruct path by backtracking from goal to start
    std::vector<GraphNode*> path = std::vector<GraphNode*>();
    while(current != start){
        path.push_back(current);
        current = pred[current];
    }
    path.push_back(start);
    std::reverse(path.begin(),path.end());
    if(debug) std::cout << "path cost :" << cost[goal] << std::endl;
    return path;
}


bool ComparePriority::operator ()(GraphNode* a, GraphNode* b)
{
        return a->getPriority() > b->getPriority();
}