#include "isearch.h"
#include <algorithm>

ISearch::ISearch()
{
    hweight = 1;
    breakingties = CN_SP_BT_GMAX;
}

ISearch::~ISearch(void) {}


SearchResult ISearch::startSearch(ILogger *Logger, const Map &map, const EnvironmentOptions &options)
{
    Node curNode, stNode, fnNode, *prevNode;
    stNode.i = map.getStart().first;
    stNode.j = map.getStart().second;
    fnNode.i = map.getGoal().first;
    fnNode.j = map.getGoal().second;
    stNode.H = computeHFromCellToCell(stNode.i, stNode.j, fnNode.i, fnNode.j, options);
    stNode.g = 0;
    stNode.F = stNode.H * ISearch::hweight;
    ISearch::open.push_back(stNode);
    while (!ISearch::open.empty()) {
        sresult.numberofsteps++;
        curNode = ISearch::open.back();
        prevNode = &curNode;
        ISearch::open.pop_back();
        ISearch::close.push_back(curNode);
        if (curNode == fnNode) {
            sresult.pathfound = true;
            sresult.nodescreated = ISearch::close.size() + ISearch::open.size();
            while (!(curNode == stNode)) {
                lppath.push_front(curNode);
                curNode = *curNode.parent;
            }
        } else {
            for (auto neiNode : ISearch::findSuccessors(curNode, map, options)) {
                if (std::find(ISearch::open.begin(), ISearch::open.end(), neiNode) == ISearch::open.end() &&
                    std::find(ISearch::close.begin(), ISearch::close.end(), neiNode) == ISearch::close.end()) {
                    neiNode.H = computeHFromCellToCell(neiNode.i, neiNode.j, fnNode.i, fnNode.j, options);
                    neiNode.g = curNode.g + 1;
                    neiNode.parent = new Node(&curNode);
                    neiNode.F = neiNode.H * ISearch::hweight + neiNode.g;
                    ISearch::open.push_back(neiNode);
                }
            }
            std::sort(ISearch::open.begin(), ISearch::open.end(), [this](const Node & a, const Node & b) {
                return (a.F > b.F || (a.F == b.F && ISearch::breakingties == (a.H > b.H)));
            });
        }
    }
    //sresult.hppath = &hppath; //Here is a constant pointer
    sresult.lppath = &lppath;
    return sresult;
}

std::vector<Node> ISearch::findSuccessors(Node curNode, const Map &map, const EnvironmentOptions &options)
{
    std::vector<Node> successors;
    Node sucNode;
    sucNode.i = curNode.i - 1;
    sucNode.j = curNode.j;
    for (size_t i = 0; i < 4; ++i){
        if (map.CellOnGrid(sucNode.i, sucNode.j))
            if (map.CellIsTraversable(sucNode.i, sucNode.j))
                successors.push_back(sucNode);
        sucNode.i += (i < 2 ? 1 : -1);
        sucNode.j += ((0 < i && i < 3) ? 1 : -1);
    }
    return successors;
}