/***************
 * JimsRobot.java
 *
 * Some motion and sound routines for the lego robot 
 * that I've been working on.
 *
 * At the moment all these methods are all static (i.e. class) methods.
 * They may be invoked in either of two ways.
 *
 *   # 1. As methods of the whole class:
 *     JimsRobot.init();           # Set the constants and motor power levels.
 *     JimsRobot.waggleDance();    # Strut the stuff.
 *
 *   # 2. As methods of an object (i.e. instance) of this class:
 *     JimsRobot dorothy = new JimsRobot();   # Create a new robot object
 *     dorothy.waggleDance();                 # Let her strut her stuff.
 * 
 * In case # 2, the init() method is called by the constructor.
 *
 * This module assumes that the robot has two motors to drive
 * wheels or treads on the two sides.  Which motors are connected
 * to which of the A,B,C spots on the Lego brick and which
 * direction the Motor.forward() and .backward() methods move
 * the robot are all defined in the init() method below.
 *
 * Included is a utility method wait(seconds), designed as a replacement
 * for Thread.sleep(millisec).  The advantages are
 *   (1) the calling methods don't need "throws Exception" everywhere, and
 *   (2) the time to wait is in (int or double) seconds, and
 *   (3) it's less to type.  :)
 *
 * The movement methods may be called in three ways:
 *   (1) moveForward(0.01);   // where 0.01 = time to move in double seconds
 *   (2) moveFoward(1);       // where 1    = time to move in int seconds
 *   (3) moveForward();       // which turns the motors on and let's them run.
 * Giving a time equal to or less than zero (or no time at all)
 * starts the motion, leaving it up to the calling routine when
 * to stop the motors or do something different.
 * 
 * The public movement methods are
 *
 *   moveForward(seconds)
 *   moveBackward(seconds)
 *   turnLeft(seconds)
 *   turnRight(seconds)
 *   spinLeft(seconds)
 *   spinRight(seconds)
 *   allStop(seconds)
 *   waggleDance(seconds)
 *   runBothMotors(leftDirection, rightDirection, seconds)
 *
 * The other public methods are
 *
 *   init(...)
 *   wait(seconds)
 *   burp()    // make a noise
 *
 * @author  Jim Mahoney
 * @version 2.0, Oct 14 2002
 *
 ************/
import josx.platform.rcx.*;

public class JimsRobot {

    final public static int FRONT = 1;	
    final public static int BACK  = -1;
    final public static int NONE  = 0;	

    private static Motor leftMotor;
    private static Motor rightMotor;
    private static int leftFrontDirection;
    private static int rightFrontDirection;

    JimsRobot(){
	init();
    }

    JimsRobot(Motor theLeftMotor,  int leftForwardGoesFront,
	      Motor theRightMotor, int rightForwardGoesFront ){
	init(theLeftMotor,  leftForwardGoesFront,
	     theRightMotor, rightForwardGoesFront);
    }

    public static void init(){
	// The default for my robot:
	//    left motor  is B, and Motor.B.forward()  moves the robot ahead,
	//    right motor is A, and Motor.A.backward() moves the robot ahead.
	init(Motor.B, FRONT, Motor.A, BACK);
    }
    public static void init(Motor theLeftMotor,  int leftForwardGoesFront,
			    Motor theRightMotor, int rightForwardGoesFront ){
	leftMotor = theLeftMotor;
	leftFrontDirection = leftForwardGoesFront;
	leftMotor.setPower(7);
	rightMotor = theRightMotor;
	rightFrontDirection = rightForwardGoesFront;
	rightMotor.setPower(7);
    }

    public static void runBothMotors(int leftDirection, int rightDirection, 
				     double seconds){
	runOneMotor(leftMotor,  leftDirection);
	runOneMotor(rightMotor, rightDirection);
	if (seconds>0.0){
	    wait(seconds);
	    allStop();
	}
    }

    private static void runOneMotor(Motor m, int direction){
	// LeJOS doesn't have a switch statement, which'd be cleaner here.
	if      (direction==FRONT){
	    m.forward();
	}
	else if (direction==BACK){
	    m.backward();
	}
	else if (direction==NONE){
	    m.stop();
	}
	else {
	}
    }

    public static void moveForward(double seconds){
	runBothMotors( leftFrontDirection  * FRONT, 
		       rightFrontDirection * FRONT,  seconds );
    }
    public static void moveForward(int seconds){
	moveForward((double)seconds);
    }
    public static void moveForward(){
	moveForward(0.0);
    }

    public static void moveBackward(double seconds){
	runBothMotors( leftFrontDirection  * BACK, 
		       rightFrontDirection * BACK,  seconds );
    }
    public static void moveBackward(int seconds){
	moveBackward((double)seconds);
    }
    public static void moveBackward(){
	moveBackward(0.0);
    }

    public static void turnRight(double seconds){
	runBothMotors( leftFrontDirection  * FRONT, NONE, seconds );
    }
    public static void turnRight(int seconds){
	turnRight((double)seconds);
    }
    public static void turnRight(){
	turnRight(0.0);
    }
    public static void turnLeft(double seconds){
	runBothMotors( NONE, rightFrontDirection  * FRONT, seconds );
    }
    public static void turnLeft(int seconds){
	turnLeft((double)seconds);
    }
    public static void turnLeft(){
	turnLeft(0.0);
    }

    public static void spinRight(double seconds){
	runBothMotors( rightFrontDirection * BACK,
		       leftFrontDirection  * FRONT,  seconds );
    }
    public static void spinRight(int seconds){
	spinRight((double)seconds);
    }
    public static void spinRight(){
	spinRight(0.0);
    }

    public static void spinLeft(double seconds){
	runBothMotors( rightFrontDirection * FRONT,
		       leftFrontDirection  * BACK,  seconds );
    }
    public static void spinLeft(int seconds){
	spinLeft((double)seconds);
    }
    public static void spinLeft(){
	spinLeft(0.0);
    }

    public static void allStop(){
	leftMotor.stop();
	rightMotor.stop();
    }

    public static void burp(){
	Sound.buzz();
	wait(1);
    }

    public static void waggleDance() {
	spinLeft(0.1);
	spinRight(0.25);
	spinLeft(0.25);
	spinRight(0.1);
	allStop();
    }

    public static void wait(double seconds) {
	try {
	    Thread.sleep( (long)(1000*seconds) );
	}
	catch (InterruptedException e1){
	}
    }
    public static void wait(int seconds){
	wait((double)seconds);
    }

}