package search;

import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.addon.tetrix.TetrixMotorController;
import lejos.util.Delay;
import lejos.util.Timer;
import lejos.util.TimerListener;
import main.Drive;

public class SquareMapping {

	// The drive is global
	private Drive marvin;
	private int map[] = new int[2]; // 0 = x-coord biggest, 1 = y-coord biggest
	
	// Init
	public SquareMapping(Drive drive) {
		
		marvin = drive;
		
	}
	
	public void goNinty(){
		TimerListener el = null;
		Timer ts = new Timer(0, el);
		ts.start();
		marvin.setWheels(-10, 10);
		while(ts.getDelay() < 525){}
		marvin.setWheels(0, 0);
	}
	
	public void sweepinSquares(){
			findCenter(findPerimeter());
			
		}
	

	public int[] findPerimeter(){ //Finds Perimeter then resets to origin; map = [0]xmax, [1]ymax
		Timer checker = new Timer(0, null);
		Timer time = new Timer(0, null);

		
		
			// Check if corner
			for(int i = 0; i<2; i++){ //find up and left
				checker.setDelay(0);
				marvin.setWheels(10, 10);
				checker.start();
				marvin.waitForBumperPress();
				checker.stop();
				map[i] = checker.getDelay();
			}
			for(int i = 0; i<2; i++){
				while(map[i] > time.getDelay()){
					goNinty(); 
				}
			}
			return map;
		}
	
	public int[] findCenter(int[] map){ //returns center[]; center[0] = x coord, center[1] = y coord
		int[] center = new int[1];
		
		center[0] = map[0]/2;
		center[1] = map[1]/2;

		return center;
	}
	
	public void spinCircles(){
		RoomMappingA rma = new RoomMappingA(marvin);
		int area = map[0] * map[1]; 
		rma.mapping(0, area);
	}
}

@@ -86,148 +86,74 @@
 jos.
-nxt.addon.tetrix.TetrixMotorController;%0Aimport lejos.util.Delay;%0Aimport lejos.util.Timer;%0Aimport lejos.util.TimerListener;%0Aimport main.Drive
+robotics.navigation.DifferentialPilot;%0Aimport lejos.util.Stopwatch
 ;%0A%0Ap
@@ -215,20 +215,32 @@
 rivate D
-rive
+ifferentialPilot
  marvin;
@@ -253,127 +253,153 @@
 ate 
-int map%5B%5D = new int%5B2%5D; // 0 = x-coord biggest, 1 = y-coord biggest%0A%09%0A%09// Init%0A%09public SquareMapping(Drive drive
+RoomMappingA rma;%0A%09TouchSensor ta;%0A%09TouchSensor tb;%0A%09%0A%09%0A%09// Init%0A%09public SquareMapping(DifferentialPilot dp, TouchSensor ts1, TouchSensor ts2
 ) %7B%0A
-%09%09%0A
 %09%09ma
@@ -410,13 +410,79 @@
  = d
-rive;
+p;%0A%09%09ta = ts1;%0A%09%09tb = ts2;%0A%09%09rma = new RoomMappingA(marvin, ta, tb);%0A%0A%09
 %0A%09%09%0A
@@ -512,16 +512,39 @@
 ty()%7B%0A%09%09
+marvin.rotate(90);%0A%09%09/*
 TimerLis
@@ -691,16 +691,21 @@
 (0, 0);%0A
+%09%09*/%0A
 %09%7D%0A%09%0A%09pu
@@ -738,20 +738,20 @@
 %7B%0A%09%09
-%09findCenter(
+int%5B%5D map = 
 find
@@ -761,22 +761,58 @@
 imeter()
-)
 ;%0A%09%09%09
+findCenter(map);%0A%09%09%09spinCircles(map);
 %0A%09%09%7D%0A%09%0A%0A
@@ -912,82 +912,65 @@
 x%0A%09%09
-Timer checker = new Timer(0, null);%0A%09%09Timer time = new Timer(0, null);%0A%0A%09%09
+Stopwatch sw = new Stopwatch();%0A%09%09int map%5B%5D = new int%5B2%5D;
 %0A%09%09%0A
@@ -1047,84 +1047,46 @@
 %09%09%09%09
-checker.setDelay(0);%0A%09%09%09%09marvin.setWheels(10, 10);%0A%09%09%09%09checker.start
+sw.reset();%0A%09%09%09%09marvin.forward
 ();%0A%09%09%09%09
 marv
@@ -1081,22 +1081,19 @@
 ();%0A%09%09%09%09
+r
 ma
-rvin
 .waitFor
@@ -1110,135 +1110,84 @@
 s();
-%0A%09%09%09%09checker.stop();%0A%09%09%09%09map%5Bi%5D = checker.getDelay();%0A%09%09%09%7D%0A%09%09%09for(int i = 0; i%3C2; i++)%7B%0A%09%09%09%09while(map%5Bi%5D %3E time.getDelay())%7B%0A%09%09
+ %0A%09%09%09%09map%5Bi%5D = sw.elapsed();%0A%09%09%09%09goNinty(); %0A%09%09%09%7D%0A%09%09%09marvin.travel(map%5B0%5D);%0A
 %09%09%09g
@@ -1199,20 +1199,34 @@
 y();
- 
 %0A%09%09%09
-%09%7D%0A%09%09%09%7D
+marvin.travel(map%5B1%5D);
 %0A%09%09%09
@@ -1269,24 +1269,25 @@
 Center(int%5B%5D
+ 
  map)%7B //ret
@@ -1474,57 +1474,19 @@
 les(
-)%7B%0A%09%09RoomMappingA rma = new RoomMappingA(marvin);
+int%5B%5D map)%7B
 %0A%09%09i

Update SquareMapping.java