//<pre>

import josx.platform.rcx.Motor;
import josx.platform.rcx.Sensor;
import josx.platform.rcx.Sound;
//import josx.platform.rcx.TextLCD;
//import josx.platform.rcx.LCD;

/**
 * <pre><b>RCXIntersectionChooser.java</b>
 * Given a spoke number, the robot when determining it is at an intersection
 * will turn left, rotate clockwise starting(inclusive)at 12:00 counting spokes and when it 
 * hits the nth spoke resume execution of intersection finder.</pre>
 * @author <a href="mailto:albaal01@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>
 * @version 2.0
 * @since 1.0
 */
public class RCXIntersectionChooser extends JavaRCX{

    public RCXIntersectionChooser (){
    }
  
    /**
     * <pre>Which spoke to take when getting to an intersection.
     * Change only this variable to take different spokes.</pre>
     */
    public static int SPOKE;
    public static int [] spokes;
    public static int index = 0; // start index
  
    /**
     * <pre>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
     * <code>CENTER_ONLY</code> state.</pre>
     * @param directions an <code>int[]</code> value - takes in an
     * array of spokes to take at each intersection so that it can
     * reach the goal node
     */
    public static void move(int [] directions){
	spokes = directions;
	while (index < spokes.length) {
	    if(state == NONE)
		lost();
	    else if(state == LEFT_ONLY){
		/*We found that without sleeping for a bit here the robot
		  will violently go back and forth between two sensors.
		  It will get where it needs to go but its very unelegant.
		  So here we are programming smoothness into the travel
		  path.  If way off such as Left sens	}
		  index++;or only. Have it move
		  until the center is on black, then sleep for approx <
		  1/8 of a second which will cause the robot to continue
		  coasting in its current direction. When it wakes up the
		  state should be close to the center sensor being exactly
		  in the center of the black line rather than on the side.
		  Note: You can tweak this value as necessary,(<100ms) be
		  sure to update the RIGHT_ONLY as well.  The same goes
		  for LEFT_CENTER and CENTER_RIGHT, although this state is
		  closer to the actual center of the line so sleep time
		  should be a lot less. (<30ms)
		*/
		while(Sensor.S2.readValue() > DARK){
		    Motor.C.backward();
		    Motor.A.backward();
		}
		try{	
		    //index++;
		    Thread.sleep(100);
		}
		catch(InterruptedException e){
		}
	    }
	    else if(state == CENTER_ONLY){
		Motor.A.forward();
		Motor.C.backward();
	    }
	    else if(state == RIGHT_ONLY){
		while(Sensor.S2.readValue() > DARK){
		    Motor.A.forward();
		    Motor.C.forward();
		}
		try{
		    Thread.sleep(100);
		}
		catch(InterruptedException e){
		}
	    }	
	    else if(state == LEFT_CENTER){
		if(isIntersection(LEFT_CENTER)){
		    try {
			Thread.sleep(30);
		    }
		    catch(InterruptedException e){
		    }
		    chooseSpoke();
		}
		while(Sensor.S2.readValue() > DARK){
		    Motor.A.backward();
		    Motor.C.backward();
		}
		try{
		    Thread.sleep(30);
		}
		catch(InterruptedException e){
		}
	    }
	    else if(state == LEFT_RIGHT){
		if(isIntersection(LEFT_RIGHT)){
		    try {
			Thread.sleep(30);
		    }
		    catch(InterruptedException e){
		    }
		    chooseSpoke();
		}
	    }
	    else if(state == CENTER_RIGHT){
		if(isIntersection(CENTER_RIGHT)){
		    try {
			Thread.sleep(30);
		    }
		    catch(InterruptedException e){
		    }
		    chooseSpoke();
		}
		while(Sensor.S2.readValue() > DARK){
		    Motor.A.forward();
		    Motor.C.forward();
		}
		try{
		    Thread.sleep(30);
		}
		catch(InterruptedException e){
		}
	    }
	    else if(state == ALL){
		if(isIntersection(ALL)){
		    try{
			Thread.sleep(30);
		    }
		    catch (InterruptedException e){
		    }
		    chooseSpoke();
		}
	    }
	}	index++;
	//TextLCD.print("FINISH");
	Motor.A.stop();
	Motor.C.stop();
	Sound.systemSound(true,2);
    }

