class TreePos
{
    private:
        int treeHeight;
        int xcord;
        int ycord;

    public:
    TreePos(int treeHeight, int x, int y)
    {
        this->treeHeight = treeHeight;
        this->xcord = x;
        this->ycord = y;
    }

    int TreeHeight()
    {
        return this->treeHeight;
    }

    int XCord()
    {
        return this->xcord;
    }

    int YCord()
    {
        return this->ycord;
    }
};

class HeapRepr
{
  public:
    int distanceCovered;
    int estimatedDistanceToGoal;
    pair<int,int> currentPosition;
    HeapRepr(int distanceCovered, int estimatedDistanceToGoal,pair<int,int> & currentPosition )
    {
        this->distanceCovered = distanceCovered;
        this->estimatedDistanceToGoal = estimatedDistanceToGoal;
        this->currentPosition = currentPosition;
    }

    HeapRepr(const HeapRepr &other)
    {
        this->distanceCovered = other.distanceCovered;
        this->estimatedDistanceToGoal = other.estimatedDistanceToGoal;
        this->currentPosition = other.currentPosition;
    }
};

bool operator > (const HeapRepr a, const HeapRepr b)
{
    return a.distanceCovered + a.estimatedDistanceToGoal > b.distanceCovered + b.estimatedDistanceToGoal;
}

class Solution {
public:
    int columns;

    static bool TreePosCompare(TreePos &a, TreePos &b)
    {
      return a.TreeHeight() < b.TreeHeight();
    }

    int CalculateEstimatedDistance(pair<int, int> &src, pair<int, int> &dst)
    {
        return abs(src.first - dst.first) + abs(src.second - dst.second);
    }

    bool isPassable(vector<vector<int>> & forest, int i, int j)
    {
        if(i<0 || j<0 || i>= forest.size() || j>=forest[0].size())
        {
            return false;
        }
        if(forest[i][j] == 0)
        {
            return false;
        }
        return true;
    }

    int AStar(
        vector<vector<int>> forest,
        pair<int, int> &src,
        pair<int, int> &dst)
    {
        priority_queue<HeapRepr, vector<HeapRepr>,greater<vector<HeapRepr>::value_type>> heap;
        unordered_set<int> visited;

        heap.push(HeapRepr(0, CalculateEstimatedDistance(src, dst), src));
        vector<pair<int, int>> directions =
        {
            make_pair(0,-1),
            make_pair(-1,0),
            make_pair(1,0),
            make_pair(0,1),
        };

        while(!heap.empty())
        {
            auto current = heap.top();
            heap.pop();

            // IF we comment out this section and uncomment "Section 1" then the code fails
            int currentOneD = OneDProject(current.currentPosition.first, current.currentPosition.second);
            if (visited.count(currentOneD)) continue;
            visited.insert(currentOneD);

            if(current.currentPosition.first == dst.first && current.currentPosition.second == dst.second)
            {
                return current.distanceCovered;
            }

            for(int i =0;i<4;i++)
            {
                HeapRepr next = HeapRepr(current);
                next.currentPosition.first += directions[i].first;
                next.currentPosition.second += directions[i].second;
                int nextOneD = OneDProject(next.currentPosition.first, next.currentPosition.second);

                if(visited.count(nextOneD) == 0 &&
                  isPassable(
                      forest,
                      next.currentPosition.first,
                      next.currentPosition.second))
                {
                    // Section 1
                    // visited.insert(nextOneD);
                    next.distanceCovered += 1;
                    next.estimatedDistanceToGoal = CalculateEstimatedDistance(next.currentPosition, dst);
                    heap.push(next);
                }
            }

        }
        return -1;
    }

    int OneDProject(int i, int j)
    {
        return i*columns + j;
    }

    pair<int, int> TwoDProject(int val)
    {
        int quotient = val/columns;
        return make_pair(quotient, val - (quotient*columns) );
    }

    int cutOffTree(vector<vector<int>>& forest) {
        columns = forest[0].size();
        vector<TreePos> trees;

        // Setup
        for(int i=0;i<forest.size();i++)
        {
            for (int j=0;j<forest[0].size();j++)
            {
                if (forest[i][j] > 1)
                {
                    trees.push_back(TreePos(forest[i][j], i, j));
                }
            }
        }

        sort(trees.begin(), trees.end(), TreePosCompare);

        auto src = make_pair(0,0);
        int steps = 0;
        for (auto it=trees.begin(); it!=trees.end(); it++)
        {
            auto dst = make_pair(it->XCord(), it->YCord());
            int distanceTraveled = AStar(forest, src, dst);

            if (distanceTraveled<0)
            {
                return -1;
            }

            swap(src, dst);
            steps+=distanceTraveled;
        }

        return steps;

    }
};