import josx.platform.rcx.Motor;
import josx.platform.rcx.Sensor;
import josx.platform.rcx.TextLCD;

/**
 * LineFollower.java Program for RCX that simply follows a black line
 * on a white board.  Robot is best started in CENTER_ONLY, a state
 * where the center sensor is the only one on the black line. However,
 * it may be able to handle starting in the LEFT_CENTER or
 * RIGHT_CENTER positions. This is untested.
 *
 * <B>Improvements in Verstion 2.0</b>
 *
 * The only two things with any changes are the LEFT_CENTER and
 * RIGHT_CENTER clauses. Since these clauses are very similar to
 * eachother, only changes to one will be described. Together, these
 * changes allow the robot to handle turns of as little as twenty-five
 * degrees. Turns smaller than this are thought to be unrealistic.
 *
 * When the robot enters the LEFT_CENTER clause, the robot will first
 * go to sleep for some period of time. This is actually a very poor
 * way of handling the problem, since the sleep method is highly
 * dependent on battary power. We suggest an improvement to this part
 * of the problem. Please see the full web documentation for further
 * suggestions on how to solve this problem.
 *
 * After entering sleep, the robot turns up the power for the wheels
 * on the outside edge of the turn. This allows the robot to bring the
 * sensors back into the line in such a manner that it will trigger
 * just center only. With the motors at the same power, when turning
 * onto an acute angle turn, the state LEFT_CENTER will trigger again,
 * and the robot will do bad things. 
 *
 * At the start of the turn, the robot sleeps some more. We found this
 * to be helpful in keeping the robot off the pointy part of the line
 * on a turn. See the web documentation for more information on
 * this. Finally, the motor powers are returned to the original
 * values.
 *
 * @author CS371 2004
 * @author <a href="mailto:albaab@gettysburg.edu">Disha Al Baqui</a>
 * @author <a href="mailto:bassdo02@gettysburg.edu">Donnie Bassett</a>
 * @author <a href="mailto:ranamo01@gettysburg.edu">Monica Ranadive</a>
 * @author <a href="mailto:schmky01@gettysburg.edu">Kyle Schmidt</a>
 * @author CS371 2002
 * @version 2.0
 * @since 1.0
 */
public class LineFollower extends JavaRCX {
  
    /**
     * Runs forever taking appropriate motor actions for each state it
     * is in.  While loops within some statements attempt to fine tune
     * the robot so it is back on the line in the CENTER_ONLY state.
     *
     * @param args a String[] value, command line arguments, none in this case
     */
    public static void main(String[] args){
	initialize();
	while (true) {
	    if(state == NONE)
		lost();
	    else if(state == LEFT_ONLY){
		while(state != CENTER_ONLY){
		    Motor.C.backward();
		    Motor.A.backward();
		}
	    }
	    else if(state == CENTER_ONLY){
		Motor.A.forward();
		Motor.C.backward();
	    }
	    else if(state == RIGHT_ONLY){
		while(state != CENTER_ONLY){
		    Motor.A.forward();
		    Motor.C.forward();
		}
	    }
	    else if(state == LEFT_CENTER){
		// Turn Left
		
		/* sleep for a bit to make sure that the center sensor is 
		   off the line
		*/
		try{
		    Thread.sleep(125);
		}catch(InterruptedException e){}

		/* increase the power on the outside wheels to allow for
		   a sharper turn.
		*/
		Motor.C.setPower(POWER+1);
		
		Motor.A.backward();
		Motor.C.backward();
		
		/* sleep for a bit to make sure that the sensors are off of the
		   starting line 
		*/
		try{
		  Thread.sleep(150);
		}catch(InterruptedException e){}
		boolean done = false;
		
		// continue turning until CENTER_ONLY is activated
		while(!done){
		    if(state == CENTER_ONLY){
			done = true;
		    }
		}
	  
      		//reset the motors to POWER
		Motor.C.setPower(POWER);
		Motor.A.setPower(POWER);
		
	    }
      
	    else if(state == LEFT_RIGHT){
		//give it a little bump forward, this way it does not jitter back and forth between the lost method and this state which will call lost again which will see that all sensors are on which will call lost again ....etc.
		Motor.A.forward();
		Motor.C.backward();
		lost();
	    }
	    else if(state == CENTER_RIGHT){
		// Turn Right
		
		/* sleep for a bit to make sure that the center sensor is 
		   off the line
		*/
		try{
		    Thread.sleep(125);
		}catch(InterruptedException e){}
		
		/* increase the power on the outside wheels to allow for
		   a sharper turn.
		*/
		Motor.A.setPower(POWER+1);
			
		Motor.A.forward();
		Motor.C.forward();
		
		/* sleep for a bit to make sure that the sensors are off of the
		   starting line 
		*/

		try{
		    Thread.sleep(150);
		}catch(InterruptedException e){}
		boolean done1 = false;
		while(!done1){
		    if(state == CENTER_ONLY){
			done1 = true;
		    }
		}

		//reset the motors to POWER
		Motor.C.setPower(POWER);
		Motor.A.setPower(POWER);
	    }
	    else if(state == ALL){
		//give it a little bump forward, this way it does not jitter back and forth between the lost method and this state which will call lost again which will see that all sensors are on which will call lost again ....etc.
		Motor.A.forward();
		Motor.C.forward();
		lost();
	    }
	}
    }

    
}// LineFollower