Cut Off Trees for Golf Event

You are asked to cut off trees in a forest for a golf event. The forest is represented as a non-negative 2D map, in this map:

1. 0 represents the obstacle can't be reached.

2. 1 represents the ground can be walked through.

3. The place with number bigger than 1 represents a tree can be walked through, and this positive number represents the tree's height.

In one step you can walk in any of the four directions top, bottom, left and right also when standing in a point which is a tree you can decide whether or not to cut off the tree.

You are asked to cut off all the trees in this forest in the order of tree's height - always cut off the tree with lowest height first. And after cutting, the original place has the tree will become a grass (value 1).

You will start from the point (0, 0) and you should output the minimum steps you need to walk to cut off all the trees. If you can't cut off all the trees, output -1 in that situation.

You are guaranteed that no two trees have the same height and there is at least one tree needs to be cut off.

Example 1:

Input: 
[
 [1,2,3],
 [0,0,4],
 [7,6,5]
]
Output: 6

Example 2:

Input: 
[
 [1,2,3],
 [0,0,0],
 [7,6,5]
]
Output: -1

Example 3:

Input: 
[
 [2,3,4],
 [0,0,5],
 [8,7,6]
]
Output: 6
Explanation: You started from the point (0,0) and you can cut off the tree in (0,0) directly without walking.

Constraints:

• 1 <= forest.length <= 50

• 1 <= forest[i].length <= 50

• 0 <= forest[i][j] <= 10^9

class Solution {
    static class Pair {
        int h, x, y;

        Pair(int h, int x, int y) {
            this.h = h;
            this.x = x;
            this.y = y;
        }
    }

    // O((m*n)*(m*n))
    public int cutOffTree(List<List<Integer>> forest) {
        List<Pair> trees = new ArrayList<>();
        int m = forest.size(), n = forest.get(0).size();
        for (int i = 0; i < m; i++)
            for (int j = 0; j < n; j++)
                if (forest.get(i).get(j) > 1)
                    trees.add(new Pair(forest.get(i).get(j), i, j));
        // Sorting on height (because we want to cut in order)
        Collections.sort(trees, (a, b) -> a.h - b.h);
        int x = 0, y = 0;
        int ans = 0;
        // Just do BFS fron start point to the next tree in order
        for (int i = 0; i < trees.size(); i++) {
            int steps = BFS(forest, x, y, trees.get(i).x, trees.get(i).y);
            if (steps == -1)
                return -1;
            ans += steps;
            x = trees.get(i).x;
            y = trees.get(i).y;
        }
        return ans;
    }

    public int BFS(List<List<Integer>> forest, int x, int y, int targetX, int targetY) {
        int m = forest.size(), n = forest.get(0).size();
        int[][] dirs = { { 1, 0 }, { 0, 1 }, { 0, -1 }, { -1, 0 } };
        boolean[][] visited = new boolean[m][n];
        Queue<Pair> q = new LinkedList<>();
        q.add(new Pair(forest.get(x).get(y), x, y));
        visited[x][y] = true;
        int steps = 0;
        while (q.size() != 0) {
            int size = q.size();
            while (size-- > 0) {
                Pair pos = q.poll();
                if (pos.x == targetX && pos.y == targetY)
                    return steps;
                if (pos.h == 0)
                    continue;
                for (int[] dir : dirs) {
                    int newX = pos.x + dir[0], newY = pos.y + dir[1];
                    if (newX >= 0 && newX < m && newY >= 0 && newY < n && forest.get(newX).get(newY) != 0
                            && !visited[newX][newY]) {
                        visited[newX][newY] = true;
                        q.add(new Pair(forest.get(newX).get(newY), newX, newY));
                    }
                }
            }
            steps++;
        }
        return -1;
    }
}