import lejos.nxt.*;
import lejos.nxt.comm.*;
import lejos.robotics.navigation.*;
import java.io.*;
import java.util.Vector;
import javax.bluetooth.*; 
/**
*<p>A java class that provides a simpler abstraction for using the 
*lego NXT mindstorm robot in its default configuration (minus the arm).</p>
*
*This class provides the following functionality:
*<table>
*<tr><td><b><i>Travelling</i></b></td> <td> Forward and backward in feet and inches</td></tr>
*<tr><td><i><b>Turning</b></i></td><td> X degrees left or right</td></tr>
*<tr><td><i><b>Color sensing</i></b></td><td>Red,grey,blue,yellow,white [colored duct tape]</td></tr>
*<tr><td><i><b>Clap sensing</i></b></td><td>Detects how many claps in 3 seconds</td></tr>
*<tr><td><i><b>Distance sensing</i></b></i></b></</td> <td>Detects how many feet and inches your robot is away from a wall</td></tr>
*<tr><td><i><b>Pickup / Drop</i></b></td><td> The ability to pick up lego blocks or drop them</td></tr>
*<tr><td><i><b>Bluetooth</i></b></td><td>Find, connect, and share data with other Robots </td></tr>
*</table>
</ul>
*<p><b>This class assumes a basic lego robot build, minus the arm assembly!</b>
*<br>You will need to connect your sensors in the following configuration
*for most of the functionality in this class to work.
*<ul>
*<li>Facing the brick, the right wheel motor is plugged into C</li>
*<li>The left wheel motor is plugged into B</li>
*<li>The arm motor is plugged into A</li>
*<li>The sound sensor is plugged into port 4</li>
*<li>The color sensor is plugged into port 3</li>
*<li>The ultrasonic sensor is plugged into port 2</li>
*<li>The touch sensor is plugged into port 1</li>
*</ul>
*<b>Example usage:</b><br>
*<pre>
*Robot rob = new Robot();
*rob.moveForward();
*rob.turAround();
*rob.moveForward();
*rob.turnAround();
*</pre>
*/
public class Robot
{
	
