Merge remote-tracking branch 'origin/story33-estimateTime' into sp4_s31_cleanups

# Conflicts:
#	mock/src/main/java/seng302/Model/Race.java
Resolved.

mock/src/main/java/seng302/Model/Race.java

@@ -240,6 +240,28 @@ public class Race implements Runnable {
         return new GPSCoordinate(endPoint.getY(), endPoint.getX());
     }
 
+    private VMG calculateHeading(Boat boat) {
+        //How fast a boat can turn, in degrees per millisecond.
+        double turnRate = 0.03;
+
+        //How much the boat is allowed to turn, considering how long since it last turned.
+        double turnAngle = turnRate * boat.getTimeSinceTackChange();
+
+        //Find the bounds on what angle the boat is allowed to travel at. The bounds cap out at [0, 360).
+        double bound1 = Math.max(boat.getHeading() - turnAngle, 0);
+        double bound2 = Math.min(boat.getHeading() + turnAngle, 360);
+
+        return boat.getPolars().calculateVMG(this.windDirection, this.windSpeed, boat.calculateBearingToDestination(), bound1, bound2);
+    }
+
+    private boolean improvesVelocity(Boat boat, VMG newHeading) {
+        double angleBetweenDestAndHeading = boat.getHeading() - boat.calculateBearingToDestination();
+        double angleBetweenDestAndNewVMG = newHeading.getBearing() - boat.calculateBearingToDestination();
+        double currentVelocity = cos(Math.toRadians(angleBetweenDestAndHeading)) * boat.getVelocity();
+        double vmgVelocity = cos(Math.toRadians(angleBetweenDestAndNewVMG)) * newHeading.getSpeed();
+        return vmgVelocity > currentVelocity;
+    }
+
     /**
      * Calculates the distance a boat has travelled and updates its current position according to this value.
      *
@@ -250,55 +272,26 @@ public class Race implements Runnable {
 
         //distanceTravelled = velocity (nm p hr) * time taken to update loop
         double distanceTravelled = (boat.getCurrentSpeed() * this.scaleFactor * millisecondsElapsed) / 3600000;
-        double totalDistanceTravelled = distanceTravelled + boat.getDistanceTravelledInLeg();
+        double totalDistanceTravelled;
 
         boolean finish = boat.getCurrentLeg().getName().equals("Finish");
         if (!finish) {
-
-
             double totalDistanceTravelledInTack = distanceTravelled;//TODO FIX// + boat.getDistanceTravelledInTack();
 
 
-
             boat.setTimeSinceTackChange(boat.getTimeSinceTackChange() + this.scaleFactor * millisecondsElapsed);
 
-
+            VMG newHeading = calculateHeading(boat);
-            //How fast a boat can turn, in degrees per millisecond.
-            double turnRate = 0.03; //Roughly 30 per second, or 12 seconds per revolution.
-
-            //How much the boat is allowed to turn, considering how long since it last turned.
-            double turnAngle = turnRate * boat.getTimeSinceTackChange();
-
-
-            //Find the bounds on what angle the boat is allowed to travel at.
-            double bound1 = boat.getHeading() - turnAngle;
-            double bound2 = boat.getHeading() + turnAngle;
-
-
-            //The bounds cap out at [0, 360).
-            bound1 = Math.max(bound1, 0);
-            bound2 = Math.min(bound2, 360);
 
             //Calculate the new VMG.
-            VMG newHeading = boat.getPolars().calculateVMG(this.windDirection, this.windSpeed,
-                    boat.calculateBearingToDestination(), bound1, bound2);
 
 
-            //Is this new VMG better than the current VMG?
+            if (improvesVelocity(boat, newHeading)) {
-
-            double angleBetweenDestAndHeading = boat.getHeading() - boat.calculateBearingToDestination();
-            double angleBetweenDestAndNewVMG = newHeading.getBearing() - boat.calculateBearingToDestination();
-            double currentVelocity = cos(Math.toRadians(angleBetweenDestAndHeading)) * boat.getCurrentSpeed();
-            double vmgVelocity = cos(Math.toRadians(angleBetweenDestAndNewVMG)) * newHeading.getSpeed();
-
-            if (vmgVelocity > currentVelocity) {
                 boat.setHeading(newHeading.getBearing());
                 boat.setCurrentSpeed(newHeading.getSpeed());
                 boat.setTimeSinceTackChange(0);
             }
 
-
-
             double azimuth = boat.getHeading();
             if (azimuth > 180) {
                 azimuth = azimuth - 360;
@@ -310,23 +303,16 @@ public class Race implements Runnable {
                 boat.setHeading(tempHeading);
             }
 
-
-
             //calc the distance travelled in a straight line to windward
             //double angleBetweenDestAndHeading = boat.getHeading() - boat.calculateBearingToDestination();
-            totalDistanceTravelled = cos(Math.toRadians(angleBetweenDestAndHeading))*totalDistanceTravelledInTack;
+            totalDistanceTravelled = cos(Math.toRadians(boat.getHeading() - boat.calculateBearingToDestination()))*totalDistanceTravelledInTack;
             boat.setDistanceTravelledInLeg(totalDistanceTravelled);
 
 
             //Calculate boat's new position by adding the distance travelled onto the start point of the leg
             azimuth = boat.getHeading();
-            if (azimuth > 180) {
+            azimuth = azimuth > 180? azimuth - 360 : azimuth;
-                azimuth = azimuth - 360;
+            boat.setCurrentPosition(calculatePosition(boat.getCurrentPosition(), totalDistanceTravelledInTack, azimuth));
-            }
-            boat.setCurrentPosition(calculatePosition(boat.getCurrentPosition(),
-                    totalDistanceTravelledInTack, azimuth));
-
-
         }
     }