/****************************************
 * VacuumBot.java
 *
 * Contains class VacuumBot for use with
 * LEGO mindstorms sets acting as
 * VacuumWorld. 
 *
 *
 * by Andy Exley
 * 12/02/02
 ****************************************/ 
import josx.platform.rcx.*;
import java.lang.*;

public class VacuumBot {
	private final int S1_BLACK = 795; // Raw value that light sensor1 returns on black
	private final int S3_BLACK = 765; // Raw value that light sensor3 returns on black
	private final int AVG_HOME = 735; // Average light sensor raw value on blue paper
	private final int AVG_DIRT = 700; // Average light sensor raw value on purple paper
	private final int TURN_DELAY = 1000; // Wait time while turning (in milliseconds)

	private static boolean bumpSensor; // boolean that is set to true while the
					// SensorListener's method is running

	// The constructor for VacuumBot sets all the sensors properly, activates
	// the light sensors, and adds a listener for the touch sensor.
	// Any time the touch sensor is tripped, the robot will back up for
	// 400 milliseconds.
	public VacuumBot()
	throws InterruptedException
	{
		Sensor.S1.setTypeAndMode(3,0x00);
		Sensor.S1.activate();
		Sensor.S3.setTypeAndMode(3,0x00);
		Sensor.S3.activate();
		Sensor.S2.setTypeAndMode(1,0);
		Sensor.S2.addSensorListener(new SensorListener() {
			public synchronized void stateChanged(Sensor src, int oldValue, int newValue)
			{
				if(src == Sensor.S2 && oldValue > 932 && newValue < 933)
				{
					bumpSensor = true;
					Motor.A.backward();
					Motor.C.backward();
					try { wait(400); }
						catch(InterruptedException e) { }
					Motor.A.stop();
					Motor.C.stop();
					bumpSensor = false;
				}
			}
		});
					
	}

	// moveToLine is a utility function that moves the robot up to a black line
	// and hopefully corrects its course using light sensors.
	private void moveToLine()
	{
		Motor.A.setPower(1);
		Motor.C.setPower(1);
		Motor.A.forward();
		Motor.C.forward();
		int s1 = Sensor.S1.readRawValue();
		int s3 = Sensor.S3.readRawValue();
		while(!(bumpSensor) && s1 <= (S1_BLACK-10) && s3 <= (S3_BLACK-10))
		{
			s1 = Sensor.S1.readRawValue();
			s3 = Sensor.S3.readRawValue();
		}
		if(!(bumpSensor))
		{
			if(s1 > (S1_BLACK-10))
			{
				Motor.A.stop();
				while(s3 <= (S3_BLACK-10))
					s3 = Sensor.S3.readRawValue();
			}
			else
			{
				Motor.C.stop();
				while(s1 <= (S1_BLACK-10))
					s1 = Sensor.S1.readRawValue();
			}
			Motor.A.stop();
			Motor.C.stop();
		}
	}
		

	public synchronized void turnRight()
	throws InterruptedException
	{
		Motor.A.setPower(1);
		Motor.C.setPower(1);
		Motor.A.forward();
		Motor.C.backward();
		try{ wait(TURN_DELAY); }
			catch (InterruptedException e) { }
		Motor.A.stop();
		Motor.C.stop();
	}

	public synchronized void turnLeft()
	throws InterruptedException
	{
		Motor.A.setPower(1);
		Motor.C.setPower(1);
		Motor.A.backward();
		Motor.C.forward();
		try{ wait(TURN_DELAY); }
			catch (InterruptedException e) { }
		Motor.A.stop();
		Motor.C.stop();
	}

	public synchronized void moveForward()
	throws InterruptedException
	{
		moveToLine();
		if(!bumpSensor)
		{
			Motor.A.setPower(4);
			Motor.C.setPower(4);
			Motor.A.forward();
			Motor.C.forward();
			try{ wait(800); }
				catch (InterruptedException e) { }
		}
	}

	// Home is represented by blue paper.
	public boolean atHome()
	{
		int avg = (Sensor.S1.readRawValue()+Sensor.S3.readRawValue())/2;
		return ((avg > AVG_HOME-15) && (avg < AVG_HOME+15));
	}

	// Dirt is represented by purple paper.
	public boolean onDirt()
	{
		int avg = (Sensor.S1.readRawValue()+Sensor.S3.readRawValue())/2;
		return ((avg > AVG_DIRT-10) && (avg < AVG_DIRT+10));
	}

	// the suck function plays a buzz to indicate that the dirt is being "cleaned"
	public void suck()
	{
		Sound.buzz();
	}

	// this returns false if bumpSensor is false. If bumpSensor is true, it waits
	// for sensorListener to finish its task (backing up) then it returns true
	public boolean hitBump()
	{
		if(bumpSensor == true)
		{
			while (bumpSensor) ; // wait for the sensorListener
						// to finish its task
			return true;
		}
		return false;
	}

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