import josx.platform.rcx.*;
import java.lang.*;

public class Move2 {
	
	private static boolean ReachTarget=false;
	private int count=0;	private int dist=4; 
	public Move2()throws InterruptedException
	{
		Sensor.S1.setTypeAndMode(3,0x80);
		Sensor.S1.activate();
		Sensor.S3.setTypeAndMode(3,0x80);
		Sensor.S3.activate();
		Sensor.S2.setTypeAndMode(3,0x80);
			Sensor.S2.activate();
		Sensor.S2.addSensorListener(new SensorListener() {
			public synchronized void stateChanged(Sensor src, int oldValue, int newValue)
			{
				if(src == Sensor.S2 &&	newValue>45 &&oldValue<45)
				{
					count++;
					try{Thread.sleep(500); 
					}catch(Exception e){
						LCD.showNumber(newValue);
						}
				 LCD.showNumber(newValue);					
				}
			if(count>=dist){
				ReachTarget=true;
		
			}
   		//Thread.sleep(500);
    	Motor.A.stop();
   		Motor.C.stop();
			}
		});
					
	}

	private void moveToLine()throws Exception
	{
		Motor.A.setPower(1);
		Motor.C.setPower(1);
		Motor.A.forward();
		Motor.C.forward();
		int s1 = Sensor.S1.readValue();
		int s3 = Sensor.S3.readValue();
		while(!(ReachTarget) && s1 <= 45 && s3 <= 45)
		{
			s1 = Sensor.S1.readValue();
			s3 = Sensor.S3.readValue();
		}
		if(!(ReachTarget))
		{
			if(s1 >45)
			{
				Motor.A.stop();
				while(s3 <= 45)
					s3 = Sensor.S3.readValue();
			}
			else
			{
				Motor.C.stop();
				while(s1 <= 45)
					s1 = Sensor.S1.readValue();
			}
			Motor.A.stop();
			Motor.C.stop();
		}
	}
		



	
	public synchronized void moveForward()
	throws InterruptedException
	{
		try{
			moveToLine();
			}catch(Exception e)
			{
			LCD.clear();
			TextLCD.print("error");
			Thread.sleep(500);
			}
		
		if(!ReachTarget)
		{
			Motor.A.setPower(4);
			Motor.C.setPower(4);
			Motor.A.forward();
			Motor.C.forward();
			try{ wait(800); }
				catch (InterruptedException e) { }
		}
	}





	public synchronized void shutdown()
	{
		Motor.A.stop();
		Motor.B.stop();
	}
}