Boat should move towards optimal angle upwind and downwind when pressing spacebar (VMG)

#story[988]

src/main/java/seng302/gameServer/GameState.java

@@ -1,10 +1,11 @@
 package seng302.gameServer;
 
-import java.util.*;
+import java.util.ArrayList;
-
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
 import seng302.client.ClientPacketParser;
 import seng302.models.Player;
-
 import seng302.models.Yacht;
 import seng302.server.messages.BoatActionType;
 
@@ -27,7 +28,7 @@ public class GameState implements Runnable {
     private static GameStages currentStage;
     
     public GameState(String hostIpAddress) {
-        windDirection = 170d;
+        windDirection = 180d;
         windSpeed = 10000d;
         yachts = new HashMap<>();
         players = new ArrayList<>();
@@ -105,7 +106,6 @@ public class GameState implements Runnable {
             case VMG:
                 playerYacht.turnToVMG();
 //                System.out.println("Snapping to VMG");
-                // TODO: 22/07/17 wmu16 - Add in the vmg calculation code here
                 break;
             case SAILS_IN:
                 playerYacht.toggleSailIn();

src/main/java/seng302/models/PolarTable.java

@@ -4,8 +4,6 @@ import java.io.BufferedReader;
 import java.io.IOException;
 import java.io.InputStream;
 import java.io.InputStreamReader;
-import java.lang.reflect.Array;
-import java.util.ArrayList;
 import java.util.HashMap;
 
 /**
@@ -182,4 +180,5 @@ public final class PolarTable {
         return closestAngle;
     }
 
+
 }

src/main/java/seng302/models/Yacht.java

@@ -4,6 +4,7 @@ import static seng302.utilities.GeoUtility.getGeoCoordinate;
 
 import java.text.DateFormat;
 import java.text.SimpleDateFormat;
+import java.util.HashMap;
 import javafx.scene.paint.Color;
 import seng302.client.ClientPacketParser;
 import seng302.controllers.RaceViewController;
@@ -136,9 +137,9 @@ public class Yacht {
 
             if (velocity > 0d) {
                 if (maxBoatSpeed != 0d) {
-                    velocity -= maxBoatSpeed / 25;
+                    velocity -= maxBoatSpeed / 600;
                 } else {
-                    velocity -= velocity / 25;
+                    velocity -= velocity / 100;
                 }
                 if (velocity < 0) {
                     velocity = 0d;
@@ -164,8 +165,7 @@ public class Yacht {
     }
 
     public void tackGybe(Double windDirection) {
-        Double normalizedHeading = heading - GameState.windDirection;
+        Double normalizedHeading = normalizeHeading();
-        normalizedHeading = (double) Math.floorMod(normalizedHeading.longValue(), 360);
         adjustHeading(-2 * normalizedHeading);
     }
 
@@ -174,8 +174,7 @@ public class Yacht {
     }
 
     public void turnUpwind() {
-        Double normalizedHeading = heading - GameState.windDirection;
+        Double normalizedHeading = normalizeHeading();
-        normalizedHeading = (double) Math.floorMod(normalizedHeading.longValue(), 360);
         if (normalizedHeading == 0) {
             if (lastHeading < 180) {
                 adjustHeading(-TURN_STEP);
@@ -196,8 +195,7 @@ public class Yacht {
     }
 
     public void turnDownwind() {
-        Double normalizedHeading = heading - GameState.windDirection;
+        Double normalizedHeading = normalizeHeading();
-        normalizedHeading = (double) Math.floorMod(normalizedHeading.longValue(), 360);
         if (normalizedHeading == 0) {
             if (lastHeading < 180) {
                 adjustHeading(TURN_STEP);
@@ -218,10 +216,43 @@ public class Yacht {
     }
 
     public void turnToVMG() {
-        // TODO: 25/07/17 wmu16 - Fix this so it grabs the optimal value from the optimal Polar
+        Double normalizedHeading = normalizeHeading();
+        Double optimalHeading;
+        HashMap<Double, Double> optimalPolarMap;
+
+        if (normalizedHeading >= 90 && normalizedHeading <= 270) { // Downwind
+            optimalPolarMap = PolarTable.getOptimalDownwindVMG(GameState.getWindSpeedKnots());
+            optimalHeading = optimalPolarMap.keySet().iterator().next();
+        } else {
+            optimalPolarMap = PolarTable.getOptimalUpwindVMG(GameState.getWindSpeedKnots());
+            optimalHeading = optimalPolarMap.keySet().iterator().next();
+        }
+        // Take optimal heading and turn into correct
+        optimalHeading =
+            optimalHeading + (double) Math.floorMod(GameState.getWindDirection().longValue(), 360L);
+
+        turnTowardsHeading(optimalHeading);
+
     }
 
+    private void turnTowardsHeading(Double newHeading) {
+        System.out.println(newHeading);
+        if (heading < 90 && newHeading > 270) {
+            adjustHeading(-TURN_STEP);
+        } else {
+            if (heading < newHeading) {
+                adjustHeading(TURN_STEP);
+            } else {
+                adjustHeading(-TURN_STEP);
+            }
+        }
+    }
 
+    private Double normalizeHeading() {
+        Double normalizedHeading = heading - GameState.windDirection;
+        normalizedHeading = (double) Math.floorMod(normalizedHeading.longValue(), 360L);
+        return normalizedHeading;
+    }
 
     public String getBoatType() {
         return boatType;