struct Coord
{
   int x;
   int y;
   int moves;
   int h;
   bool operator< (const Coord& rhs) const
   {
       return this->h > rhs.h;
   }
   Coord (int x, int y, int moves, int h):x(x),y(y),moves(moves),h(h){};    
};

class Solution 
{
   private:
   vector<vector<int>> _forest;
   unordered_map<int,pair<int,int>> _hashMap;
   set<int> _orderedNodes;
   int search(pair<int,int>& start, int goal);
   vector<vector<int>> _dir{{-1,0},{1,0},{0,1},{0,-1}};
   public:
   int cutOffTree(vector<vector<int>>& forest);
};


int Solution::cutOffTree(vector<vector<int>>& forest)
{
   _forest=forest;
   pair<int,int> start = make_pair(0,0);
   int res = 0;
   
   // populate the hashMap
   for(int i = 0;i < _forest.size(); ++i)
   {
       for(int j = 0;j < _forest[0].size(); ++j)
       {
           if(_forest[i][j])
           {
               _hashMap[_forest[i][j]] = make_pair(i,j);
               _orderedNodes.insert(_forest[i][j]);

           }

       }
   }
   
   for(auto & num:_orderedNodes)
   {
       int goal = num;
       //cout << "Searching for: " << goal << endl;
       //cout << "Searching from: " << start.first << "," << start.second << endl;
       
       int moves = search(start,goal);
       //cout << "Moves: " << moves << endl;
       if(moves == -1)
       {
           return -1;
       }
       
       res += moves;
       
       //set start
       start = _hashMap[goal];
       
       // make goal 1
       _forest[_hashMap[goal].first][_hashMap[goal].second] = 1;        
       
   }


   return res;

}

int Solution::search(pair<int,int>& start, int goal)
{
    unordered_set<int> visited; 
    pair<int,int> goalCoord = _hashMap[goal];
    Coord startCoord(start.first,start.second,0,0);
    priority_queue<Coord> bfs;
    bfs.push(startCoord);
    visited.insert(_forest[startCoord.x][startCoord.y]);
         
    while(!bfs.empty())
    {
        Coord front = bfs.top();
        bfs.pop();
        if(_forest[front.x][front.y] == goal)
            return front.moves;
        for(int i = 0; i < _dir.size(); ++i)
        {
            int x = front.x + _dir[i][0];
            int y = front.y + _dir[i][1];
            cout << "X:Y nei:" << x << "," << y << endl; 
           
            if(x >= 0 && x < _forest.size() && y >= 0 && y < _forest[0].size() && _forest[x][y] && visited.find(_forest[x][y]) == visited.end())
            {
                cout << "inserting" << endl;
                cout << "Forest:" << _forest[x][y]<< endl; 
                bfs.push(
                Coord (x,y,front.moves + 1,abs(x - goalCoord.first)+ abs(y - goalCoord.second)));
                visited.insert(_forest[x][y]);
            }
        }
        
    }
    
    return -1;    
    
}