Finished path navigation

Change-Id: Ibfba63ac41ae9e701851be58eff6ec984ae12e72
Signed-off-by: Ricky Barrette <rickbarrette@gmail.com>

IOIOTruck/src/com/TwentyCodes/android/IOIOTruck/MapFragment.java

@@ -76,7 +76,29 @@ public class MapFragment extends UserOverlayMapFragment implements OnLocationSel
 	 * @see com.TwentyCodes.android.location.LocationSelectedListener#onLocationSelected(com.google.android.maps.GeoPoint)
 	 */
 	@Override
-	public void onLocationSelected(GeoPoint point) {
+	public void onLocationSelected(final GeoPoint point) {
+		
+		if(mDirectionsCompleteListener != null)
+			new Thread( new Runnable(){
+				@Override
+				public void run(){
+					try {
+						new DirectionsOverlay(getMap(), getUserLocation(), point, MapFragment.this);
+					} catch (IllegalStateException e) {
+						// TODO Auto-generated catch block
+						e.printStackTrace();
+					} catch (ClientProtocolException e) {
+						// TODO Auto-generated catch block
+						e.printStackTrace();
+					} catch (IOException e) {
+						// TODO Auto-generated catch block
+						e.printStackTrace();
+					} catch (JSONException e) {
+						// TODO Auto-generated catch block
+						e.printStackTrace();
+					}
+				}
+			}).start();
 		
 		removePath();
 		
@@ -118,36 +140,6 @@ public class MapFragment extends UserOverlayMapFragment implements OnLocationSel
 			mDirectionsOverlay.removePath();
 	}
 	
-	/**
-	 * (non-Javadoc)
-	 * @see com.TwentyCodes.android.fragments.UserOverlayMapFragment#setDestination(com.google.android.maps.GeoPoint)
-	 */
-	@Override
-	public void setDestination(final GeoPoint destination) {	
-		if(mDirectionsCompleteListener != null)
-			new Thread( new Runnable(){
-				@Override
-				public void run(){
-					try {
-						new DirectionsOverlay(getMap(), getUserLocation(), destination, MapFragment.this);
-					} catch (IllegalStateException e) {
-						// TODO Auto-generated catch block
-						e.printStackTrace();
-					} catch (ClientProtocolException e) {
-						// TODO Auto-generated catch block
-						e.printStackTrace();
-					} catch (IOException e) {
-						// TODO Auto-generated catch block
-						e.printStackTrace();
-					} catch (JSONException e) {
-						// TODO Auto-generated catch block
-						e.printStackTrace();
-					}
-				}
-			}).start();
-		super.setDestination(destination);
-	}
-	
 	/**
 	 * sets the distener for the directions overlay
 	 * @param listener

IOIOTruck/src/com/TwentyCodes/android/IOIOTruck/NavigationActivity.java

@@ -77,15 +77,13 @@ public class NavigationActivity extends FragmentActivity implements CompassListe
 		 * aborts the thread 
 		 * @author ricky barrette
 		 */
-		public void abort() {
+		public synchronized void abort() {
 			isAborted = true;
 		}
 		
 		@Override
 		public void run(){
-			while (true) {
+			while (!isAborted) {
-					if (isAborted) 
-						break;
 					updateLog("\nDistance: "+ mDistance +getString(R.string.m)
 							+"\nDrive: "+mIOIOManager.getDriveValue()
 							+"\nSteering: "+mIOIOManager.getSteerValue()
@@ -93,9 +91,7 @@ public class NavigationActivity extends FragmentActivity implements CompassListe
 							+"\nisRunning: "+isRunning);
 							if(mPoints != null)
 								updateLog("Point = "+mIndex +" of "+ mPoints.size());
-					
 							updateLastUpdateTextView();
-					
 					try {
 						sleep(1000);
 					} catch (InterruptedException e) {
@@ -124,9 +120,7 @@ public class NavigationActivity extends FragmentActivity implements CompassListe
 	private IOIOTruckManager mIOIOManager;
 	private MapFragment mMap;
 	private TextView mLog;
-	private GeoPoint mPoint;
 	private ProgressBar mProgress;
-	private int mMaxDistance = 0; //meters
 	private boolean isRunning = false;
 	private Button mGoButton;
 	private float mBearing;
@@ -138,7 +132,7 @@ public class NavigationActivity extends FragmentActivity implements CompassListe
 	private TextView mLastUpdateTextView;
 	private long mLast;
 	private WakeLock mWakeLock;
-	private int mCount;
+	private int mCount = 0;
 	private ArrayList<GeoPoint> mPoints;
 	private int mIndex = 0;
 	private GeoPoint mDestPoint;
@@ -171,7 +165,7 @@ public class NavigationActivity extends FragmentActivity implements CompassListe
 				break;
 				
 			case R.id.mark_my_lcoation_button:
-				GeoPoint point = mMap.getUserLocation();
+				final GeoPoint point = mMap.getUserLocation();
 				
 				if(point != null){
 					mMap.onLocationSelected(point);
@@ -180,7 +174,7 @@ public class NavigationActivity extends FragmentActivity implements CompassListe
 				break;
 				
 			case R.id.my_location_button:
-				GeoPoint user = mMap.getUserLocation();
+				final GeoPoint user = mMap.getUserLocation();
 				
 				if(user != null){
 					mMap.setMapCenter(user);
@@ -198,7 +192,7 @@ public class NavigationActivity extends FragmentActivity implements CompassListe
 	 */
 	@Override
 	public void onCompassUpdate(float bearing) {
-		bearing = GeoUtils.calculateBearing(mMap.getUserLocation(), mPoint, bearing);
+		bearing = GeoUtils.calculateBearing(mMap.getUserLocation(), mMap.getDestination(), bearing);
 
 		if(bearing > 355 || bearing < 5)
 			mIOIOManager.setSteerValue(IOIOTruckValues.STEER_STRAIGHT);
@@ -261,9 +255,8 @@ public class NavigationActivity extends FragmentActivity implements CompassListe
 				}
 		
 			mPoints = points;
-			mPoint = points.get(0);
+			mMap.setDestination(points.get(0));
-			mMap.setDestination(mPoint);
+			mWayPoints.add(new PathOverlay(points.get(0), 5, Color.MAGENTA));
-			mWayPoints.add(new PathOverlay(mPoint, 5, Color.MAGENTA));
 			mMap.getMap().getOverlays().addAll(mWayPoints);
 			mWayPoints.addAll(path);
 		}
@@ -280,33 +273,43 @@ public class NavigationActivity extends FragmentActivity implements CompassListe
 	 * @see com.TwentyCodes.android.location.GeoPointLocationListener#onLocationChanged(com.google.android.maps.GeoPoint, int)
 	 */
 	@Override
-	public synchronized void onLocationChanged(GeoPoint point, int accuracy) {
+	public synchronized void onLocationChanged(final GeoPoint point, final int accuracy) {
 		mLast = System.currentTimeMillis();
 		mAccuracyTextView.setText(accuracy+getString(R.string.m));
-		
 		mDistance = updateProgress(point);
+		final GeoPoint currentDest = mMap.getDestination();
 		
 		/*
 		 * if we have a destination, check to see if we are there yet
+		 * if we are then increment mCount
 		 */
-		if(mPoint != null)
+		if(point != null)
-			if(GeoUtils.isIntersecting(point, (float) (accuracy / 1E3), mPoint, Debug.RADIUS, Debug.FUDGE_FACTOR)) {
+			if(currentDest != null)
-				mCount++;
+				
 				
 				/*
-				 * if we get 5 positives, we are at our waypoint/dest
+				 * are we closer than 15 feet?
 				 */
-				if(mCount > 5){
+				if (GeoUtils.distanceKm(point, currentDest) < 0.009144) {
+//				if(GeoUtils.isIntersecting(point, (float) (accuracy / 1E3), currentDest, Debug.RADIUS, Debug.FUDGE_FACTOR)) {
+					updateLog("Count = "+ (++mCount));
+					/*
+					 * if we get 6 positives, we are problay at our waypoint/dest
+					 */
+					if(mCount == 6){
+						
+						mCount = 0;
 						
 						/*
-					 * if ther points list is null, or there are no more points
+						 * if the points list is null, or there are no more waypoints
 						 */
 						if(mPoints == null || mIndex == mPoints.size()){
 							mIOIOManager.setDriveValue(IOIOTruckValues.DRIVE_STOP);
 							updateGoButton(true);
 							updateLog(R.string.dest_reached);
+							mMap.setDestination(null);
 						} else {
-						mIndex ++;
+								updateLog("Index = " + (++mIndex));
 							
 							/*
 							 * if there are more waypoints, then move on to the next
@@ -314,25 +317,23 @@ public class NavigationActivity extends FragmentActivity implements CompassListe
 							 */
 							if(mIndex < mPoints.size()) {
 								updateLog("Waypoint reached, moving to next");
-							mPoint = mPoints.get(mIndex);
+								mMap.setDestination(mPoints.get(mIndex));
 							} else {
 								updateLog("last Waypoint reached, moving to dest");
-							mPoint = mDestPoint;
+								mMap.setDestination(mDestPoint);
 							}
 							
-						/*
+							updateLog("New dest = "+ mMap.getDestination().toString());
-						 * we have to notify the compass that the point has changed
-						 */
-						mMap.setDestination(mPoint);
 						}
 					}
+					
 				} else {
 					Log.v(TAG, "Driving Forward");
 					mCount = 0;
 					mIOIOManager.setDriveValue(IOIOTruckValues.DRIVE_FORWARD);
 				}
-		else{
+		else {
-			Log.v(TAG, "Lost GPS signal, stopping");
+			updateLog("Lost GPS signal (point was null), stopping");
 			mIOIOManager.setDriveValue(IOIOTruckValues.DRIVE_STOP);
 		}
 
@@ -487,13 +488,10 @@ public class NavigationActivity extends FragmentActivity implements CompassListe
 	 * @author ricky barrette
 	 */
 	private int updateProgress(GeoPoint point) {
-		int distance = (int) (GeoUtils.distanceKm(point, mPoint) * 1000);
+		int distance = (int) (GeoUtils.distanceKm(point, mMap.getDestination()) * 1000);
-		if (distance > mMaxDistance) {
+		if (distance > mProgress.getMax())
-			mMaxDistance = distance;
 			mProgress.setMax(distance);
-		}		
 		mProgress.setProgress(distance);
 		return distance;
 	}
-	
 }