/*************************
 *
 * FollowTheBlackAndWhiteRoad
 *
 *   or 
 *
 *     OK - now let's put all the pieces together.
 * 
 * This class implements a lego robot that follows the edge of 
 * a black/white line, as well as backs up and re-orienting 
 * when it bumps into things.
 * 
 * It's built on the backs of several other classes, namely
 *
 *   JimsRobot		for basic motor control and utiltity methods,
 *   LightSensor	to manage the light sensor,
 *   Bumper		to manage the touch sensors, and
 *   LegoMusic		to get us in the mood.
 *
 * (Those classes also have their own test routines, namely
 *  TestJimsRobot (also plays songs), TestLightSensor, and TestBumper.)
 *
 * The logic to calibrate and run the optical sensor,
 * as well as the overall robot behavior is defined here,
 * though I may move the optical sensor code elswhere later.
 *
 * Here's what you do:
 *
 *   (1) Push "RUN" to start it up, as usual.
 *   (2) Put the light sensor over the "white" color, and press "PRGM".
 *   (3) Now put it on the "black" color, and press "VIEW" to remember that.
 *   (4) Finally, place the robot where you'd like it to start doing it's
 *	 thing, and then press "RUN" again.
 *       It should then
 *          * Play a little song, and then start wandering forward.
 *          * While it sees white, it'll turn rightwards.
 *	    * While it sees black, it'll turn leftwards.
 *          * If a bumper is hit, it'll back up a bit 
 *            spin away from that side.
 * 
 * So there you go.
 *
 * @author Jim Mahoney
 * @version 1.0, Oct 14, 2002
 *
 ********************************************/
import josx.platform.rcx.*;

class FollowTheBlackAndWhiteRoad {

    public static void main(String[] args) {
	int loopCounter=0;
	final int CHANGE_WANDER  = 10;  // loops to change the wander-mode 
	final int SAME_COLOR = 200; // loop threshold for changing color

	// Set up our objects.
	Bumper leftBumper  = new Bumper(Sensor.S1, "in by default");
	Bumper rightBumper = new Bumper(Sensor.S2, "in by default");
	JimsRobot driverDan = new JimsRobot(); // default is (B,A)=(left,right)
	LightSensor eagleEye = new LightSensor(Sensor.S3);
	TextLCD.print("B.W.");
	driverDan.wait(0.5);

	while (true) {                             // --- outer loop --
	    driverDan.allStop();
	    eagleEye.displayValue();
	    driverDan.wait(0.2);
	    if      ( Button.VIEW.isPressed() ) {  // VIEW button?
		eagleEye.defineBlack();            //   This is black.
		Sound.beep();
	    }
	    else if ( Button.PRGM.isPressed() ) {  // PRGM button?
		eagleEye.defineWhite();            //   This is white.
		Sound.beep();
	    }
	    else if ( Button.RUN.isPressed() ) {   // RUN button? 
		TextLCD.print("Go!");
		driverDan.wait(0.2);
		loopCounter=0;
		LegoMusic.playSoundTheCharge();		// We're off!
		while (true) {				    // --- inner loop -
		    eagleEye.displayValue();
		    driverDan.wait(0.1);
		    if ( Button.RUN.isPressed() ) {         //  RUN again?
			driverDan.burp();		    //    Buzz, go to
			break;				    //    outer loop.
		    }
		    else if ( leftBumper.wasHit() ) {	    // Left bumper?
			Sound.beep();
			driverDan.moveBackward(0.5);	    //   Backup, 
			driverDan.spinRight(0.8);           //   Spin right.
			loopCounter=0;
		    }
		    else if ( rightBumper.wasHit() ) {	    // Right bumper?
			Sound.beep();
			driverDan.moveBackward(0.5);	    //   Backup, 
			driverDan.spinLeft(0.8);            //   Spin left.
			loopCounter=0;
		    }
		    else if  ( eagleEye.seesWhite() ) {     // White?
			//TextLCD.print("Wh");
			driverDan.turnRight();              //   Turn right.
			loopCounter++;
			if (loopCounter>SAME_COLOR) { 
			    eagleEye.raiseWhite(); 
			}
		    }
		    else if ( eagleEye.seesBlack() ) {      // Black?
			//TextLCD.print("Bl");
			driverDan.turnLeft();               //   Turn left.
			loopCounter--;
			if (loopCounter<-SAME_COLOR){ 
			    eagleEye.lowerBlack(); 
			}
		    }
		    else {				   //  Meander ahead.
			loopCounter++;
			if (loopCounter<CHANGE_WANDER){
			    // Eventually I'd 
			    // like to put something here
			    // to slow it down - remember 
			    // motor settings, stop for a bit,
			    // then resume.
			}
			else {
			    loopCounter=0;
			    double randomChoice = Math.random(); // 0 to 1
			    if ( randomChoice < 0.5 ) { 
				driverDan.moveForward();
			    }
			    else if ( randomChoice < 0.7 ) {
				driverDan.turnRight();
			    }
			    else if ( randomChoice < 0.9 ) {
				driverDan.turnLeft();
			    }
			    else {
				driverDan.waggleDance();
				loopCounter = CHANGE_WANDER;
			    }
			}
		    }
		}
	    }
	}
    }
}