package nav;
import communication.*;
import BasicDataType.*;
import lejos.nxt.Motor;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.TachoPilot;
import java.util.ArrayList;
public class Nav implements INav,CommunicationListener {
private IPositioning positioning;
private ITravel travel;
private ISearch search;
private Pilot pilot;
private Map map;
private Communicator comm;
//private Sender send;
//private Rect boundRect;
public static final int MapSize = 10;
private static final float RESOLUTION = 300;
//Just for testing.
private Rect bound;
private Position initPointPosition = new Position();
public Rect boundRect[] = new Rect[4];
public Vector initPoint[] = new Vector[4];
public Nav(){
boundRect[0] = new Rect(0,4,4,0);
boundRect[3] = new Rect(5,9,9,5);
boundRect[2] = new Rect(0,9,4,5);
boundRect[1] = new Rect(5,4,9,0);
initPoint[0] = new Vector(0,0);
initPoint[3] = new Vector(5,5);
initPoint[2] = new Vector(0,5);
initPoint[1] = new Vector(5,0);
/*
boundRect[0] = new Rect(1,5,5,1);
boundRect[1] = new Rect(6,5,10,1);
boundRect[2] = new Rect(1,10,5,6);
boundRect[3] = new Rect(6,10,10,6);
initPoint[0] = new Vector(1,1);
initPoint[1] = new Vector(6,1);
initPoint[2] = new Vector(1,6);
initPoint[3] = new Vector(6,6);
*/
pilot = new TachoPilot(56,112,Motor.B,Motor.C,false);
pilot.setMoveSpeed(100);
pilot.setTurnSpeed(50);
map = new Map((byte)MapSize);
search = new Search(map);
comm = Communicator.createStandardCommunicator(map);
comm.addListener(this);
initPointPosition.setPosition(initPoint[comm.GetID()], 0);
positioning = new Positioning(RESOLUTION,initPointPosition);
travel = new Travel(RESOLUTION, map);
bound = new Rect(0, MapSize -1 , MapSize - 1,0);
}
public ArrayList<MapUpdate> getMap(){
return map.getVisitedSquares();
}
/* (non-Javadoc)
* @see nav.INav#init()
*/
public boolean init(){
bound = boundRect[comm.GetID()];
return true;
}
/**
* when the whole map searched
* @return
*/
private boolean end(){
for (int i = 0; i < MapSize; i++) {
for (int j = 0; j < MapSize; j++) {
if (map.getSquareDifference((byte)i, (byte)j) == 0) {
return false;
}
}
}
return true;
}
/**
* finish search the aera
* @return
*/
private boolean finishBound(){
for (int i = bound.left; i <= bound.right; i++) {
for (int j = bound.bottom; j <= bound.top; j++) {
if (map.getSquareDifference((byte)i, (byte)j) == 0) {
return false;
}
}
}
return true;
}
public void RecreateComm(int value){
System.out.println(value);
if (value == 2) {
comm = new Communicator(false,comm.getMap());
comm.addListener(this);
} else if(value == 3){
comm= new Communicator(true,comm.getMap());
comm.addListener(this);
}
}
/* (non-Javadoc)
* @see nav.INav#run()
*/
public boolean run(){
System.out.println("Run~!");
//int ComReturn = 0;
while (finishBound() == false) {
positioning.UpdatePosition(pilot);
Position temp = new Position();
temp = search.getNext(positioning.getPosition(), map, bound);
if (temp == null) {
while (end() == false) {
bound.setRect(0,MapSize - 1, MapSize - 1,0);
positioning.UpdatePosition(pilot);
Position temp1 = new Position();
temp1 = search.getNext(positioning.getPosition(), map, bound);
RecreateComm(travel.targetDestination(positioning.getPosition(), temp1, pilot, comm));
}
return true;
}
System.out.println(temp.pos.x + " " + temp.pos.y + " " + temp.dir);
RecreateComm(travel.targetDestination(positioning.getPosition(), temp, pilot, comm));
}
System.out.println("Area Finished");
while (end() == false) {
bound.setRect(0,MapSize - 1, MapSize - 1,0);
positioning.UpdatePosition(pilot);
Position temp = new Position();
temp = search.getNext(positioning.getPosition(), map, bound);
RecreateComm(travel.targetDestination(positioning.getPosition(), temp, pilot, comm));
}
return true;
}
/**when it finish run
* @return
*/
private boolean finish(){
return true;
}
public void printMap(){
ArrayList<MapUpdate> mapn = map.getVisitedSquares();
if(mapn!=null){
for(int i = 0;i<mapn.size();i++){
System.out.println("Coord"+(mapn.get(i)).getCoordX()+" "+(mapn.get(i)).getCoordY());
}
}else{
System.out.println("empty map");
}
}
public void MasterDivide() {
Direction coord1 = new Direction((byte) 0, (byte) 0, (byte) 0);
Direction coord2 = new Direction((byte) 4, (byte) 9, (byte) 0);
comm.sendDirection(coord1, coord2);
}
@Override
//needed for comm.addListener(this)
//TODO add check so that the map updated is delayed if search currently uses the maps
public void receiveMapUpdate(MapUpdate update) {
System.out.println("Receive");
if(update.getExpValue() > 0) {
map.propagateOccupied(update.getCoordX(),update.getCoordY() );
} else if(update.getExpValue() < 0) {
map.propagateEmpty(update.getCoordX(),update.getCoordY() );
} else
System.out.println("Error in receiveMapUpdate(), explorationValue = 0!");
}
@Override
public void receiveDirection(Direction coord1, Direction coord2) {
// TODO Auto-generated method stub
if (coord1.getId() == comm.GetID()) {
bound.bottom = coord1.getCoordY();
bound.left = coord1.getCoordX();
bound.right = coord2.getCoordX();
bound.top = coord2.getCoordY();
}
}
}