//<pre>

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


/**
 * <pre><b>MultipleIntersectionChooser.java</b>
 * <b>Given:</b> multiple intersections to choose from and an ordering of spoke numbers to take at successive intersections.
 * The robot when determining it is at an intersection, will turn left a bit, then rotate clockwise starting(inclusive)at 12:00
 * counting spokes. When it hits the nth spoke it resumes intersection finder behavior.
 * Once done with iterating through array of spokes, the program terminates.
 * Can optionally change code to reset index to 0 if you hit the end of the array to allow continuous looping.
 * </pre>
 *
 * @author CS371 2002
 * @version 1.0
 * @since 1.0
 */
public class MultipleIntersectionChooser extends JavaRCX {
  public MultipleIntersectionChooser (){
  }
  
  /**
   * The order of spokes to take as robot comes to different intersections.
   * Change values in this array to change the ordering of spokes taken at successive intersections.
   */
  public static final int[] SPOKES = {1,2,3,4};
  
  /**
   * The index of <code>SPOKES</code> that keeps track of which spoke is next
   */    
  public static int index = 0;
  
  
  /**
   * <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 args a <code>String[]</code> value, command line arguments (none in our case)
   */
  public static void main(String[] args){
    initialize();
    while (true) {
      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 sensor 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{
	  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))
	  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)){
	  chooseSpoke();
	}
      }
      else if(state == CENTER_RIGHT){
	if(isIntersection(CENTER_RIGHT))
	  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))
	  chooseSpoke();
      }
    }
  }
  

  /**
   * <pre>Determines whether at an intersection or not.  
   * This method is thread safe because it is called from within main, so no call to <code>isIntersection()</code> will occur here.
   * 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, the state of the sensors that called this method.
   * @return a <code>boolean</code> value, whether or not the robot is at an intersection.
   */
  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;
  }
  
  
  
  /**
   * <pre>Given the index of <code>SPOKES</code> above and that the robot is at an intersection
   * it will rotate to <code>SPOKES[index]</code> by counting black lines it crosses.
   * This method is thread safe from main, so calls to <code>isIntersection()</code> will not 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.
    */
    Motor.A.backward();
    Motor.C.backward();
    try{
      Thread.sleep(125);
    }
    catch(InterruptedException e){
    }
    
    int spokes = 0;
    //assume robot is to left of north line on white
    while(spokes <  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
      */
      while(Sensor.S2.readValue() > DARK){
	Motor.A.forward();
	Motor.C.forward();
      }
      
      //rotate until hit white
      while(Sensor.S2.readValue() <= DARK){
	Motor.A.forward();
	Motor.C.forward();
      }	    
      //robot has now passed a line, increase spoke count
      spokes++;
      
      /*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){
      }
    }
    
    //increment so that next spoke of array will be chosen at next intersection
    index++;
    
    //if have gone through each spoke in array, terminate
    if(index == SPOKES.length)
      powerOff();
    
    
    //Have robot start moving forward onto new spoke
    Motor.A.forward();
    Motor.C.backward();
  }

}// MultipleIntersectionChooser

//</pre>