import java.util.*;

public class MovementLogic
{
    public static final int ROUTINE_A = 0;
    public static final int ROUTINE_B = 1;
    public static final int ROUTINE_C = 2;
    public static final int ROUTINE_D = 3;

    public static final String MOVE_LEFT = "L";
    public static final String MOVE_RIGHT = "R";

    public MovementLogic (Map theMap, boolean debug)
    {
        _theMap = theMap;
        _path = new Stack<String>();
        _robotFacing = CellId.ROBOT_FACING_UP;
        _currentMoveDirection = "";
        _currentPosition = new Coordinate(_theMap.findStartingPoint());
        _debug = debug;
    }

    public void createMovementFunctions ()
    {
        createPath();

        String pathElement = _path.pop();

        System.out.println("Path:");

        while (pathElement != null)
        {
            System.out.println(pathElement);

            try
            {
                _path.pop();
            }
            catch (Exception ex)
            {
                pathElement = null;
            }
        }
    }

    public void createMovementRoutine ()
    {
        
    }

    /*
     * Create path using only L and R.
     */

    private boolean createPath ()
    {
        System.out.println("createPath from: "+_currentPosition);

        if (_currentPosition != null)
        {
            /*
             * Try L then R.
             * 
             * Robot always starts facing up. Change facing when we
             * start to move but remember initial facing so we can
             * refer movement directions as L or R.
             */

             // only change facing when we start to move off again

            if (!tryMoveLeft(_currentPosition))
                tryMoveRight(_currentPosition);

            return true;
        }
        else
        {
            System.out.println("Robot not found!");

            return false;
        }
    }

    /**
     * isStart == is this the first time we are heading in this direction? If
     * so then change facing.
     */

    private boolean tryMoveLeft (Coordinate curr)
    {
        // go as far left as we can from current location

        Coordinate coord = leftCoordinate(curr);

        System.out.println("tryMoveLeft: "+coord+" with facing: "+_robotFacing);

        if (_theMap.isScaffold(coord))
        {
            _currentMoveDirection = MOVE_LEFT;
            _path.push(_currentMoveDirection);

            return tryMoveLeft(coord);
        }
        else
        {
            System.out.println("Left change facing! "+curr);

            changeFacing(MOVE_LEFT);

            //return changeDirection(curr);

            return false;
        }
    }

    private boolean tryMoveRight (Coordinate curr)
    {
        // go as far right as we can from current location

        Coordinate coord = rightCoordinate(curr);

        System.out.println("tryMoveRight: "+coord+" with facing: "+_robotFacing);
        
        if (_theMap.isScaffold(coord))
        {
            _currentMoveDirection = MOVE_RIGHT;
            _path.push(_currentMoveDirection);

            return tryMoveRight(coord);
        }
        else
        {
            System.out.println("Right change facing! "+curr);
            
            changeFacing(MOVE_RIGHT);

            //return changeDirection(curr);

            return false;
        }
    }

    private void changeFacing (String moveDirection)
    {
        switch (_robotFacing)
        {
            case CellId.ROBOT_FACING_UP:
            {
                if (moveDirection.equals(MOVE_LEFT))
                    _robotFacing = CellId.ROBOT_FACING_LEFT;
                else
                    _robotFacing = CellId.ROBOT_FACING_RIGHT;
            }
            break;
            case CellId.ROBOT_FACING_DOWN:
            {
                if (moveDirection.equals(MOVE_LEFT))
                    _robotFacing = CellId.ROBOT_FACING_RIGHT;
                else
                    _robotFacing = CellId.ROBOT_FACING_LEFT;
            }
            break;
            case CellId.ROBOT_FACING_LEFT:
            {
                if (moveDirection.equals(MOVE_LEFT))
                    _robotFacing = CellId.ROBOT_FACING_DOWN;
                else
                    _robotFacing = CellId.ROBOT_FACING_UP;
            }
            break;
            case CellId.ROBOT_FACING_RIGHT:
            default:
            {
                if (moveDirection.equals(MOVE_LEFT))
                    _robotFacing = CellId.ROBOT_FACING_UP;
                else
                    _robotFacing = CellId.ROBOT_FACING_DOWN;
            }
            break;
        }
    }

    /*

    private final boolean changeDirection (Coordinate coord)
    {
        
    }*/

    private final Coordinate rightCoordinate (Coordinate coord)
    {
        int x = coord.getX();
        int y = coord.getY();

        switch (_robotFacing)
        {
            case CellId.ROBOT_FACING_DOWN:
            {
                x--;
            }
            break;
            case CellId.ROBOT_FACING_UP:
            {
                x++;
            }
            break;
            case CellId.ROBOT_FACING_LEFT:
            {
                y++;
            }
            break;
            case CellId.ROBOT_FACING_RIGHT:
            default:
            {
                y--;
            }
            break;
        }

        return new Coordinate(x, y);
    }

    private final Coordinate leftCoordinate (Coordinate coord)
    {
        int x = coord.getX();
        int y = coord.getY();

        System.out.println("leftCoordinate "+_robotFacing);

        switch (_robotFacing)
        {
            case CellId.ROBOT_FACING_DOWN:
            {
                x++;
            }
            break;
            case CellId.ROBOT_FACING_UP:
            {
                x--;
            }
            break;
            case CellId.ROBOT_FACING_LEFT:
            {
                y--;
            }
            break;
            case CellId.ROBOT_FACING_RIGHT:
            default:
            {
                y++;
            }
            break;
        }

        return new Coordinate(x, y);
    }

    private Map _theMap;
    private Stack<String> _path;
    private String _robotFacing;
    private String _currentMoveDirection;
    private Coordinate _currentPosition;
    private boolean _debug;
}
@@ -2490,32 +2490,93 @@
  else%0A        %7B%0A
+            System.out.println(%22Not scaffolding at %22+coord);%0A
             Syst
@@ -2667,34 +2667,32 @@
             
-//
 return c
 hangeDirecti
@@ -2683,55 +2683,19 @@
 rn c
-hangeDirection(curr);%0A%0A            return false
+reatePath()
 ;%0A  
@@ -3178,32 +3178,93 @@
  else%0A        %7B%0A
+            System.out.println(%22Not scaffolding at %22+coord);%0A
             Syst
@@ -3377,18 +3377,16 @@
     
-//
 return c
 hang
@@ -3385,55 +3385,19 @@
 rn c
-hangeDirection(curr);%0A%0A            return false
+reatePath()
 ;%0A  
@@ -4693,101 +4693,8 @@
  %7D%0A%0A
-    /*%0A%0A    private final boolean changeDirection (Coordinate coord)%0A    %7B%0A        %0A    %7D*/%0A%0A
     
@@ -4812,32 +4812,126 @@
  coord.getY();%0A%0A
+        System.out.println(%22rightCoordinate facing: %22+_robotFacing+%22 and position: %22+coord);%0A%0A
         switch (
@@ -5240,34 +5240,34 @@
                y
-++
+--
 ;%0A            %7D%0A
@@ -5373,34 +5373,34 @@
                y
---
+++
 ;%0A            %7D%0A
@@ -5645,16 +5645,24 @@
 ordinate
+ facing:
  %22+_robo
@@ -5660,32 +5660,56 @@
 : %22+_robotFacing
++%22 and position: %22+coord
 );%0A%0A        swit
@@ -6022,34 +6022,34 @@
                y
---
+++
 ;%0A            %7D%0A
@@ -6155,34 +6155,34 @@
                y
-++
+--
 ;%0A            %7D%0A

Update MovementLogic.java