	/**
	* Set the number of beats per minutes
	* <p>This determines how long notes are played</p>
	* @param bpm The number of beats per minute
	*/
	public void setBPM(int bpm)
	{
		double perSecond = 60.0/bpm;
		m_beatLength = perSecond * 1000;
	}
	/**
	* Internal function used to determine how long a note should be played
	* @param value The type of note to be played
	* @return  The number of microseconds to play the song
	*/
	public int getNoteLength(String value)
	{
		int length = 0;
		if (value.equals("w"))
			length = getLength(4);
		else if (value.equals("dh"))
			length = getLength(3);
		else if (value.equals("h"))
			length = getLength(2);
		else if (value.equals("dq"))
			length = getLength(1.5);
		else if (value.equals("q"))
			length = getLength(1);
		else if (value.equals("8th"))
			length = getLength(.5);
		else if (value.equals("16th"))
			length = getLength(.25);
		else if (value.equals("32nd"))
			length = getLength(1.0/8.0);
		else if (value.equals("64th"))
			length = getLength(1.0/16.0);
		return length;
	}
	/**
	* A musical rest function that causes the Robot to remain silent for a certain period of time 
	* @param value The note type to rest.  See the note types for playNote for more detail
	*/
	public void rest(String value)
	{
		try{ Thread.sleep(getNoteLength(value));} 
		catch(InterruptedException e){}
	}
	/**
	* Internal function used to compute the length of a particular note
	* @param length The numerical representation of a note (1 for a quarter note, etc...)
	* @return  The number of microseconds of a particular note
	*/
	public int getLength(double length)
	{
		return (int)(length*m_beatLength);
	}
	//Wikpedia provided the multiplication factor
	/**
	* Internal function used to get the frequency to give to the NXT brick
	* @param  note The note to find the frequency for
	* @param octave  The octave that the note is in
	* @return The frequency that the NXT can play
	*/
	public int getFrequency(String note, int octave)
	{
		//Start with C0
		double base = 16.35;

		//Jump to the right octave	
		while (octave > 0)
		{	
			for (int i=0; i<12; i++)
				base = base * Math.pow(2,1.0/12.0);
			octave--;
		}

		//Jump to the right note
		int jump =0;
		if (note.equals("C#"))
			jump = 1;
		if (note.equals("D"))
			jump = 2;
		if (note.equals("D#"))
			jump =3;
		if (note.equals("E"))
			jump = 4;
		if (note.equals("F"))
			jump = 5;
		if (note.equals("F#"))
			jump=6;
		if (note.equals("G"))
			jump = 7;
		if (note.equals("G#"))
			jump = 8;
		if (note.equals("A"))
			jump = 9;
		if (note.equals("A#"))
			jump = 10;
		if (note.equals("B"))
			jump = 11;

		for (int i=0; i<jump; i++)
			base = base * Math.pow(2,1.0/12.0);

		return (int)base;
	}
	/**
	* Play a specific note using the Robot's speakers
	* <p>Notes are in the format of NoteOcatve.  For example A4, A#3, and B2.  The type of note is w=whole, dh= dotted half , h = half, dq = dotted quarter, q=quarter, 8th=eight note, 16th = sixteenth note  </p>
	*<p>To play an A4 quarter note you would call playNote("A4", "q");</p>
	* @param note The note to play, in format of NoteOctave
	* @param type The type of note to play
	*/
	public void playNote( String note, String type)
	{
		String theNote = " ";   
		String theOctave = " "; 
		if (note.length() ==3 )
		{
			theNote = String.valueOf(note.charAt(0)) +
				  String.valueOf(note.charAt(1));
			theOctave = String.valueOf(note.charAt(2));
		}
		if (note.length() ==2 )
		{
			theNote = String.valueOf(note.charAt(0));
			theOctave = String.valueOf(note.charAt(1));
		}
		if (m_insturment ==0)
			Sound.playTone(
			getFrequency(theNote, Integer.parseInt(theOctave)),
			getNoteLength(type));
		if (m_insturment == 1)
			Sound.playNote(Sound.PIANO,
			getFrequency(theNote, Integer.parseInt(theOctave)),
			getNoteLength(type));
		if (m_insturment == 2)
			Sound.playNote(Sound.FLUTE, 
			getFrequency(theNote, Integer.parseInt(theOctave)),
			getNoteLength(type));
		if (m_insturment == 3)
			Sound.playNote(Sound.XYLOPHONE,
			getFrequency(theNote, Integer.parseInt(theOctave)),
			getNoteLength(type));

		try{ Thread.sleep(getNoteLength(type));} 
		catch(InterruptedException e){}
	}
	/**
	* Get the type of insturment to be used while playing notes
	* @return The insturment being used
	*/
	public String getInstrument()
	{
		if (m_insturment == 0)
			return "None";
		if (m_insturment == 1)
			return "Piano";
		if (m_insturment == 2)
			return "Flute";
		if (m_insturment == 3)
			return "Xylophone";
		return "None";
	}
	/**
	* Set the insturment to be used
	* <pre>Valid insturments are: 
	* Piano
	* Flute
	* Xylophone
	* Any other string will result in regular tones being used when playing notes.
	* </pre>
	* @param insturment The insturment to use
	*/
	public void setInstrument(String insturment)
	{
		if (insturment.equals( "Flute"))
			m_insturment = 2;
		else if (insturment.equals( "Piano"))
			m_insturment = 1;
		else if (insturment.equals( "Xylophone"))
			m_insturment = 3;
		else
			m_insturment = 0;
	}
	/**
	* Connect all of the sensors and initialize data for the robot
	*/
	public Robot()
	{
		//Initialize variables
		m_array = new int[150];
		m_results = new int[8];
		m_robotNames = new String[90];
		m_bluetooth = null;
		m_insturment = 0;
		//Make the sensors for the robot
		makeLCD();
		makeTouch();
		makeLight();
		makeUltra();
		makePilot();
		makeSound();
		m_switch=true;
	}
	/**
	* Detect the number of claps in a 3 second interval
	* @return The number of claps detected
	*/
	public int numClaps()
	{
		int numDetected=0;
		m_sound.setDBA(true);
		//Clean out internal array
		zeroArrays();
		int claps=0;
		for (int i=0; i<150; i++)
		{
			int value = m_sound.readValue();
			//Convert to DB
			m_array[i] =(int)( (double)value/100.0 * 90.0);
			//value = 60-(value/2);
			//if (i <100 )
			//	lcd.setPixel(1,i,value);
			try { Thread.sleep(20);}
			catch (InterruptedException e){}
		}
		int peak=0;
		int valley=0;
		//Now detect the peaks for the wave
		for (int i=0; i<150; i++)
		{
			//Clap detected (25 db threshold)
			if (m_array[i] >25 )
			{
				//find the peak of the clap
				peak=i;
				while ((peak < 149) && (m_array[peak+1] > m_array[peak]))
					peak++;
				//find the valley of the clap
				valley=peak;
				while ( (valley < 149) && m_array[valley] > 25)
					valley++;

				claps++;
				i=valley;
			}
		}
		//displayInt(claps,0);
		return claps;
	}
	
