MAC0318/.metadata/.plugins/org.eclipse.core.resources/.history/27/707c77822cb80017145c9b6064c27648

package robots;

import lejos.robotics.mapping.LineMap;
import lejos.robotics.navigation.Pose;

public class VirtualRobot implements Robot {
	private Pose pose;
	private Simulate simthread;
	private RobotReturn rr;
	
	private class Simulate extends Thread {
		public boolean run = true;
		public void run() {
			int ang = 0;
			int add = 5;
			while(run) {
				DataPose data = new DataPose();
				data.setDistance(60);
				data.setPose(pose);
				data.setSensorAngle(ang);
				
				rr.robotData(data);
				
				ang += add;
				if (ang == -90 || ang == 90) add *= -1;
				
				try {
					Thread.sleep(100);
				} catch (InterruptedException e) {
				}
			}
		}
		
	}

	public VirtualRobot(LineMap map) {
		simthread = new Simulate();
		pose = new Pose();
		pose.setHeading(90);
	}

	@Override
	public void moveForward() {
		double dx = Math.sin(Math.toRadians(pose.getHeading()))*5;
		double dy = Math.cos(Math.toRadians(pose.getHeading()))*5;
		pose.translate((float)dx, (float)dy);
	}

	@Override
	public void moveLeft() {
		pose.rotateUpdate(45);
	}

	@Override
	public void moveRight() {
		pose.rotateUpdate(-45);
	}

	@Override
	public void moveBackward() {
		double dx = Math.sin(Math.toRadians(pose.getHeading()))*5;
		double dy = Math.cos(Math.toRadians(pose.getHeading()))*5;
		pose.translate((float)-dx, (float)-dy);
	}

	@Override
	public boolean connect(RobotReturn r) {
		rr = r;
		simthread.start();
		return true;
	}

	@Override
	public void stop() {		
	}

	@Override
	public void exit() {
		simthread.run = false;
		try {
			simthread.join();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
	}

}