    /**
     * <pre>Determines whether at an intersection or not.  This method
     * is thread safe because it is called from within main.  The
     * watcher thread has no effect on anything here, it only goes
     * back into effect when execution returns to the main.</pre>
     * @param triggerState an <code>int</code> value
     * @return a <code>boolean</code> value
     */
    public static boolean isIntersection(int triggerState){
	/*Move robot forward for 1/8 second which will allow you to
	  judge whether it is an intersection based on the triggering
	  state which called isIntersection and the state the robot is
	  in after the short roll forward.
	*/
	Motor.A.forward();
	Motor.C.backward();
	try{
	    Thread.sleep(125);
	}
	catch(InterruptedException e){
	}
	finally{
	    Motor.A.stop();
	    Motor.C.stop();
	}
	/*Read values after the roll foward*/
	if(Sensor.S1.readValue() > DARK && Sensor.S2.readValue() > 
	   DARK && Sensor.S3.readValue() > DARK){
	    /*Check to make sure the CENTER_RIGHT is not a corner, but
	      an intersection such as a T or Y.  Therefore it does a
	      scan left, then returns to its original location.  We
	      had chose to sleep the Thread 1/4 of a second.
	    */
	    if(triggerState == CENTER_RIGHT){
		Motor.A.backward();
		Motor.C.backward();
		try{
		    Thread.sleep(250);
		}
		catch(InterruptedException e){
		}
		Motor.A.stop();
		Motor.C.stop();
		/*
		  If when the thread wakes up and a sensor is on a
		  black line, we know we hit an intersection of some
		  kind.  Possible problem here with given sensor
		  configuration although we have never seen it happen
		  is that the line could be resting in between two
		  sensors. Highly unlikely but something for next
		  group to think about.  Maybe Scan possibly one more
		  short distance to the left which would move sensors
		  to where the gaps between them were previously.
		  This would be a very short sleep (<100millis)
		*/
		if((Sensor.S1.readValue() <= DARK) || (Sensor.S2.readValue() <= DARK) || 
		   (Sensor.S3.readValue() <= DARK)){
		    return true;
		}
		else{
		    //it was a corner	
		    Motor.A.forward();
		    Motor.C.forward();
		    try{
			Thread.sleep(250);
		    }
		    catch(InterruptedException e){
		    }
		    Motor.A.stop();
		    Motor.C.stop();
		    return false;
		}
	    }
	    /*Check to make sure the LEFT_CENTER is not a corner, but
	      an intersection such as a T or Y.  Therefore it does a
	      scan right, then returns to its original location.  We
	      had chose to sleep the thread 1/4 of a second
	    */
	    if(triggerState == LEFT_CENTER){
		Motor.A.forward();
		Motor.C.forward();
		try{
		    Thread.sleep(250);
	  
		}
		catch(InterruptedException e){
		}
		Motor.A.stop();
		Motor.C.stop();
	
		/*
		  If when the thread wakes up and a sensor is on a
		  black line, we know we hit an intersection of some
		  kind.  Possible problem here with given sensor
		  configuration although we have never seen it happen
		  is that the line could be resting in between two
		  sensors. Highly unlikely but something for next
		  group to think about.  Maybe Scan possibly one more
		  short distance to the right which would move sensors
		  to where the gaps between them were previously.
		  This would be a very short sleep (<100millis)
		*/
		if((Sensor.S1.readValue() <= DARK) || (Sensor.S2.readValue() <= DARK) 
		   ||(Sensor.S3.readValue()<= DARK)){
		    return true;
		}
		else{
		    //it was a corner
		    Motor.A.backward();
		    Motor.C.backward();
		    try{
			Thread.sleep(250);
		    }
		    catch(InterruptedException e){
		    }
		    Motor.A.stop();
		    Motor.C.stop();
		    return false;
		}
	    }
	}
	//we know it is an itersection
	return true;
    }
    
    /**
     * <code>rotateBackToParent</code> - in order to count the number
     * of spokes on each intersection properly, the robot turns around
     * 180 degrees facing the direction from which it came
     *
     */
    private static void rotateBackToParent()
    {
	//TextLCD.print("Parent");
	Motor.C.backward();
	Motor.A.backward();
	try{
	    Thread.sleep(1600); // because it takes about 3 seconds to
				// rotate 360 degrees at full battery
				// power
	}
	catch(InterruptedException e){
	}
	while(state != CENTER_ONLY){
	    Motor.C.backward();
	    Motor.A.backward();
	}
 	Motor.C.stop();
	Motor.A.stop();
	//TextLCD.print("Leave");
	//LCD.clear();
    }
    
    /**
     * <pre>Given the value of <code>SPOKE</code> above and that the
     * robot is at an intersection it will rotate to
     * <code>SPOKE</code> and it thread safe from main, so no new
     * calls to <code>isIntersection()</code> will be fired
     * here.</pre>
     */
    public static void chooseSpoke(){
	/*Move robot slightly to left 1/8 sec You may need to tweak
	  this try between 1/8 and 1/4 second, Sometimes robot comes
	  in at weird angle and the turn left wont get it on white to
	  begin with.  It would still be on the north black line and
	  wind up going an extra spoke. So be sure the twitch gets it
	  to the left of the line north of its front.
	*/
	if(index > 0 && index < spokes.length - 1){
	    rotateBackToParent();	    
	}
	if(spokes[index] > 0){
	    for(int i = 0; i < spokes[index] ; i++){
		Sound.beep();
		try{
		    Thread.sleep(500);
		}
		catch(InterruptedException e){
		}
	    }
	}
	int spokeCount = 0;
	
	//assume robot is to left of north line on white
	while(spokeCount <  spokes[index]){
	    //LCD.showNumber(spokes);//debug 
	    /*by having it look for black first we increase the chance
	      that it will count the first line facing north because
	      if it is white when it did the little twitch left then
	      we move until hit black otherwise it is black and we
	      skip the loop.  Rotate until hit black
	    */

	    //rotate until hit white
	    while(Sensor.S2.readValue() > DARK){
		Motor.A.forward();
		Motor.C.forward();
	    }

	    //robot has now passed a line
	    while(Sensor.S2.readValue() <= DARK){
		Motor.A.forward();
		Motor.C.forward();
	    }

	    //increase spoke count
	    spokeCount++;
      
	    /*have robot pause 1.5 seconds this way we are positive
	     *light sensors are not rushed in reading.  we found if
	     *you continously scan non-stop you get weird counts,
	     *better to be slower and accurate than fast and
	     *inaccurate.
	     */
	    try{
		Motor.A.stop();
		Motor.C.stop();
		Thread.sleep(1500);
	    }
	    catch(InterruptedException e){
	    }
	}
	index++;
	//Have robot start moving forward onto new spoke
	Motor.A.forward();
	Motor.C.backward();
    }
  
}// RCXIntersectionChooser

//</pre>