/***
 * LineWalker
 * is a simple LeJOS robot that follows the edge of a dark/light line.
 *
 * The robot should contain 2 motors (on ports A,B)
 * and one light sensor (on port S3).
 *
 * Compile and download the code as usual, i.e.
 *   1) turn on the lego brick and aim it at the IR tower, then
 *   2) at the command line type
 *      $ lejosc LineWalker.java
 *      $ lejosfirmdl
 *      $ lejos LineWalker
 * 
 * To see it do its thing, first find a flat surface with a sharp color line
 * it can follow, such as the edge of a strip of duct tape.  Then
 *   1) Push "Run" to start the program.  The text should display "W B G",
 *      which means "push the white, black, or go (green) button".
 *   2) Put the light sensor over the light color, and push the 
 *      white Prgm button.  You should hear two beeps.
 *   3) Then put the sensor over the dark color, and push the 
 *      the black View button.  Again you'll get two beeps.
 *      At this point the sensor is calibrated.
 *   4) Finally, position the robot where you want it to start,
 *      and push the Run button again.  It should then do a "waggle dance"
 *      along the dark/light border.  To stop the walk, push Run again.
 *
 * This code is adaptation of TinyEdgeWalker from the
 * Fall 2002 Lego Robots course at Marlboro College,
 * and may be used as open source under the GPLv2.
 *
 * @version 1.0 Feb 16 2006
 * @author Jim Mahoney
 *
 ****/

import josx.platform.rcx.*;

class LineWalker {

    static int whiteValue;
    static int blackValue;

    public static void main(String[] args) {
        initialize();
        while (true){
            display("W B G");
            if (Button.PRGM.isPressed()){
                whiteValue = getSensorValue("White");
            }
            else if (Button.VIEW.isPressed()){
                blackValue = getSensorValue("Black");
            }
            else if (Button.RUN.isPressed()){
                walkTheLine();
            }
	}
    }

    static void walkTheLine(){
	// Depending on how the motors are wired in your robot,
	// you'll need to swap around (A/B).(forward/back) here
	// to get this to work properly.
        display("walk");
        while (true){
            if ( getSensorDiff(whiteValue) < getSensorDiff(blackValue) ){
		// Turn left.
                Motor.A.backward();
                Motor.B.stop();
            }
            else {
		// Turn right.
                Motor.A.stop();
                Motor.B.forward();
            }
            wait(0.05);  // Slow the loop to avoid jiggling too fast.
	    if (Button.RUN.isPressed()){
		Motor.A.stop();
		Motor.B.stop();
		Sound.beep();
		wait(1.0);
		Sound.beep();
		return;
	    }
        }
    }

    static int getSensorValue(String message){
        display(message);
        Sound.beep();
        wait(1.0); // Pause for 1 sec between beeps to show the message.
        Sound.beep();
        return Sensor.S3.readValue();
    }

    static int getSensorDiff(int referenceValue){
        return Math.abs(Sensor.S3.readValue() - referenceValue);
    }

    static void initialize(){
        Sensor.S3.setTypeAndMode(SensorConstants.SENSOR_TYPE_LIGHT,
                                 SensorConstants.SENSOR_MODE_PCT);
        Sensor.S3.activate();
        Motor.A.setPower(7);
        Motor.B.setPower(7);
    }

    static void display(String message){
	TextLCD.print(message);
	wait(0.1); // Give the brick a moment to finish this.
    }

    static void wait(double seconds) {
        int milliseconds = (int)(1000.0*seconds);
        try {
	    Thread.sleep(milliseconds);	
        }
        // Thread.sleep may "throw an exception",
        // which must be "caught", or Java gets unhappy.
        catch (InterruptedException e){
        }
    }

}