import lejos.hardware.Button;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.EV3LargeRegulatedMotor;
import lejos.hardware.motor.EV3MediumRegulatedMotor;
import lejos.hardware.port.MotorPort;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3GyroSensor;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;

public class EV3StairClimber {

	private static final int DRIVE_STRAIGHT_ONCE_ON_STAIR = 800;
	public static final float MARK = (float) 15.0;
	public static final int NUMBER_OF_STEPS = 2;
	public static final int PULL_UP = 5500; // for 16 cm

	public static int stepsDone = 1;

	static EV3MediumRegulatedMotor backWheelsDriver = initializeBackWheelsDriver();

	static EV3LargeRegulatedMotor frontWheelsDriver = initializeFrontWheelsDriver();

	static EV3LargeRegulatedMotor middleWheelsDriver = initializeMiddleWheelsDriver();

	static EV3TouchSensor touchSensor = initializeTouchSensor();
	static SampleProvider touchMode = getTouchMode();

	static EV3GyroSensor gyroSensor = initializeGyroSensor();
	static SampleProvider angleMode = getAngleMode();

	public static void main(String[] args) {
		// TODO Auto-generated method stub

		LCD.drawString("EV3 Stair Climber", 0, 2);

		Delay.msDelay(2000);

		float[] sample = new float[1];
		float[] touchSample = new float[1];
		boolean singleStepDone = false;

		while (Button.ESCAPE.isUp()) {

			LCD.clear();

			moveTheRobotForwardWithFrontAndBackWheels();
			fetchAndDisplayGyroSample(angleMode, sample);
			fetchAndDisplayTouchSample(touchMode, touchSample);

			if (gyroSensorSampleTooNegative(sample)) {

				changeTheSpeedOfBackAndMiddleWheels();

				while (!gyroSensorSamplePositive(sample)
						&& !touchSensorTouched(touchSample) && Button.ESCAPE.isUp()) {

					pullUpTheMiddleWheels();

					fetchAndDisplayGyroSample(angleMode, sample);

				}

				stopPullUp();
				singleStepDone = true;
			}

			if (singleStepDone) {

				moveForwardToFixTheFrontFourWheelsOnStair();

				changeTheSpeedOfBackWheels();

				pullUpTheBackWheels();

				singleStepDone = false;

				incrementStepCount();

				displayNumberOfStepsClimbed();

				if (allStepsAreClimbed()) {
					//printMessage("All Steps Climbed");
					break;
				}

			}

		}

		stopTheRobot();

	}

	private static boolean touchSensorTouched(float[] touchSample) {
		return touchSample[0] == 1.0;
	}

	private static void fetchAndDisplayTouchSample(SampleProvider touchMode2,
			float[] sample) {
		// TODO Auto-generated method stub
		touchMode.fetchSample(sample, 0);
		LCD.drawInt(new Float(sample[0]).intValue(), 5, 5);
	}

	private static SampleProvider getTouchMode() {
		// TODO Auto-generated method stub
		return touchSensor.getTouchMode();
	}

	private static EV3TouchSensor initializeTouchSensor() {
		// TODO Auto-generated method stub
		EV3TouchSensor touchSensor = new EV3TouchSensor(SensorPort.S2);
		return touchSensor;
	}

	private static void changeTheSpeedOfBackWheels() {
		// TODO Auto-generated method stub

		backWheelsDriver.setSpeed(540);
	}

	private static void moveForwardToFixTheFrontFourWheelsOnStair() {
		// TODO Auto-generated method stub
		backWheelsDriver.backward();
		Delay.msDelay(DRIVE_STRAIGHT_ONCE_ON_STAIR);
	}

	private static void changeTheSpeedOfBackAndMiddleWheels() {
		// TODO Auto-generated method stub

		middleWheelsDriver.setSpeed(630);
		backWheelsDriver.setSpeed(-121);
	}

	private static void printMessage(String message) {
		LCD.clear();
		LCD.drawString(message, 5, 5);
		Delay.msDelay(3000);
	}

	private static void pullUpTheBackWheels() {
		startPullUpTheBackWheels();
		waitToPullUpBackWheels();
		stopPullUp();

		// middleWheelsDriver.setSpeed(540);
		// middleWheelsDriver.rotateTo(2520);
	}

	private static void stopTheRobot() {
		frontWheelsDriver.stop();
		middleWheelsDriver.stop();
		backWheelsDriver.stop();
	}

	private static void moveTheRobotForwardWithFrontAndBackWheels() {
		moveTheBackWheelsForward();
		moveTheFrontWheelsForward();
	}

	private static void stopPullUp() {
		middleWheelsDriver.stop();
	}

	private static void waitToPullUpBackWheels() {
		Delay.msDelay(PULL_UP);
	}

	private static int incrementStepCount() {
		++stepsDone;
		return stepsDone;
	}

	private static void displayNumberOfStepsClimbed() {
		LCD.drawString("Step = " + stepsDone, 7, 7);
	}

	private static void startPullUpTheBackWheels() {
		middleWheelsDriver.setSpeed(540);
		middleWheelsDriver.forward();
	}

	private static boolean allStepsAreClimbed() {
		return stepsDone > NUMBER_OF_STEPS;
	}

	private static EV3LargeRegulatedMotor initializeMiddleWheelsDriver() {
		middleWheelsDriver = new EV3LargeRegulatedMotor(MotorPort.D);
		middleWheelsDriver.setSpeed(180);
		return middleWheelsDriver;
	}

	private static EV3LargeRegulatedMotor initializeFrontWheelsDriver() {
		frontWheelsDriver = new EV3LargeRegulatedMotor(MotorPort.B);
		frontWheelsDriver.setSpeed(630);
		return frontWheelsDriver;
	}

	private static EV3MediumRegulatedMotor initializeBackWheelsDriver() {
		backWheelsDriver = new EV3MediumRegulatedMotor(MotorPort.A);
		backWheelsDriver.setSpeed(450);
		return backWheelsDriver;
	}

	private static EV3GyroSensor initializeGyroSensor() {
		gyroSensor = new EV3GyroSensor(SensorPort.S3);
		gyroSensor.reset();

		return gyroSensor;
	}

	private static SampleProvider getAngleMode() {

		SampleProvider angleMode = gyroSensor.getAngleMode();
		return angleMode;
	}

	private static void moveTheFrontWheelsForward() {
		frontWheelsDriver.backward();
	}

	private static void moveTheBackWheelsForward() {
		backWheelsDriver.backward();
	}

	private static void pullUpTheMiddleWheels() {
		middleWheelsDriver.backward();
	}

	private static void fetchAndDisplayGyroSample(SampleProvider angleMode,
			float[] sample) {
		angleMode.fetchSample(sample, 0);
		LCD.drawInt(new Float(sample[0]).intValue(), 5, 5);
	}

	private static boolean gyroSensorSamplePositive(float[] sample) {
		return sample[0] >= 0.0;
	}

	private static boolean gyroSensorSampleTooNegative(float[] sample) {
		return sample[0] < (-MARK);
	}

}