TreasureHunter

// TreasureHunter.cpp : Defines the entry point for the console application.
//

#include <iostream>
#include <fstream>
#include <vector>
#include <utility>
#include <deque>
#include <algorithm>
#include <iterator>
#include <set>
#include <cfloat>

#include <assert.h>

using namespace std;


// disable warning that debugging information has lines that are truncated
// occurs in stl headers
#pragma warning( disable : 4786 )

template <class T> class AStarState;

// The AStar search class. UserState is the users state space type
template <class UserState> class AStarSearch
{
public: // data
    enum
    {
        SEARCH_STATE_NOT_INITIALISED,
        SEARCH_STATE_SEARCHING,
        SEARCH_STATE_SUCCEEDED,
        SEARCH_STATE_FAILED,
        SEARCH_STATE_OUT_OF_MEMORY,
        SEARCH_STATE_INVALID
    };


    // A node represents a possible state in the search
    // The user provided state type is included inside this type
public:

    class Node
    {
    public:

        Node *parent; // used during the search to record the parent of successor nodes
        Node *child; // used after the search for the application to view the search in reverse
        UserState m_UserState;

        float g; // cost of this node + it's predecessors
        float h; // heuristic estimate of distance to goal
        float f; // sum of cumulative cost of predecessors and self and heuristic

        Node() :
            parent(0),
            child(0),
            g(0.0f),
            h(0.0f),
            f(0.0f)
        {}
    };


    // For sorting the heap the STL needs compare function that lets us compare
    // the f value of two nodes
    class HeapCompare_f
    {
    public:
        bool operator() (const Node *x, const Node *y) const { return x->f > y->f; }
    };

public: // methods
    // constructor just initialises private data
    AStarSearch() :
        m_AllocateNodeCount(0),
        m_State(SEARCH_STATE_NOT_INITIALISED),
        m_CurrentSolutionNode(NULL),
        m_CancelRequest(false)
    {}

    AStarSearch(int MaxNodes) :
        m_AllocateNodeCount(0),
        m_State(SEARCH_STATE_NOT_INITIALISED),
        m_CurrentSolutionNode(NULL),
        m_CancelRequest(false)
    {}

    // call at any time to cancel the search and free up all the memory
    void CancelSearch() { m_CancelRequest = true; }

    // Set Start and goal states
    void SetStartAndGoalStates(UserState Start, UserState Goal)
    {
        m_CancelRequest = false;
        m_Start = AllocateNode();
        m_Goal = AllocateNode();
        assert((m_Start != NULL && m_Goal != NULL)); //THROW
        m_Start->m_UserState = Start;
        m_Goal->m_UserState = Goal;
        m_State = SEARCH_STATE_SEARCHING;
        // Initialise the AStar specific parts of the Start Node
        // The user only needs fill out the state information
        m_Start->g = 0;
        m_Start->h = m_Start->m_UserState.GoalDistanceEstimate(m_Goal->m_UserState);
        m_Start->f = m_Start->g + m_Start->h;
        m_Start->parent = 0;
        // Push the start node on the Open list
        m_OpenList.push_back(m_Start); // heap now unsorted
        // Sort back element into heap
        push_heap(m_OpenList.begin(), m_OpenList.end(), HeapCompare_f());
        // Initialise counter for search steps
        m_Steps = 0;
    }

    // Advances search one step
    unsigned int SearchStep()
    {
        // Firstly break if the user has not initialised the search
        assert((m_State > SEARCH_STATE_NOT_INITIALISED) && (m_State < SEARCH_STATE_INVALID)); //THROW
        // Next I want it to be safe to do a searchstep once the search has succeeded...
        if ((m_State == SEARCH_STATE_SUCCEEDED) || (m_State == SEARCH_STATE_FAILED)) {
            return m_State;
        }
        // Failure is defined as emptying the open list as there is nothing left to
        // search...
        // New: Allow user abort
        if (m_OpenList.empty() || m_CancelRequest) {
            FreeAllNodes();
            m_State = SEARCH_STATE_FAILED;
            return m_State;
        }
        // Incremement step count
        m_Steps++;
        // Pop the best node (the one with the lowest f)
        Node *n = m_OpenList.front(); // get pointer to the node
        pop_heap(m_OpenList.begin(), m_OpenList.end(), HeapCompare_f());
        m_OpenList.pop_back();
        // Check for the goal, once we pop that we're done
        if (n->m_UserState.IsGoal(m_Goal->m_UserState))
        {
            // The user is going to use the Goal Node he passed in
            // so copy the parent pointer of n
            m_Goal->parent = n->parent;
            m_Goal->g = n->g;

            // A special case is that the goal was passed in as the start state
            // so handle that here
            if (false == n->m_UserState.IsSameState(m_Start->m_UserState)) {
                FreeNode(n);
                // set the child pointers in each node (except Goal which has no child)
                Node *nodeChild = m_Goal;
                Node *nodeParent = m_Goal->parent;
                do              {
                    nodeParent->child = nodeChild;
                    nodeChild = nodeParent;
                    nodeParent = nodeParent->parent;
                } while (nodeChild != m_Start); // Start is always the first node by definition
            }
            // delete nodes that aren't needed for the solution
            FreeUnusedNodes();
            m_State = SEARCH_STATE_SUCCEEDED;
            return m_State;
        }
        else // not goal
        {
            // We now need to generate the successors of this node
            // The user helps us to do this, and we keep the new nodes in
            // m_Successors ...
            m_Successors.clear(); // empty vector of successor nodes to n
            // User provides this functions and uses AddSuccessor to add each successor of
            // node 'n' to m_Successors
            bool ret = n->m_UserState.GetSuccessors(this, n->parent ? &n->parent->m_UserState : NULL);
            if (!ret)
            {
                typename vector< Node * >::iterator successor;
                // free the nodes that may previously have been added
                for (successor = m_Successors.begin(); successor != m_Successors.end(); successor++) {
                    FreeNode((*successor));
                }
                m_Successors.clear(); // empty vector of successor nodes to n
                // free up everything else we allocated
                FreeAllNodes();
                m_State = SEARCH_STATE_OUT_OF_MEMORY;
                return m_State;
            }

            // Now handle each successor to the current node ...
            for (typename vector< Node * >::iterator successor = m_Successors.begin(); successor != m_Successors.end(); successor++) {
                //  The g value for this successor ...
                float newg = n->g + n->m_UserState.GetCost((*successor)->m_UserState);
                // Now we need to find whether the node is on the open or closed lists
                // If it is but the node that is already on them is better (lower g)
                // then we can forget about this successor

                // First linear search of open list to find node
                typename vector< Node * >::iterator openlist_result;
                for (openlist_result = m_OpenList.begin(); openlist_result != m_OpenList.end(); openlist_result++) {
                    if ((*openlist_result)->m_UserState.IsSameState((*successor)->m_UserState)) {
                        break;
                    }
                }
                if (openlist_result != m_OpenList.end())
                {
                    // we found this state on open
                    if ((*openlist_result)->g <= newg) {
                        FreeNode((*successor));
                        // the one on Open is cheaper than this one
                        continue;
                    }
                }

                typename vector< Node * >::iterator closedlist_result;
                for (closedlist_result = m_ClosedList.begin(); closedlist_result != m_ClosedList.end(); closedlist_result++) {
                    if ((*closedlist_result)->m_UserState.IsSameState((*successor)->m_UserState)) {
                        break;
                    }
                }

                if (closedlist_result != m_ClosedList.end())
                {
                    // we found this state on closed
                    if ((*closedlist_result)->g <= newg) {
                        // the one on Closed is cheaper than this one
                        FreeNode((*successor));
                        continue;
                    }
                }

                // This node is the best node so far with this particular state
                // so lets keep it and set up its AStar specific data ...

                (*successor)->parent = n;
                (*successor)->g = newg;
                (*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate(m_Goal->m_UserState);
                (*successor)->f = (*successor)->g + (*successor)->h;

                // Remove successor from closed if it was on it
                if (closedlist_result != m_ClosedList.end()) {
                    // remove it from Closed
                    FreeNode((*closedlist_result));
                    m_ClosedList.erase(closedlist_result);
                    // Fix thanks to ...
                    // Greg Douglas <gregdouglasmail@gmail.com>
                    // who noticed that this code path was incorrect
                    // Here we have found a new state which is already CLOSED
                    // anus
                }

                // Update old version of this node
                if (openlist_result != m_OpenList.end()) {
                    FreeNode((*openlist_result));
                    m_OpenList.erase(openlist_result);

                    // re-make the heap
                    // make_heap rather than sort_heap is an essential bug fix
                    // thanks to Mike Ryynanen for pointing this out and then explaining
                    // it in detail. sort_heap called on an invalid heap does not work
                    make_heap(m_OpenList.begin(), m_OpenList.end(), HeapCompare_f());
                }

                // heap now unsorted
                m_OpenList.push_back((*successor));
                // sort back element into heap
                push_heap(m_OpenList.begin(), m_OpenList.end(), HeapCompare_f());
            }
            // push n onto Closed, as we have expanded it now
            m_ClosedList.push_back(n);
        } // end else (not goal so expand)

        return m_State; // Succeeded bool is false at this point.

    }

    // User calls this to add a successor to a list of successors
    // when expanding the search frontier
    bool AddSuccessor(UserState State)
    {
        Node *node = AllocateNode();
        if (node) {
            node->m_UserState = State;
            m_Successors.push_back(node);
            return true;
        }
        return false;
    }

    // Free the solution nodes
    // This is done to clean up all used Node memory when you are done with the
    // search
    void FreeSolutionNodes()
    {
        Node *n = m_Start;
        if (m_Start->child)
        {
            do
            {
                Node *del = n;
                n = n->child;
                FreeNode(del);
                del = nullptr;
            } while (n != m_Goal);

            FreeNode(n); // Delete the goal
        }
        else
        {
            // if the start node is the solution we need to just delete the start and goal
            // nodes
            FreeNode(m_Start);
            FreeNode(m_Goal);
        }

    }

    unsigned int SearchSolution(unsigned int& nbStep, std::vector<UserState>& solution)
    {
        unsigned int SearchState;
        unsigned int SearchSteps = 0;

        do
        {
            SearchState = SearchStep();
            SearchSteps++;
            /*cout << "Steps:" << SearchSteps << "\n";
            int len = 0;
            cout << "Open:\n";
            UserState *p = GetOpenListStart();
            while (p)
            {
            len++;
            p = GetOpenListNext();

            }
            cout << "Open list has " << len << " nodes\n";
            len = 0;
            cout << "Closed:\n";
            p = GetClosedListStart();
            while (p)
            {
            len++;
            p = GetClosedListNext();
            }
            cout << "Closed list has " << len << " nodes\n";
            //*/
        } while (SearchState == AStarSearch<UserState>::SEARCH_STATE_SEARCHING);

        solution.clear();
        if (SearchState == AStarSearch<UserState>::SEARCH_STATE_SUCCEEDED)
        {
            //cout << "Search found goal state\n";
            UserState *node = GetSolutionStart();
            int steps = 0;
            solution.push_back(*node);
            for (;;)
            {
                node = GetSolutionNext();
                if (!node) {
                    break;
                }
                solution.push_back(*node);
                steps++;
            };
            //cout << "Solution steps " << steps << endl;
            // Once you're done with the solution you can free the nodes up
            FreeSolutionNodes();
        }
        else if (SearchState == AStarSearch<UserState>::SEARCH_STATE_FAILED)
        {
            //cout << "Search terminated. Did not find goal state\n";
        }

        nbStep = SearchSteps;

        return SearchState;
    }


    // Functions for traversing the solution

    // Get start node
    UserState *GetSolutionStart()
    {
        m_CurrentSolutionNode = m_Start;
        if (m_Start)
        {
            return &m_Start->m_UserState;
        }
        else
        {
            return nullptr;
        }
    }

    // Get next node
    UserState *GetSolutionNext()
    {
        if (m_CurrentSolutionNode)
        {
            if (m_CurrentSolutionNode->child)
            {
                Node *child = m_CurrentSolutionNode->child;
                m_CurrentSolutionNode = m_CurrentSolutionNode->child;
                return &child->m_UserState;
            }
        }
        return nullptr;
    }

    // Get end node
    UserState *GetSolutionEnd()
    {
        m_CurrentSolutionNode = m_Goal;
        if (m_Goal) {
            return &m_Goal->m_UserState;
        }
        else {
            return nullptr;
        }
    }

    // Step solution iterator backwards
    UserState *GetSolutionPrev()
    {
        if (m_CurrentSolutionNode)
        {
            if (m_CurrentSolutionNode->parent)
            {
                Node *parent = m_CurrentSolutionNode->parent;
                m_CurrentSolutionNode = m_CurrentSolutionNode->parent;
                return &parent->m_UserState;
            }
        }
        return nullptr;
    }

    // Get final cost of solution
    // Returns FLT_MAX if goal is not defined or there is no solution
    float GetSolutionCost()
    {
        if (m_Goal && m_State == SEARCH_STATE_SUCCEEDED)
        {
            return m_Goal->g;
        }
        else
        {
            return FLT_MAX;
        }
    }

    // For educational use and debugging it is useful to be able to view
    // the open and closed list at each step, here are two functions to allow that.

    UserState *GetOpenListStart()
    {
        float f, g, h;
        return GetOpenListStart(f, g, h);
    }

    UserState *GetOpenListStart(float &f, float &g, float &h)
    {
        iterDbgOpen = m_OpenList.begin();
        if (iterDbgOpen != m_OpenList.end())
        {
            f = (*iterDbgOpen)->f;
            g = (*iterDbgOpen)->g;
            h = (*iterDbgOpen)->h;
            return &(*iterDbgOpen)->m_UserState;
        }

        return nullptr;
    }

    UserState *GetOpenListNext()
    {
        float f, g, h;
        return GetOpenListNext(f, g, h);
    }

    UserState *GetOpenListNext(float &f, float &g, float &h)
    {
        iterDbgOpen++;
        if (iterDbgOpen != m_OpenList.end())
        {
            f = (*iterDbgOpen)->f;
            g = (*iterDbgOpen)->g;
            h = (*iterDbgOpen)->h;
            return &(*iterDbgOpen)->m_UserState;
        }

        return nullptr;
    }

    UserState *GetClosedListStart()
    {
        float f, g, h;
        return GetClosedListStart(f, g, h);
    }

    UserState *GetClosedListStart(float &f, float &g, float &h)
    {
        iterDbgClosed = m_ClosedList.begin();
        if (iterDbgClosed != m_ClosedList.end())
        {
            f = (*iterDbgClosed)->f;
            g = (*iterDbgClosed)->g;
            h = (*iterDbgClosed)->h;

            return &(*iterDbgClosed)->m_UserState;
        }

        return nullptr;
    }

    UserState *GetClosedListNext()
    {
        float f, g, h;
        return GetClosedListNext(f, g, h);
    }

    UserState *GetClosedListNext(float &f, float &g, float &h)
    {
        iterDbgClosed++;
        if (iterDbgClosed != m_ClosedList.end())
        {
            f = (*iterDbgClosed)->f;
            g = (*iterDbgClosed)->g;
            h = (*iterDbgClosed)->h;

            return &(*iterDbgClosed)->m_UserState;
        }

        return nullptr;
    }

    // Get the number of steps

    int GetStepCount() { return m_Steps; }

    void EnsureMemoryFreed()
    {}

private: // methods

    // This is called when a search fails or is cancelled to free all used
    // memory
    void FreeAllNodes()
    {
        // iterate open list and delete all nodes
        typename vector< Node * >::iterator iterOpen = m_OpenList.begin();

        while (iterOpen != m_OpenList.end())
        {
            Node *n = (*iterOpen);
            FreeNode(n);
            iterOpen++;
        }
        m_OpenList.clear();
        // iterate closed list and delete unused nodes
        typename vector< Node * >::iterator iterClosed;
        for (iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed++)
        {
            Node *n = (*iterClosed);
            FreeNode(n);
        }
        m_ClosedList.clear();
        // delete the goal
        FreeNode(m_Goal);
    }


    // This call is made by the search class when the search ends. A lot of nodes may be
    // created that are still present when the search ends. They will be deleted by this
    // routine once the search ends
    void FreeUnusedNodes()
    {
        // iterate open list and delete unused nodes
        typename vector< Node * >::iterator iterOpen = m_OpenList.begin();
        while (iterOpen != m_OpenList.end())
        {
            Node *n = (*iterOpen);
            if (!n->child)
            {
                FreeNode(n);
                n = nullptr;
            }
            iterOpen++;
        }
        m_OpenList.clear();
        // iterate closed list and delete unused nodes
        typename vector< Node * >::iterator iterClosed;
        for (iterClosed = m_ClosedList.begin(); iterClosed != m_ClosedList.end(); iterClosed++)
        {
            Node *n = (*iterClosed);
            if (!n->child)
            {
                FreeNode(n);
                n = nullptr;
            }
        }
        m_ClosedList.clear();
    }

    // Node memory management
    Node *AllocateNode()
    {
        Node *p = new Node;
        return p;
    }

    void FreeNode(Node *node)
    {
        m_AllocateNodeCount--;
        delete node;
    }

private: // data

    // Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article)
    vector< Node *> m_OpenList;
    // Closed list is a vector.
    vector< Node * > m_ClosedList;
    // Successors is a vector filled out by the user each type successors to a node
    // are generated
    vector< Node * > m_Successors;
    // State
    unsigned int m_State;
    // Counts steps
    int m_Steps;
    // Start and goal state pointers
    Node *m_Start;
    Node *m_Goal;
    // Node list
    Node *m_CurrentSolutionNode;

    //Debug : need to keep these two iterators around
    // for the user Dbg functions
    typename vector< Node * >::iterator iterDbgOpen;
    typename vector< Node * >::iterator iterDbgClosed;
    // debugging : count memory allocation and free's
    int m_AllocateNodeCount;

    //To Stop the search loop .... When it will be inside ...
    bool m_CancelRequest;
};

template <class T> class AStarState
{
public:
    virtual ~AStarState() {}
    virtual float GoalDistanceEstimate(T &nodeGoal) = 0; // Heuristic function which computes the estimated cost to the goal node
    virtual bool IsGoal(T &nodeGoal) = 0; // Returns true if this node is the goal node
    virtual bool GetSuccessors(AStarSearch<T> *astarsearch, T *parent_node) = 0; // Retrieves all successors to this node and adds them via astarsearch.addSuccessor()
    virtual float GetCost(T &successor) = 0; // Computes the cost of travelling from this node to the successor node
    virtual bool IsSameState(T &rhs) = 0; // Returns true if this node is the same as the rhs node
};


typedef vector<vector<char>> maze_t;

int fact(int i)
{
    if (i == 1)
        return i;
    else
        return i * fact(i - 1);
}

struct testcase_t
{
    int num;
    maze_t maze;
    int earliestTime; //-1 if no solution
    int expectedResult;

    testcase_t() { num = 0; earliestTime = -1; expectedResult = -1; }


    void printMaze()
    {
        cout << "Maze Number " << num << endl;
        cout << "Maze " << maze.size() << "x" << maze.size() << endl;
        cout << "Expected result " << expectedResult << endl;
        for (auto &row : maze) {
            for (auto &c : row) {
                cout << c << " ";
            }
            cout << endl;
        }
    }

    void printResult()
    {
        //cout << "Result " << earliestTime << ((expectedResult == earliestTime) ? " Success !!!! " : ((expectedResult < earliestTime) ? " Failure expected time smaller " : " Failure expected time bigger ")) << endl;
        cout << earliestTime << endl;
    }

    struct pos_t {
        int x; int y;
        pos_t(){ x = 0; y = 0; }
        pos_t(int X, int Y){ x = X; y = Y; }
        pos_t(pos_t const & r) { x = r.x; y = r.y; }
        pos_t& operator=(pos_t const & r) { x = r.x; y = r.y; return *this; }
        bool operator==(pos_t const & r) { return x == r.x && y == r.y; }
        bool operator < (pos_t const & p) const
        {
            return (x < p.x) || ((x == p.x) && (y < p.y));
        }
        friend ostream& operator << (ostream &o, const pos_t& p)
        {
            return o << "(" << p.x << "," << p.y << ")";
        }

        bool isIn(maze_t const & m) { return x >= 0 && x < (int)(m.size()) && y >= 0 && y < (int)(m.size()); }
        bool isAvailable(maze_t const & m) { return m[x][y] != *("#"); }
        unsigned int ManhattanDistanceTo(pos_t const &p) { return (unsigned int)(abs(x - p.x) + abs(y - p.y)); }
        vector<pos_t> GetCanditate(maze_t const & m)
        {
            vector<pos_t> c = { pos_t(x - 1, y), pos_t(x, y - 1), pos_t(x + 1, y), pos_t(x, y + 1) };
            auto it = begin(c);
            while (it != end(c)) {
                if (!(*it).isIn(m) || !(*it).isAvailable(m)){
                    it = c.erase(it);
                }
                else {
                    ++it;
                }
            }
            return c;
        }
    };

    struct search_state_t
    {
        pos_t _curr;
        const maze_t * _maze_ref;

        search_state_t() : _curr(), _maze_ref(nullptr) {}
        search_state_t(const pos_t& p, const maze_t * m) :_curr(p), _maze_ref(m){}
        search_state_t(search_state_t const & r) { _curr = r._curr; _maze_ref = r._maze_ref; }
        search_state_t& operator=(search_state_t const & r) { _curr = r._curr; _maze_ref = r._maze_ref; return *this; }

        float GoalDistanceEstimate(search_state_t &nodeGoal)
        {
            return (float)(_curr.ManhattanDistanceTo(nodeGoal._curr));
        }
        bool IsGoal(search_state_t &nodeGoal)
        {
            return _curr == nodeGoal._curr;
        }
        bool GetSuccessors(AStarSearch<search_state_t> *astarsearch, search_state_t *parent_node)
        {
            auto c = _curr.GetCanditate(*_maze_ref);
            auto it = begin(c);
            while (it != end(c)) {
                if (parent_node != nullptr && (*it) == parent_node->_curr){
                    it = c.erase(it);
                }
                else {
                    astarsearch->AddSuccessor(search_state_t(*it, _maze_ref));
                    ++it;
                }
            }
            return true;
        }
        float GetCost(search_state_t &successor)
        {
            return 1.0;
        }
        bool IsSameState(search_state_t &rhs)
        {
            return _curr == rhs._curr;
        }

    };

    void solve()
    {
        //cout << "Enter solve for maze " << maze.size() << "x" << maze.size() << endl;
        vector<pos_t> treasurePos;
        for (auto i = 0u; i < maze.size(); ++i)
        {
            for (auto j = 0u; j < maze[i].size(); ++j)
            {
                if (maze[i][j] == *("*"))
                    treasurePos.push_back(pos_t(i, j));
            }
        }

        deque<vector<pos_t>> treasure_order;
        std::sort(begin(treasurePos), end(treasurePos));
        do {
            vector<pos_t> c;

            c.push_back(pos_t(0, 0));
            copy(begin(treasurePos), end(treasurePos), back_inserter(c));
            c.push_back(pos_t(maze.size() - 1, maze.size() - 1));

            /*copy(begin(c), end(c), ostream_iterator<pos_t>(cout, " -> "));
            cout << endl;//*/
            treasure_order.push_back(c);

        } while (std::next_permutation(begin(treasurePos), end(treasurePos)));//*/
        //cout << "we stored " << treasure_order.size() << " path to get all the treasure (=nbTreasure! = " << fact((int)treasurePos.size()) << ")" << endl;

        vector<vector<pair<pos_t, pos_t>>> allCandidatePath;
        for (auto &p : treasure_order)
        {
            vector<pair<pos_t, pos_t>> candidatePath;
            for (auto b = begin(p); b < end(p) - 1; ++b)
            {
                candidatePath.push_back(make_pair(*b, *(b + 1)));
            }
            allCandidatePath.push_back(candidatePath);
        }

        /*for_each(begin(allCandidatePath), end(allCandidatePath),
            [](vector<pair<pos_t, pos_t>> const & p)
            {
            for_each(begin(p), end(p), [](pair<pos_t, pos_t> const & j)
            {
            cout << j.first << "->" << j.second;
            });
            cout << endl;
            });*/
        int minPath = std::numeric_limits<int>::max();
        for (auto &p : allCandidatePath)
        {
            int minPathCandidate = 0;
            //if A* path from b.first to b.second exist
            // store path lenght and next tuple.
            //else
            // mark that path as invalid and continue with nex candidate path...
            auto it = begin(p);
            while (it != end(p))
            {
                auto checkpath = [this, &minPathCandidate](pair<pos_t, pos_t> const & j)
                {
                    //cout << "Search A* from " << j.first << " to " << j.second << std::endl;
                    AStarSearch<search_state_t> astarsearch;
                    astarsearch.SetStartAndGoalStates(search_state_t(j.first, &(this->maze)), search_state_t(j.second, &(this->maze)));
                    std::vector<search_state_t> path;
                    unsigned int nb_step;
                    auto res = astarsearch.SearchSolution(nb_step, path);
                    if (res == AStarSearch<search_state_t>::SEARCH_STATE_SUCCEEDED) {
                        /*std::cout << "Found with " << nb_step << " step a path of " << path.size() << std::endl;
                        for (const auto& s : path){
                            std::cout << s._curr << " -> ";
                        }
                        std::cout << std::endl;//*/
                        minPathCandidate += path.size() - 1;
                    }
                    else {
                        //std::cout << "No solution with " << std::endl;
                        minPathCandidate = 0;
                    }
                    astarsearch.EnsureMemoryFreed();
                };

                checkpath(*it);
                if (minPathCandidate == 0)
                    break;

                it++;
            }

            //if path is success get its total length and store it
            if (minPathCandidate != 0)
            {
                minPath = std::min(minPath, minPathCandidate);
            }
        }

        //Get Min Length and Min index.
        //Display Minimal Path and duration !
        this->earliestTime = ((minPath != std::numeric_limits<int>::max()) ? minPath : -1);
        /*cout << "**************************************\n";
        cout << "Min Path = " <<  this->earliestTime << endl;
        cout << "**************************************\n";*/

    }
};

void LoadTestCases(istream& in, vector<testcase_t>& tests)
{
    int num = 0;
    int nbTest = 0;
    in >> nbTest;

    while (nbTest > 0)
    {
        int mazeSize = 0;
        in >> mazeSize;
        testcase_t test;
        test.num = num++;
        for (auto i = 0; i < mazeSize; ++i)
        {
            vector<char> row;
            for (auto j = 0; j < mazeSize; ++j)
            {
                char c;
                in >> c;
                row.push_back(c);
            }
            test.maze.push_back(row);
        }
        in >> test.expectedResult;
        tests.push_back(test);

        --nbTest;
    }
}

int main() {
    vector<testcase_t> tests;

    LoadTestCases(cin, tests);
    /*ifstream testFile("testSet.txt");
    LoadTestCases(testFile, tests);//*/

    for (auto test : tests)
    {
        //cout << "*********** test " << test.num << " *************" << endl << endl;
        //test.printMaze();

        test.solve();
        test.printResult();

        //cout << endl << "************************" << endl;
    }

    return 0;
}