	/**
	* Stop the Robot's movement
	*/
	public void stop()
	{
	 	stopMotors();
	}
	/**
	* Set the volume of the Robot's speakers
	* @param vol The volume to set, must be from 0 to 100
	*/
	public void setVolume(int vol)
	{
		if (vol < 0 || vol > 100)
			return;
		Sound.setVolume(vol);
	}
	/**
	* Get the volume that the Robot is using
	* @return  The volume from 0-100 that the Robot is using
	*/
	public int getVolume()
	{
		return Sound.getVolume();
	}
	/**
	* Sever the Bluetooth connection with another Robot
	*/
	public void disconnect()
	{
		if (m_bluetooth != null)
		{
			try
			{
				m_input.close();
				m_output.close();
				m_bluetooth.close();
			}
			catch(IOException e){};
		}
	}
	/**
	* Returns whether or not there is more than one foot of free space in front of the Robot
	* @return Whether or not there is more than one foot of free space in front of the Robot
	*/
	public boolean frontIsClear()
	{
		if (distanceToWall() >= 1.0)
			return true;
		else 
			return false;
	}
	/**
	* Return the distance (in feet and inches) from the color sensor to the nearest wall 
	* <p>This function takes 5 seconds to finish, and is accurate to within a couple of inches most of the time.  Occasionally the sensor will return bad data</p>
	* @return The distance between the color sensor and the nearest wall
	*/
	public double distanceToWall()
	{
		int dist= getDistance();
		if (dist < 40)
			return 0.0;
		double feet=0.0;
		while (dist >= 40.0)
		{
			feet++;
			dist-=30.0;
		}
		//Take off remainder of first foot
		double remainder = dist - 13.0;
		double conversion = 30.5/12.0;
		double inches = (remainder/conversion)/10;
		if (inches < 0)
			inches=0;
		double result = feet + inches;
		String hold = Double.toString(result);
		double value = 0.0;
		if (hold.length()>5)	
		{
			String cut = hold.substring(0,5);
			value = Double.parseDouble(cut);
		}
		else
			value = Double.parseDouble(hold);
		return  value;
	}
	//Get the distance from the wall from the sensor
	//Due to hardware that is unreliable this
	//takes about 3 seconds to perform and returns an 
	//integer distance, which is then transformed
	//via the distance to wall function into feet + inches.
	/**
	* Get the distance between the color sensor and the wall, in NXT units
	* @return The distance between the color sensor and the wall
	*/
	public int getDistance()
	{
		m_ultra.reset();
		int value;
		zeroArrays();
		for (int a=0; a<8; a++)
		{
			for (int i=0; i<8; i++)
			{
				m_ultra.ping();
				//Minimum time to sleep that allows us to get 
				//consistent numbers
				try { Thread.sleep(30);}
				catch (InterruptedException e) {};
				m_array[i] = m_ultra.getDistance();
			}
			sort();
			//Discard beginning and end b/c sensor is not so good...
			value = (m_array[m_array.length-6] + m_array[m_array.length-5] + m_array[m_array.length-4])/3;
			m_results[a] = value;
		}
		value=0;
		for (int a=0; a<8; a++)
			value += m_results[a];
		//displayInt(dist,0);
		//m_ultra.getDistances(distances);
		//for (int i=0; i<8; i++)
		//	displayInt(distances[i],i);
		m_ultra.off();
		sleep(.05);
		return value/8;
	}
	/**
	* Grab a block using the claw
	*/
	public void grasp()
	{
		Motor.A.setSpeed(25);
		Motor.A.resetTachoCount();
		Motor.A.backward();
		int num = Motor.A.getTachoCount();
		while (num > -58)
		{
			num= Motor.A.getTachoCount();
			sleep(.01);
		}
		Motor.A.stop();
		sleep(.5);
	}
	/**
	* Release a block grabbed by the claw
	*/
	public void release()
	{
		Motor.A.setSpeed(25);
		int num = Motor.A.getTachoCount();
		if (num < 0)
			Motor.A.forward();
		else 	
			return;
		while (num < 0)
		{
			num= Motor.A.getTachoCount();
			sleep(.01);
		}
		Motor.A.stop();
		sleep(.5);
	}
	/**
	* Turn the Robot left 90 degrees
	*/
	public void turnLeft()
	{
		turnXDegreesLeft(90);
	}
	/**
	* Move the Robot forward 1 foot
	*/
	public void moveForward()
	{
		moveForwardXFeet(1.0);
	}
	/**
	* Move the Robot backward 1 foot
	*/
	public void moveBackward()
	{
		moveBackwardXFeet(1.0);
	}
	/**
	* Turn the Robot right by 90 degrees
	*/
	public void turnRight()
	{
		turnXDegreesRight(90);
	}
	/**
	* Turn the Robot 180 degrees
	*/
	public void turnAround()
	{
		turnLeft();
		turnLeft();
	}
	/**
	* Turn the Robot a certain number of degrees to the left
	* @param X  The number of degrees to turn
	*/
	public void turnXDegreesLeft(int X)
	{
		X=X+45;
		turnXDegrees(X, "B");
	}

