private void buildMovementPlan() {
        // Find an unexplored location to move to.
        // Note that this method tries to find the closest position based on a
        // heuristic (turn cost and Manhattan distance).
        final Vector2 unexplored = findUnexploredPosition();

        // All unexplored locations exhausted, we are done exploring all
        // squares so simply return
        if (unexplored == null) {
            return;
        }

        // Use an A* search to find the best path from the current position to
        // the unexplored position.
        LinkedList<Vector2> path = findPath(unexplored);

        // First element in the path is the starting location, so we can remove
        // it
        if (!path.isEmpty()) {
            path.remove();
        }

        Vector2 current = iState.getAgentPosition();
        Vector2 next;
        int currentDirection = iState.getAgentDirection();
        int nextDirection;

        // Iterate through each location on the path, and build the correct
        // sequence of actions in the plan.
        while (!path.isEmpty()) {
            next = path.remove();

            // Get the correct direction between the two tiles.
            // Since current and next are adjacent, the direction number is
            // found by converting (next - current) vectors to an integer.
            nextDirection = Vector2.vectorToDirection(Vector2
                    .sub(next, current));

            if (nextDirection != currentDirection) { // We only need to turn
                                                     // when directions are
                                                     // different.

                int diff = nextDirection - currentDirection;
                if (diff > 3) { // Handle negative directions by making them
                                // positive instead.
                    diff = diff - 4;
                } else if (diff < 0) {
                    diff = diff + 4;
                }
                if (diff == 1) {
                    plan.add(new TurnRight());
                } else if (diff == 2) {
                    plan.add(new TurnRight());
                    plan.add(new TurnRight());
                } else if (diff == 3) { // Turn left to avoid having to turn
                                        // right three times
                    plan.add(new TurnLeft());
                }
            }

            // Finally, move forward.
            plan.add(new GoForward());

            // Go to the next position and direction in the path
            current = next;
            currentDirection = nextDirection;
        }
    }