public int stateHeuristic() {
        if (Main.walkingDistance == null) {
            return 0;
        }


        int sum = 0;
        List<Position> boxesClone = new ArrayList<>();
        List<Position> goalsClone = new ArrayList<>();
        boxesClone.addAll(boxes);
        goalsClone.addAll(Main.goals);

        // Pair up a box to it's closest goal and sum the
        // walking distance
        for (Position box : boxesClone) {
            if (Main.goals.contains(box)) {
                continue; // We don't have to add any punctuation
            }
            int min = Integer.MAX_VALUE;
            Position g = null;
            for (Position goal : goalsClone) {
                if (boxesClone.contains(goal)) {
                    continue; //There is another box in this goal
                }

                int d = Main.walkingDistance.get(new PositionPair(box, goal));
                if (d < min) {
                    min = d;
                    g = goal;
                }
            }
            goalsClone.remove(g);
            sum += min;
        }

        for (Position box : boxes) {
            if (Main.isGoal(box)) {
                sum -= 25;
            }
        }

        return sum;
    }