	/**
	* Turn the Robot a certain number of degrees to the right 
	* @param X  The number of degrees to turn
	*/
	public void turnXDegreesRight(int X)
	{
		X=X+45;
		turnXDegrees(X, "C");
	}
	/**
	* Internal function, turn a specific motor X degrees
	* @param X  The number of degrees to turn
	* @param motor  The motor to turn based on the Sensor port it is plugged into
	*/
	public void turnXDegrees(int X, String motor)
	{
		if (motor.equals("C"))
			m_pilot.rotate(-1*X);
		else if (motor.equals("B"))
			m_pilot.rotate(X);
		sleep(.05);
		/*
		int num=0;
		if (motor.equals("C"))	
		{
			Motor.C.resetTachoCount();
			Motor.C.forward();
			num  = Motor.C.getTachoCount();
		}
		if (motor.equals("B"))
		{
			Motor.B.resetTachoCount();
			Motor.B.forward();
			num  = Motor.B.getTachoCount();
		}
		//Obtained by measuring distance / tach number
		//for speeds 200-600 for 1 and 2 seconds
		int tach = 52;
		//90 degree case
		//Width of robot (minus some tweaking :D)
		double width= 4.6803;
		//Now make a right triangle with both sides being
		//the same length, i.e. turning 90 degrees to the right
		//Figure out the length of the hypotenuse
		double length = Math.sqrt((width * width)*2);
		double distPerDegree = length/90.0;
		
		while (num < tach*(distPerDegree*X))
		{
			sleep(.001);
			if (motor.equals("C"))
				num = Motor.C.getTachoCount();
			if (motor.equals("B"))
				num = Motor.B.getTachoCount();
		}
		stopMotors();
		sleep(.05);
	*/
	}
	/**
	* Move the Robot forward a certain number of feet and inches
	*<p>Remember, 1.5 feet is 18 inches!</p>
	* @param X The number of feet and inches to move forward 
	*/
	public void moveForwardXFeet(double X)
	{
		/*
		m_pilot.travel((float)X*12);
		sleep(.05);
		*/
		Motor.C.resetTachoCount();
		Motor.B.resetTachoCount();
		int numB = Motor.B.getTachoCount();
		//int numC = Motor.C.getTachoCount();
		//startForward();
		startBackward();
		//Obtained by measuring distance / tach number
		//for speeds 200-600 for 1 and 2 seconds
		int tach = 52;
		//while (numB < tach* (X*12))
		while (numB > -1*(tach* (X*12)))
		{
			sleep(.001);
			numB = Motor.B.getTachoCount();
		}
		stopMotors();
		sleep(.05);
	}
	/**
	* Move the Robot backward a certain number of feet and inches
	*<p>Remember, 1.5 feet is 18 inches!</p>
	* @param X The number of feet and inches to move backward 
	*/
	public void moveBackwardXFeet(double X)
	{
		//m_pilot.travel((float)X*12, true);
		//sleep(.05);
		Motor.C.resetTachoCount();
		Motor.B.resetTachoCount();
		int numB = Motor.B.getTachoCount();
		//startBackward();
		startForward();
		//Obtained by measuring distance / tach number
		//for speeds 200-600 for 1 and 2 seconds
		int tach = 52;
		//while (numB > -1*(tach* (X*12)))
		while (numB < tach* (X*12))
		{
			sleep(.001);
			numB = Motor.B.getTachoCount();
		}
		stopMotors();
		sleep(.05);
	}

