/********************
 * RunSensorRobot.java
 *
 * This robot has a few sensors.
 * 
 * Lego brick connections:
 *  1: left bumper  (touch sensor default is in)
 *  2: right bumper (ditto)
 *  3: optical sensor
 *  A: right motor
 *  B: left motor
 * 
 * @author Jim Mahoney
 * @version 1.0, Oct 11, 2002
 *
 **/

import josx.platform.rcx.*;


class RunSensorRobot {
 
    private static boolean leftIsAlreadyPressed;
    private static boolean rightIsAlreadyPressed;

    public static void main(String[] args) throws Exception {
	initSensorsAndMotors();
	while (true) {
	    boolean rightPressed = rightBumper();
	    boolean leftPressed  = leftBumper();

	   if ( rightPressed ) {
		if (! rightIsAlreadyPressed ) {
		    beepNTimes(3);
		}
		rightIsAlreadyPressed = true;
	    }
	   else {
	       rightIsAlreadyPressed = false;
	   }

	  if ( leftPressed ) {
		if (! leftIsAlreadyPressed) {
		    beepNTimes(2);
		}
		leftIsAlreadyPressed = true;
	    }
	    else {
		leftIsAlreadyPressed = false;
	    }

	    // displayOpticalSensor();

	}
    }

    public static void initSensorsAndMotors(){
	leftIsAlreadyPressed = false;
	rightIsAlreadyPressed = false;
	Motor.A.setPower(7);
	Motor.B.setPower(7);
	Motor.C.setPower(7);
	Sensor.S1.setTypeAndMode(SensorConstants.SENSOR_TYPE_TOUCH,
				 SensorConstants.SENSOR_MODE_BOOL);
	Sensor.S2.setTypeAndMode(SensorConstants.SENSOR_TYPE_TOUCH,
				 SensorConstants.SENSOR_MODE_BOOL);
	Sensor.S3.setTypeAndMode(SensorConstants.SENSOR_TYPE_LIGHT,
				 SensorConstants.SENSOR_MODE_RAW);
	Sensor.S1.activate();
	Sensor.S2.activate();
	Sensor.S3.activate();
	TextLCD.print("Hi!");
    }

    public static void displayOpticalSensor() throws Exception {
	//int whatDoISee = Sensor.S3.readRawValue();
	//TextLCD.print(" "+whatDoISee);
	//Thread.sleep(200);
    }

    public static void beepNTimes(int n) throws Exception {
	while (n>0) {
	    Sound.beep();
	    Thread.sleep(200);
	    n--;
	}
    }

    public static boolean rightBumper(){
	return ! Sensor.S2.readBooleanValue();
    }

    public static boolean leftBumper(){
	return ! Sensor.S1.readBooleanValue();
    }
    public static void startMovingForward(){
	Motor.A.backward();
	Motor.B.forward();
    }

    public static void startTurningLeft(){
	Motor.A.backward();
    }

    public static void startTurningRight(){
	Motor.B.forward();
    }

    public static void startMovingBackward(){
    }

    public static void allEnginesStop(){
	Motor.A.stop();
	Motor.B.stop();
    }

    //  public static wait(double secs){
    //  long startTime = System.currentTimeMillis();
    //  long timeToWait = (long)(secs*1000);
    //     if ( System.currentTimeMillis
    // }

}