Cut off Trees for Golf Event

use A* Algorithm

Maintain two lists, search list1 first and than search list2

node in list1 is 2 steps less than node in list2

class Solution(object):
    def cutOffTree(self, forest):
        """
        :type forest: List[List[int]]
        :rtype: int
        """
        m = len(forest)
        n = len(forest[0])
        forest.append([0] * n)
        for row in forest:
            row.append(0)

        self.order = collections.OrderedDict()
        res = 0
        self.order[0] = (0,0)

        for i in range(m):
            for j in range(n):
                if forest[i][j] > 1:
                    self.order[forest[i][j]] = (i, j)

        def find_small_path(p1, p2):
            theMin = abs(p1[0] - p2[0]) + abs(p1[1] - p2[1])
            stack1, stack2 = [p1], []
            visited = set()

            while stack1 or stack2:
                if not stack1:
                    stack1, stack2 = stack2, stack1
                    theMin += 2

                p = stack1.pop()
                if p == p2:
                    return theMin

                if p in visited:
                    continue
                visited.add(p)

                i = p[0]
                j = p[1]

                tmp1, tmp2 = [], []

                if i < p2[0]:
                    tmp1.append((i+1, j))
                    tmp2.append((i-1, j))
                elif i > p2[0]:
                    tmp1.append((i-1, j))
                    tmp2.append((i+1, j))
                else:
                    tmp2.append((i-1, j))
                    tmp2.append((i+1, j))

                if j < p2[1]:
                    tmp1.append((i, j+1))
                    tmp2.append((i, j-1))
                elif j > p2[1]:
                    tmp1.append((i, j-1))
                    tmp2.append((i, j+1))
                else:
                    tmp2.append((i, j-1))
                    tmp2.append((i, j+1))

                for v in tmp1:
                    if forest[v[0]][v[1]] != 0:
                        stack1.append(v)

                for v in tmp2:
                    if forest[v[0]][v[1]] != 0:
                        stack2.append(v)

            return -1



        self.order = sorted(self.order.items())

        for i in range(len(self.order)-1):
            path = find_small_path(self.order[i][1], self.order[i+1][1])
            if path == -1:
                return -1
            res += path

        return res