import com.ridgesoft.io.Display;
import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.robotics.Motor;
import com.ridgesoft.robotics.PushButton;
import com.ridgesoft.robotics.AnalogInput;
import com.ridgesoft.robotics.AnalogShaftEncoder;

public class MyBot {
    public static void main(String args[]) {
        // Add your code here
		try{

			Display display = IntelliBrain.getLcdDisplay();
			PushButton startButton = IntelliBrain.getStartButton();
			PushButton stopButton = IntelliBrain.getStopButton();
			AnalogInput leftWheelInput = IntelliBrain.getAnalogInput(4);
			AnalogInput rightWheelInput = IntelliBrain.getAnalogInput(5);
			Motor leftMotor = IntelliBrain.getMotor(1);
			Motor rightMotor = IntelliBrain.getMotor(2);
			AnalogShaftEncoder leftEncoder = new AnalogShaftEncoder(leftWheelInput, 250, 750, 30, Thread.MAX_PRIORITY);
			AnalogShaftEncoder rightEncoder = new AnalogShaftEncoder(rightWheelInput, 250, 750, 30, Thread.MAX_PRIORITY);

			Runnable functions[] = new Runnable[] {
										new GoStraight(leftMotor, 
													  rightMotor, 
													  leftEncoder, 
													  rightEncoder,
													  5000),
										new GoWobbly(leftMotor, 
													  rightMotor, 
													  leftEncoder, 
													  rightEncoder,
													  5000),
										new GoCrazy(leftMotor, 
													  rightMotor, 
													  leftEncoder, 
													  rightEncoder,
													  5000),
										new MotorTest(leftMotor, 
													  rightMotor, 
													  leftEncoder, 
													  rightEncoder,
													  5000),
									};

				startButton.waitReleased();
				IntelliBrain.setTerminateOnStop(false);
				int selectedFunction = 0;
				display.print(0, "Function");
				display.print(1, functions[selectedFunction].toString());

				while(!startButton.isPressed()){
					if (stopButton.isPressed()) {
						if (++selectedFunction >= functions.length)
							selectedFunction = 0;
						display.print(1, functions[selectedFunction].toString());
						stopButton.waitReleased();
					}
				}

				IntelliBrain.setTerminateOnStop(true);

				functions[selectedFunction].run();

    	}
		catch (Throwable t){
			t.printStackTrace();
		}
	}
}