Node<Plot>* search(std::map<Plot*, std::vector<Plot*> >& path, Plot& start, Plot& goal)
{
Node<Plot>* node_goal = new Node<Plot>(goal);
Node<Plot>* node_start = new Node<Plot>(start);
PriorityQueue queue;
std::unordered_map<Plot*, int> closeList;
queue.insert(node_start);
int iter = 0;
while (!queue.empty())
{
iter++;
Node<Plot> *current(*(queue.begin()));
queue.erase(queue.begin());
std::vector<Plot*>& neighbours(path[current->node]);
for (Plot *p : neighbours)
{
Node<Plot>* next = new Node<Plot>(*p);
next->g = current->g + manhattan_distance(current->node, p);
next->h = manhattan_distance(p, &goal);
next->f = next->g + next->h;
next->x = current->x + 1; // nb turns
next->parent = current;
if (p == node_goal->node)
{
std::cout << "nbIter: " << iter << std::endl;
return next;
}
if (isWorthTrying(next, queue, closeList))
{
queue.insert(next);
}
}
closeList[current->node] = current->f;
delete current;
}
return nullptr;
}
bool isWorthTrying(Node<Plot>* node, PriorityQueue& pqueue, std::unordered_map<Plot*, int>& closeList)
{
auto found(pqueue.find(node));
if (found != pqueue.end())
{
if ((*found)->f < node->f) return false;
else
{
Node<Plot>* n(*found);
pqueue.erase(found);
delete n;
}
}
auto cl_found(closeList.find(node->node));
if (cl_found != closeList.end())
{
if (cl_found->second < node->f) return false;
else closeList.erase(cl_found);
}
return true;
}
