package edu.kit.mindstormer.program.implementation;

import lejos.hardware.Button;
import lejos.hardware.Key;
import edu.kit.mindstormer.movement.Movement;
import edu.kit.mindstormer.movement.State;
import edu.kit.mindstormer.program.AbstractProgram;
import edu.kit.mindstormer.sensor.Sensor;

public class RollerBox extends AbstractProgram {

	private final int speed = 34;
	private final int rotationSpeed = speed / 2;

	public RollerBox() {
		super("RollerBox");
	}

	public void run() {
		// while (!quit.get()) {

		// move till wall is touched
		moveToWall();

		// increase distance from wall
		setBack();

		// rotate in order to drive through roller box backwards
		positionBackwards();

		// drive into roller box
		setBack();

		// drive through roller box
		driveThroughBox();

		// drive out of roller box
		driveOut();

		// reposition
		positionBackwards();

		Button.ESCAPE.simulateEvent(Key.KEY_RELEASED);
	}

	private void driveOut() {
		Movement.moveDistance(-10, speed);
		while (!State.stopped(true, true)) {
		}
	}

	private void driveThroughBox() {
		Movement.move(-speed);
		while (!State.stopped(true, true))
			if (Sensor.sampleDistance() > 20)
				break;
		Movement.stop();
	}

	private void positionBackwards() {
		Movement.rotate(90, rotationSpeed);
		while (!State.stopped(true, true)) {
		}
	}

	private void setBack() {
		Movement.moveDistance(-30, speed);
		while (!State.stopped(true, true)) {
		}
	}

	private void moveToWall() {
		Movement.move(speed);
		while (!State.stopped(true, true))
			if (Sensor.sampleTouchBoth() >= 1)
				break;
		Movement.stop();
	}
}

@@ -427,144 +427,20 @@
 lic 
-RollerBox() %7B%0A%09%09super(%22RollerBox%22);%0A%09%7D%0A%0A%09public void run() %7B%0A%09%09// while (!quit.get()) %7B%0A%0A%09%09// move till wall is touched%0A%09%09moveToWall();%0A
+void run() %7B
 %0A%09%09/
@@ -471,32 +471,34 @@
  wall%0A%09%09setBack(
+10
 );%0A%0A%09%09// rotate 
@@ -546,33 +546,26 @@
 wards%0A%09%09
-positionBackwards
+turnAround
 ();%0A%0A%09%09/
@@ -598,16 +598,18 @@
 setBack(
+20
 );%0A%0A%09%09//
@@ -718,33 +718,26 @@
 ition%0A%09%09
-positionBackwards
+turnAround
 ();%0A%0A%09%09B
@@ -838,18 +838,18 @@
 stance(-
-10
+25
 , speed)
@@ -1082,25 +1082,18 @@
 oid 
-positionBackwards
+turnAround
 () %7B
@@ -1115,9 +1115,10 @@
 ate(
-9
+18
 0, r
@@ -1201,16 +1201,28 @@
 setBack(
+int distance
 ) %7B%0A%09%09Mo
@@ -1246,224 +1246,68 @@
 ce(-
-30, speed);%0A%09%09while (!State.stopped(true, true)) %7B%0A%09%09%7D%0A%09%7D%0A%0A%09private void moveToWall() %7B%0A%09%09Movement.move(speed);%0A%09%09while (!State.stopped(true, true))%0A%09%09%09if (Sensor.sampleTouchBoth() %3E= 1)%0A%09%09%09%09break;%0A%09%09Movement.stop();
+distance, speed);%0A%09%09while (!State.stopped(true, true)) %7B%0A%09%09%7D
 %0A%09%7D%0A

Fix roller box