	/**
	* Set how fast the Robot moves
	* <p>You can use speeds from 100-800, however 300 is recommended</p>
	* @param speed The speed you want the Robot to go
	*/
	public void setSpeed(int speed)
	{
		Motor.B.setSpeed(speed);
		Motor.C.setSpeed(speed);
		m_pilot.setSpeed(speed);
	}
	/**
	* Move the Robot forward until you tell it to stop
	*/
	public void startForward()
	{
		if (m_switch)
		{
			Motor.C.forward();
			Motor.B.forward();
			m_switch=false;
		}
		else
		{
			Motor.B.forward();
			Motor.C.forward();
			m_switch=true;
		}
	}
	/**
	* Move the Robot backward until you tell it to stop
	*/
	public void startBackward()
	{
		Motor.C.backward();
		Motor.B.backward();
	}
	/**
	* Cause the Robot (and your program) to sleep for a certain number of seconds
	* @param numSeconds How long to sleep
	*/
	public void sleep(int numSeconds)
	{
		try
		{
			Thread.sleep(1000*numSeconds);
		}
		catch (InterruptedException e)
		{
		
		}
	}
	/**
	* Tell the program to sleep for a certain number of seconds, can be 1.5 or 2.5
	* @param numSeconds The number of seconds to sleep
	*/
	public void sleep(double numSeconds)
	{
		try
		{
			numSeconds *=100;
			Thread.sleep(10*(int)numSeconds);
		}
		catch (InterruptedException e)
		{

		}
	}
	/**
	* Returns if the Robot has backed into anything
	* @return Whether or not it has backed into anything
	*/
	public boolean isTouching()
	{
		return m_touch.isPressed();
	}
	/**
	* Display a String in one of the 8 rows for the Robot display (starts at 0)
	* @param textToDisplay The String to write
	* @param line The line to write it on
	*/
	public void displayString(String textToDisplay, int line)
	{
		lcd.drawString(textToDisplay,0 ,line);
	}
	/**
	* Display a double in one of the 8 rows for the Robot display (starts at 0)
	* @param num The double to write
	* @param line The line to write it on
	*/
	public void displayDouble(double num, int line)
	{
		lcd.drawString(Double.toString(num),0 ,line);
	}
	/**
	* Display an integer in one of the 8 rows for the Robot display (starts at 0)
	* @param num The double to write
	* @param line The line to write it on
	*/
	public void displayInt(int num, int line)
	{
		lcd.drawInt(num,0,line);
	}
	/**
	* Remove all previously displayed information on the LCD
	*/
	public void clearDisplay()
	{
		lcd.clear();
	}
	/**
	* Read the color that the color sensor is on
	* <p>Can currently detect red, blue, white, yellow, and grey</p>
	* @return The color detected
	*/
	public String readColor() 
	{
		m_light.setFloodlight(true);
		sleep(.5);
		//White
		int high = 590;
		//Black
		int low = 342;
		m_light.setLow(low);
		m_light.setHigh(high);
		int sum=0;
		int val=0;
		Boolean change=true;
		for (int i=0; i<10; i++)
		{
			val = m_light.readNormalizedValue();
			sleep(.05);
			sum+= m_light.readValue();
			if (change)
			{
				moveForwardXFeet( (1.0/12.0)*.1);
				change = false;
			}
			else
			{
				moveBackwardXFeet( (1.0/12.0)*.1);
				change = true;

			}
		}
		val = sum/10;
		m_light.setFloodlight(false);
		
		/*
		clearDisplay();
		displayInt(val,1);
		*/
		if (val >=20 && val <=60)
			return "red";
		else if (val >=60)
			return "white";
		else if (val <8)
			return "black";
		//My robot can detect multiple colors ok
		//others need to be simplified for accuracte
		//detection
		/*
		else if (val >= 77 && val <82)
			return "yellow";
		if (val >= 15 && val <=25)
			return "blue";
		else if (val >=42 && val <=65)
			return "grey";
		*/
		sleep(1);
		return "blank";
	}
	/**
	* Stop the Robot's movement
	*/
	public void stopMotors()
	{
		Motor.C.stop();
		Motor.B.stop();
	}
	/**
	* Create the LCD class used to display information
	*/
	public void makeLCD()
	{
		lcd = new LCD();
	}
	/**
	* Create the touch sensor 
	*/
	public void makeTouch()
	{
		m_touch = new TouchSensor(SensorPort.S1);
	}
	/**
	* Create the light sensor 
	*/
	public void makeLight()
	{
		m_light = new LightSensor(SensorPort.S3);
		m_light.setFloodlight(false);
	}
	/**
	* Create the ultra sonic sensor 
	*/
	public void makeUltra()
	{
		m_ultra = new UltrasonicSensor(SensorPort.S2);
		m_ultra.off();
	}

