class Solution {
    func cutOffTree(_ forest: [[Int]]) -> Int {
        guard forest.count > 0 else { return -1 }
        let n = forest.count
        let m = forest.first!.count
        
        // first find trees to cut, and sort the tree       
        var trees = [[Int]]()
        
        for i in 0..<n {
            for j in 0..<m {
                if forest[i][j] > 1 {
                    trees.append([forest[i][j], i, j])
                }
            }
        }
        trees.sort { $0[0] < $1[0] }

        // use distance(souce, destination) to get the steps between each tree
        // add the steps/distance to the result
        var result = 0
        var prev = [forest[0][0],0, 0]
        for i in 0...trees.count-1 {
            let current = trees[i]
            
            let d = distance(prev[1], prev[2], current[1], current[2], forest)
            // print("\(prev) to \(current) distance:\(d)")
            prev = [current[0],current[1], current[2]]
            if d < 0 { return -1 }
            result += d
        }
        return result
    }
    
    func distance(_ crtRow: Int, _ crtCol: Int, _ destRow: Int, _ destCol: Int, _ forest: [[Int]]) -> Int {
        
        var d = 0
        if crtRow == destRow && crtCol == destCol { return d }
        let up = [-1, 0]
        let left = [0, 1]
        let down = [1, 0]
        let right = [0, -1]
        let directions = [up, left, down, right]
        
        var visited = forest
        var queue = [[crtRow, crtCol]]

        visited[crtRow][crtCol] = -1
        
        var dist = 0
        
        while !queue.isEmpty {
            let count = queue.count
            // print("current queue count:\(count)")
            for crtIndex in 0..<count {
                let node = queue.removeFirst()
                let i = node[0]
                let j = node[1]
                visited[i][j] = -1
                // print("visit [\(i),\(j)], crt distance: \(dist)")
                if i == destRow && j == destCol { 
                    return dist 
                }
                
                for direction in directions {
                    let r = i + direction[0]
                    let c = j + direction[1]

                    if (0..<visited.count).contains(r) &&
                    (0..<visited[0].count).contains(c) &&
                    visited[r][c] > 0 {
                    
                        visited[r][c] = -1
                        queue.append([r, c])
                    }
                    // print("add [\(r),\(c)], new queue size:\(queue.count)")
                }
            }
            // print("current turn finish")
            dist += 1
        }
        return -1
    }
}

class Solution {

    func cutOffTree(_ forest: [[Int]]) -> Int {
        var trees: [Tree] = []
        var forest = forest

        for (row, arr) in forest.enumerated() {
            for (col, val) in arr.enumerated() {
                if val >= 1 {
                    trees.append(Tree(val, Coord(row, col)))
                }
            }
        }

        trees.sort { $0.height < $1.height }
        var minSteps = bfs(forest, Coord(0,0),trees[0].coord)

        if trees.count == 1 || minSteps == -1 {return minSteps}
        for index in 1..<trees.count {
            let lastCoord = trees[index - 1].coord
            let minStepsBetweenTrees = bfs(forest, lastCoord, trees[index].coord)
            if minStepsBetweenTrees == -1 {
                return -1
            }
            minSteps += minStepsBetweenTrees
        }
        return minSteps
    }

    func bfs(_ grid: [[Int]], _ pos: Coord, _ target: Coord) -> Int {
        var grid = grid
        var queue: [Coord] = [pos]
        var minSteps = 0
        let colOffsets = [0,0,-1,1]
        let rowOffsets = [-1,1,0,0]

        while !queue.isEmpty {
            let queueCount = queue.count

            for _ in 0..<queueCount {
                let curr = queue.removeFirst()
                grid[curr.row][curr.col] = -1
                if curr.row == target.row && curr.col == target.col {
                    return minSteps
                }

                for num in 0..<4 {
                    let nextRow = curr.row + rowOffsets[num]
                    let nextCol = curr.col + colOffsets[num]

                    if (0..<grid.count).contains(nextRow) &&
                    (0..<grid[0].count).contains(nextCol) &&
                    grid[nextRow][nextCol] > 0
                    { 
                        grid[nextRow][nextCol] = -1
                        queue.append(Coord(nextRow, nextCol))
                    }
                }
            }
            minSteps += 1
        }
        return -1
    }
}

struct Tree {
    var height: Int
    var coord: Coord

    init(_ height: Int, _ coord: Coord) {
        self.height = height
        self.coord = coord
    }
}


struct Coord {
    var row: Int
    var col: Int

    init(_ row: Int, _ col: Int) {
        self.row = row
        self.col = col
    }
}