MAC0318/.metadata/.plugins/org.eclipse.core.resources/.history/37/20e590971cb80017145c9b6064c27648

package robots;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.util.LinkedList;

import lejos.pc.comm.NXTComm;
import lejos.pc.comm.NXTCommException;
import lejos.pc.comm.NXTCommFactory;
import lejos.pc.comm.NXTInfo;
import lejos.robotics.navigation.Pose;

public class BluetoothRobot implements Robot {
	private String name;
	private NXTComm nxtComm;
	
	public static final byte FORWARD = 0;
	public static final byte STOP = 1;
	public static final byte EXIT = 2;
	public static final byte LEFT = 3;
	public static final byte RIGHT = 4;
	public static final byte BACKWARD = 5;

	private DataOutputStream output;
	private DataInputStream input;
	private Send sendthread;
	private Receiver receivethread;
	
	private LinkedList<DataPose> data;
	
	private class Receiver extends Thread {
		public boolean run = true;
		@Override
		public void run() {
			int bytes_valiable = -1;
			
			while(run) {
				try {
					bytes_valiable = input.available();
				} catch (IOException e1) {
					// TODO Auto-generated catch block
					e1.printStackTrace();
				}
				if (bytes_valiable >= 0) {
					try {
						if (input.readByte() != '@') continue;
						int angle = input.readByte();
						float alpha = input.readFloat();
						float x = input.readFloat();
						int distance = input.readByte();
						float y = input.readFloat();
						DataPose d = new DataPose();
						d.setDistance(distance);
						d.setSensorAngle(angle);
						d.setPose(new Pose(x, y, alpha+90));
						
						data.add(d);
						
					} catch (IOException e1) {
						continue;
					}
				}
			}
		}
	}

	private class Send extends Thread {
		private int before;
		private int command;
		private boolean run = true;
		
		@Override
		public void run() {
			before = -1;
			command = STOP;
			
			while(run) {
				if (before != command) {
					try {
						System.out.println("Send cmd");
						output.write(command);
						output.flush();
						if (command == EXIT) run = false;
					} catch (IOException e1) {
						System.out.println("Erro send cmd");
						before = -1;
						send(command);
					}
					before = command;
				}
				
				try {
					Thread.sleep(50);
				} catch (InterruptedException e) {

				}
			}
		}
		public void send(int cmd) {
			command = cmd;
		}
	}
	
	
	BluetoothRobot (String name) {
		this.name = name;
		data = new LinkedList<DataPose>();
	}

	@Override
	public void moveForward() {
		sendthread.send(FORWARD);
	}

	@Override
	public void moveLeft() {
		sendthread.send(LEFT);
	}

	@Override
	public void moveRight() {
		sendthread.send(RIGHT);
		
	}

	@Override
	public void moveBackward() {
		sendthread.send(BACKWARD);
	}

	@Override
	public boolean connect () {
		try {
			nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
			NXTInfo[] nxtInfo = nxtComm.search(name); //find brick with NXT_ID by doing a Bluetooth inquiry
			if (nxtInfo.length == 0) { // failed to find a brick with the ID
				System.err.println("NO NXT found");
				return false;
			}
			if (!nxtComm.open(nxtInfo[0])) { // the brick was found but a connection could not be establish
				System.err.println("Failed to open NXT");
				return false;
			}
			
			input = new DataInputStream(nxtComm.getInputStream()); // open data input stream 
			output = new DataOutputStream(nxtComm.getOutputStream()); // open data output stream
		} catch (NXTCommException e) {
			return false;
		}
		
		receivethread.start();
		sendthread.start();
		
		return true;	
	}

	@Override
	public void stop() {
		sendthread.send(STOP);
	}

	@Override
	public void exit() {
		sendthread.send(EXIT);
		input.close();
		output.close();
		
	}

	@Override
	public DataPose getData() {
		return data.pollFirst();
	}

}