private void cacheHeuristic(){
        //Calculate manhattan distance between boxes and the closes goal
        for(Position box : boxes){
            int min = 10000;
            for(Position goal : goals){
                int distance = Math.abs(goal.x-box.x) + Math.abs(goal.y-box.y);
                if (distance < min){
                    min = distance;
                }
            }
            //If the box is on the goal the give a bonus to prefer states with many boxes on goals
            if (min == 0)
            {
                heuristic += 10;
            }
            else
                heuristic -= min;
        }
    }