package edu.kit.mindstormer.program.implementation;

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

public class ChainBridge extends AbstractProgram {

	@Override
	public void run() {
		Movement.move(true, 20);
		while(!Sensor.sampleTouchBoth());

		Movement.moveCircle(-90, true, 7, 10);
		State.waitForMovementMotors();
		
		Movement.moveDistance(-40, 10);
		State.waitForMovementMotors();
		
		Sound.beep();
		Movement.stop();
		while(Sensor.sampleDistance() < 40) {
			Movement.holdDistance2(false, 10, 10);
		}
		Sound.beep();
		
		//Movement.move(true, 10);
		//while(Sensor.sampleDistance() > 20);
		//Movement.alignParallel(40, 10, 30);
		//Movement.moveDistance(-40, 10);
		//State.waitForMovementMotors();
		Movement.moveDistance(-170, 45);
		State.waitForMovementMotors();
		
		Movement.move(false, 10);
		while(Sensor.sampleDistance() > 25);
		while(Sensor.sampleDistance() < 15) {
			Movement.holdDistance2(false, 10, 10);
		}
		Movement.moveDistance(20, 10);
		State.waitForMovementMotors();
		Movement.rotate(180, 10);
		State.waitForMovementMotors();
		Movement.move(true, 30);
		while(!Sensor.sampleTouchBoth());
		Movement.moveCircle(-90, true, 7, 10);
		State.waitForMovementMotors();
		Movement.moveDistance(40, 10);
		State.waitForMovementMotors();
		Movement.stop();
		
	}

}

@@ -435,34 +435,33 @@
 e(-90, true, 7, 
-10
+8
 );%0A%09%09State.waitF
@@ -908,10 +908,11 @@
 70, 
-45
+100
 );%0A%09
@@ -965,57 +965,266 @@
 move
-(false, 10);%0A%09%09while(Sensor.sampleDistance() %3E 25
+Distance(10, 10);%0A%09%09State.waitForMovementMotors();%0A%09%09%0A%09%09Movement.rotate(-20, 10);%0A%09%09State.waitForMovementMotors();%0A%09%09%0A%09%09Movement.moveDistance(-20, 10);%0A%09%09State.waitForMovementMotors();%0A%09%09%0A%09%09while(Sensor.sampleDistance() %3E 25);%0A%09%09Movement.moveDistance(-10, 10
 );%0A%09

add fix to chain bridge