#include "path.hpp"

/*____________________________________________________________________________
protected and virtual method to modify
____________________________________________________________________________*/

//get all direction from here
stack<node> * pathfind::getAllNeighbor(int from)
{
    return new stack<node>;;
}

//initialisation
void pathfind::init()
{
    _found = false;
    _open.clear();
    _closed.clear();
}

/*____________________________________________________________________________
private method
____________________________________________________________________________*/

//find in open list any node that math , 
int pathfind::findInOpen(int index, int parent)
{
    for (int i=0;i<_open.size();i++)
        if(_open.at(i).index == index && _open.at(i).parent == parent)
            return i;
    return _open.size();
}

//find in open list lowest cost matching index
int pathfind::findBestRoute(int index)
{
    int best = -1;
    for (int i = 0; i<_open.size();i++)
        if (_open.at(i).index == index && (best==-1 || (best!=i && _open.at(best).cost > _open.at(i).cost)))
            best = i;
    return best;
}

//try to add new node to open, if not already in with better cost
bool pathfind::addToOpen(node & nu)
{
    int dupe = findInOpen(nu.index,nu.parent);           //have we already passed here before?
    if (dupe==_open.size())                        //if not add it and recurse(word??)
        _open.push_back(nu);
    else if (_open.at(dupe).cost > nu.cost)          //if yes, replace only if we have better cost
        _open.at(dupe) = nu;
    else                                       //else dont call recurse
        return false;
    return true;
}

//recursive open list builder with cost accumulation
void pathfind::recursif(int index, int goal, int cost)
{
    //from here get all neighbor
    stack <node>* neighbor = getAllNeighbor(index);
    bool recurse;
    int dupe;
    while(!neighbor->empty())
    {
        node nu = neighbor->top();                  //candidate node
        nu.cost += cost;                            //add cost we gone through
        if (addToOpen(nu))                          //if not already in open or better
            if (nu.index != goal)                   //and not acheived goal
                recursif(nu.index,goal,nu.cost);    //continue recursive algo
            else
                _found = true;                     //found goal
        
        neighbor->pop();                            //done with that neighbor
    }
    delete neighbor;
}

/*____________________________________________________________________________
public method
____________________________________________________________________________*/

//construtor
pathfind::pathfind()
{
    init();
}

//return node in closed list containing path in the right order
const node & pathfind::at(int i)const
{
    if (i< _closed.size())
        return _closed.at(_closed.size() - 1 - i);
}

//return number of node in closed list
int pathfind::size()const
{
    return _closed.size();
}

//return number of node in closed list
bool pathfind::found()const
{
    return _found;
}

//process the the path finding
void pathfind::fromTo(int start, int goal)
{
    init();
    //recursively populate the open list
    recursif(start,goal,0);
    //the rebuild closed list
    if (_found)
    {
        int i = findBestRoute(goal);
        while (_open.at(i).index != start)
        {
            _closed.push_back(_open.at(i));
            i = findBestRoute(_open.at(i).parent);
        }
    }
}