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

public class wander {

//static boolean light1 = false;
//static boolean light2 = false;
static int light = 55;
static int closeEnough = 65;
static int tooFar = 80;
static int light1 = 0;
static int light2 = 0;
static boolean ext = false;
		
    public static void main(String[] args)
	throws InterruptedException {
		
		while(!ext)
		{
			
			TextLCD.print("Seek");
			
			Sound.buzz();
			
			try {
				Thread.sleep (1000);
			} catch (InterruptedException e) {}
	
	
			Motor.A.setPower(7);
			Motor.B.setPower(7);
			Motor.C.setPower(7);
			Motor.A.forward();
			Motor.C.forward();
			
			
			//S1 and S3 are each actually a light sensor and a touch sensor
			//I found a trick to just pretend they're light sensors, since the touch
			//sensor will return "100%" when it is activated, otherwise the light's
			//reading is used.  This way, I can have 4 sensors on two ports!
			
			// 3 is the code for light sensor and 0x80 is the code for %
			// 1 is touch and 0x20 would be boolean
			
			Sensor.S1.setTypeAndMode (3, 0x80);
			Sensor.S1.activate();
			Sensor.S1.addSensorListener (new SensorListener() {
				public void stateChanged (Sensor src, 
							  int oldValue, 
							  int newValue) {
					try {
		
		
			
				    // Will be called whenever sensor value changes			    
				    if(newValue>=tooFar) // if we were just bumped
				    {
				    	bumped(true);
				    }
				    else if(newValue>light)
				    {
				    	light1 = newValue;
				    	Motor.B.forward();
				    	Motor.A.setPower(3);
						Motor.C.setPower(3);
						if((light2+light1)>100)
							approach(newValue);
						else
							search(true);
	
						LCD.showNumber(newValue);
						try {
				        	Thread.sleep (100);
				        } catch (InterruptedException e) {}
					}
					} catch (InterruptedException e) {}
				}
			});
	
			Sensor.S3.setTypeAndMode (3, 0x80);
			Sensor.S3.activate();
			Sensor.S3.addSensorListener (new SensorListener() {
				public void stateChanged (Sensor src, 
							  int oldValue, 
							  int newValue) {
				  try {
	
				    if(newValue>=tooFar) // if we were just bumped
						bumped(false);
						
				    else if(newValue>light-5)
				    {
				    	Motor.B.forward();
				    	Motor.A.setPower(3);
						Motor.C.setPower(3);
						if((light1+light2)>100)
							approach(newValue);
						else
							search(false);
	
						LCD.showNumber(newValue);
						try {
				        	Thread.sleep (100);
				        } catch (InterruptedException e) {}
					}
				  } catch (InterruptedException e) {}
				}
			});
			
			//For now, keep running until told to stop
			Button.RUN.waitForPressAndRelease();
		}
	}
	
    public static boolean bumped(boolean left)
	throws InterruptedException {

    	if(Motor.B.isMoving())
    	{
    		Motor.A.stop();
    		Motor.B.stop();
    		Motor.C.stop();
    		Sound.beepSequence();
    		ext = true;
    		return false;
    	}
		else
		{
			
		    //back up
			Motor.A.backward();
			Motor.C.backward();
			try {
			    Thread.sleep (500);
			} catch (InterruptedException e) {}
			
			//turn
			if(left)
				Motor.A.forward();
			else
				Motor.C.forward();
			double d = Math.random();
			try {
			    Thread.sleep ((int)(500* d));
			} catch (InterruptedException e) {}
			
			//go forward again
			Motor.A.forward();
			Motor.C.forward();
		}
		return true;
	}
	
	
	public static void approach(int val)
	throws InterruptedException {
		//Motor.A.stop();
		//Motor.C.stop();
		if(val<closeEnough-5)
		{
			Motor.A.setPower(1);
			Motor.C.setPower(1);
			Motor.A.forward();
			Motor.C.forward();
		}
	}

	public static void search(boolean left)
	throws InterruptedException {
		if(left)
		{		
			Sound.playTone(220,5);
			Motor.A.backward();
			Motor.C.forward();			
		} else {
			Sound.playTone(440,10);
			Motor.C.backward();
			Motor.A.forward();
		}
		
		try {
	    	Thread.sleep (100);
        } catch (InterruptedException e) {}
		Motor.A.forward();
		Motor.C.forward();
	}
}