	/**
	* Create the TachoPilot class, currently unused
	*/
	public void makePilot()
	{
		m_pilot = new LegacyPilot(2.21f,4.4f, Motor.C, Motor.B);
		m_pilot.setSpeed(300);
	}
	/**
	* Create the SoundSensor
	*/
	public void makeSound()
	{
		m_sound = new SoundSensor(SensorPort.S4);
	}
	/**
	* Sort the internal arrays
	*/
	public void sort()
	{
		int min = m_array[0];
		int temp=m_array[0];
		for (int i=0; i< m_array.length; i++)
		{
			min = i;
			for (int j=min+1; j<m_array.length; j++)
			{
				if (m_array[j] < m_array[min])
				{
					temp = m_array[min];
					m_array[min] = m_array[j];
					m_array[j] = temp;
				}
			}
		}	
	}
	/**
	* Resets the contents of the classes internal arrays
	* <p>You should never have to call this method</p>
	*/
	public void zeroArrays()
	{
		for (int i=0; i<m_array.length; i++)
			m_array[i] = 0;	
		for (int i=0; i<m_results.length; i++)
			m_results[i] = 0;
		for (int i=0; i<m_robotNames.length; i++)
			m_robotNames[i] = "NoName";
	}
	/**
	* Listen for another Robot's bluetooth connection attempt
	* <p>This will cause your robot to sit and wait until it
	* connects with another robot.</p>
	*/
	public void listen()
	{
		//m_bluetooth = Bluetooth.waitForConnection();
		m_input = m_bluetooth.openDataInputStream();
		m_output = m_bluetooth.openDataOutputStream();
	}
	/**
	* Get an integer from the other Robot using Bluetooth
	* @return The integer read from another robot, or -1 if it failed.
	*/
	public int getInt()
	{
		sleep(.05);
		if (m_bluetooth == null)
			return -1;
		int value=0;
		try { value = m_input.readInt();}
		catch(IOException e){value=-1;};
		return value;
	}
	/**
	* Get a double from the other Robot using Bluetooth
	* @return The double read from another robot, or -1 if it failed.
	*/
	public double getDouble()
	{
		sleep(.05);
		if (m_bluetooth == null)
			return 0;
		double value=0.0;
		try { value = m_input.readDouble();}
		catch(IOException e){value=-1.0;};
		return value;
	}
	/**
	* Get a string from the other Robot using Bluetooth
	* @return The string read from another robot, or BAD if it failed.
	*/
	public String getString()
	{
		sleep(.05);
		if (m_bluetooth == null)
			return "BAD";
		String value="BAD";
		try
		{
			int size = m_input.readInt();
			byte[] svalue = new byte[size];
			m_input.read(svalue,0, size);
			value = new String(svalue,0, size);
		}
		catch (IOException e){}
		return value;
	}
	/**
	* Send an integer to the other Robot using Bluetooth
	* @param number The integer to send.
	*/
	public Boolean sendInt(int number)
	{
		if (m_bluetooth == null)
			return false;
		try 
		{
			m_output.writeInt(number);
			m_output.flush();
			sleep(.05);
			return true;
		}
		catch (IOException e){return false;}
	}
	/**
	* Send a double to the other Robot using Bluetooth
	* @param number The double to send.
	*/
	public Boolean sendDouble(double number)
	{
		if (m_bluetooth == null)
			return false;
		try 
		{
			m_output.writeDouble(number);
			m_output.flush();
			sleep(.05);
			return true;
		}
		catch (IOException e){return false;}
	}
	/**
	* Send a string to the other Robot using Bluetooth
	* @param text The string to send.
	*/
	public Boolean sendString(String text)
	{
		if (m_bluetooth == null)
			return false;
		try
		{
			byte[] svalue  = text.getBytes(text);
			m_output.writeInt(svalue.length);
			m_output.write(svalue, 0, svalue.length);
			m_output.flush();
			sleep(.05);
			return true;
		}
		catch (IOException e) {return false;}
	}
	/**
	* Connect to another Robot using its name
	* @param name  The name of the robot to connect to
	* @return Whether or not the connection succeeded
	*/
	public Boolean connectTo(String name)
	{
		RemoteDevice btrd = Bluetooth.getKnownDevice(name);
		if (btrd == null)
			return false;
		m_bluetooth = Bluetooth.connect(btrd);
		byte[] status = Bluetooth.getConnectionStatus();
		if (status == null)
		{
			m_bluetooth.close();
			return false;
		}
		m_input = m_bluetooth.openDataInputStream();
		m_output = m_bluetooth.openDataOutputStream();
		return true;	
	}
	/**
	* Find other Robots in the vicinity of your robot.
	* <p>Requires the other Robots to be powered on, and takes 15 seconds.</p>
	* <p>This function adds the Robots it found to the NXTs list of
	* devices it can connect to using Bluetooth</p>
	* @return Whether or not any Robots were found
	*/
	public Boolean findRobots()
	{
		zeroArrays();
		byte[] cod = {0,0,0,0};
		/*
		Vector devList = Bluetooth.inquire(5,10,cod);
		if (devList == null)
			return false;	
		for (int i=0; i< devList.size(); i++)	
		{
			RemoteDevice btrd = ((RemoteDevice) (devList.elementAt(i)));
			Bluetooth.addDevice(btrd);
			m_robotNames[i] = btrd.getFriendlyName(false);
		}
		if (devList.size() > 0)	
			return true;
		else 
			return false;
		*/
		return false;
	}
	/**
	* Remove all Robots from the internal cache.
	*/
	void removeRobots()
	{
	/*
		zeroArrays();
		Vector devices = Bluetooth.getKnownDevicesList();
		if (devices == null)
			return ;
		if (devices.size() < 1)
			return;
		for (int i=0; i<devices.size(); i++)
			Bluetooth.removeDevice((RemoteDevice)devices.elementAt(i));	
	*/
		Bluetooth.reset();
	}
	/**
	* Get all of the Robots that have been discovered using findRobots
	* @return An array containing the names you can pass to the connectTo function 
	*/
	String[] getRobotNames()
	{
	/*
		Vector devices = Bluetooth.getKnownDevicesList();
		if (devices == null)
			return null;
		if (devices.size() < 1)
			return null;
		*/
		//String[] names = new String[devices.size()];
		String[] names = new String[10];
		/*
		for (int i=0; i<devices.size(); i++)
		{
			RemoteDevice r = (RemoteDevice)(devices.elementAt(i));	
			names[i] = r.getFriendlyName(false);
		}
		*/
		return names;
	}
	/**
	* Get one of the Robots that have been discovered using findRobots
	* @param num The particular Robot's name 
	* @return The name of a particular Robot
	*/
	String getRobotName(int num)
	{
		if (num > -1 && num < m_robotNames.length)
			return m_robotNames[num];
		else
			return "No Robot found";
	}
	/**
	* Gets the name of your Robot
	* <p> This is used with the Bluetooth identification process</p>
	* @return The name of the Robot
	*/
	String getName()
	{
		return Bluetooth.getFriendlyName();
	}
	/**
	* Set the name of your Robot
	* <p> This is used with the Bluetooth identification process</p>
	* @param name the name of the Robot
	*/
	void setName(String name)
	{
		if (name == null)
			return;	
		Bluetooth.setFriendlyName(name);
	}
	//The LCD object
	LCD lcd;

