/*
* initialize the priority queue with the initial state
* explored <-- empty
* loop do
* if empty(queue) return failure
* node <-- queue.pop()
* if node is goal, then return solution(node)
* add node to explored
* for each action in Action(problem, node) do
* child node <-- childnode(problem, node, action)
* if child is not in queue or expolored, add node to queue
* otherwise, if the child.state is in queue and with a higher pathcost,
* then replace that node with the child
*
*/
bool UCSearch::doSearch(Problem &problem, std::vector<MapNode *> &path)
{
MapNode *init = problem.getInitState();
insertQueue(init);
while(!_queue->isEmpty())
{
MapNode *node = _queue->pop();
if(problem.isGoal(node))
{
node->getPathFromRoot(path);
return true;
}
insertExplored(node);
std::vector<MovAction_t> & action = problem.getActions();
std::vector<MovAction_t>::iterator it;
for(it=action.begin(); it<action.end(); it++)
{
if(*it == MOV_ACT_NOOP) {
continue;
}
MapNode *child = NULL;
int res = 0;
res = problem.movAction(node, *it, &child);
if(res == OP_OK)
{
if(!isInQueue(child) && !isInExplored(child))
{
insertQueue(child);
}
else
{
if(isInQueue(child))
{
//get old one
MapNode *old = findQueueByKey(child->getKey());
if(child->getPathCost() < old->getPathCost())
{
//update;
#ifdef HW1_DEBUG
std::cout <<"before update, child cost: " << child->getPathCost();
std::cout <<", old cost: " << old->getPathCost();
#endif
updateQueue(old, child);
#ifdef HW1_DEBUG
std::cout <<"after, old cost: " << old->getPathCost();
#endif
}
}
}
}
}
}
return false;
}
MapNode * Search::getFromQueue()
{
MapNode * node = _queue->pop();
int key = node->getKey();
std::map<int, MapNode *>::iterator it;
it = _stored.find(key);
_stored.erase(it);
return node;
}