	//Old code - Used as a 15 minute how does the robot
	//move and how can I control it based on that.
	//I wasn't aware of the pilot class when I wrote this :(
	//Robot moves 3.66667 inches in 1 second at speed 200
	final double m_inchesPerSecond = 3.66666666666666666666667;
	//Robot moves 87.8048... degrees per second
	final double m_numDegPerSecond = 87.804878049;

	//The  sensors
	TouchSensor      m_touch;
	LightSensor      m_light;
	UltrasonicSensor m_ultra;
	SoundSensor      m_sound;

	//Number of inches across for the wheel
	double wheelDiameter=2.0 + 3.0/16.0;
	double wheelSpace = 3.0 + 5.0/16.0;

	//Lejos method of going X feet , X degrees
	//I found my method was more accurate so
	//this is unused, but left in the code just
	//in case you prefer to use it
	LegacyPilot m_pilot;
	//temporary storage for sensor data
	//Currently 150 ints
	int[] m_array;
	//Currently 8 ints, the average of 64 distance measurements
	int[] m_results;
	//Currently 90 strings (30 students w/ 3 robots each!)
	String[] m_robotNames;

	//Beats per minute, for playing music
	double m_beatLength;
	//Type of insturment (none by default)
	int m_insturment;

	//Network connections for bluetooth client
	DataInputStream m_input;
	DataOutputStream m_output;
	BTConnection m_bluetooth;
	//Do you start w/ the left or right motor first?
	Boolean m_switch;
}