Added testing for the vmg autopilot

tags: #story[1107]

src/test/java/seng302/models/YachtTest.java

@@ -7,6 +7,7 @@ import org.junit.AfterClass;
 import org.junit.BeforeClass;
 import org.junit.Test;
 import seng302.gameServer.GameState;
+import seng302.model.PolarTable;
 import seng302.model.Yacht;
 
 
@@ -14,7 +15,7 @@ public class YachtTest {
 
     private static Yacht y1;
     //Yacht y2;
-    private static Double windDirection = 45d;
+    private static Double windDirection = 180d;
     private static Double windSpeed = 20d;
     private static GameState gs;
 
@@ -44,6 +45,44 @@ public class YachtTest {
 
     }
 
+    @Test
+    public void vmgTest() {
+
+        PolarTable.parsePolarFile(getClass().getResourceAsStream("/config/acc_polars.csv"));
+        Double upwind = PolarTable.getOptimalUpwindVMG(windSpeed).keySet().iterator().next();
+        Double downwind = PolarTable.getOptimalDownwindVMG(windSpeed).keySet().iterator().next();
+
+        HashMap<Double, Double> values = new HashMap<>();
+
+        upwind = (double) Math.floorMod(upwind.longValue() + windDirection.longValue(), 360L);
+        Double upwindRight = upwind;
+        Double upwindLeft = 360 - upwindRight;
+        downwind = (double) Math.floorMod(downwind.longValue() + windDirection.longValue(), 360L);
+        Double downwindRight = downwind;
+        Double downwindLeft = 360 - downwindRight;
+
+        System.out.println(
+            String.format("%f %f %f %f", upwindLeft, upwindRight, downwindLeft, downwindRight));
+
+        values.put(190d, upwindRight);
+        values.put(170d, upwindLeft);
+        values.put(10d, downwindLeft);
+        values.put(350d, downwindRight);
+
+        for (Double begin : values.keySet()) {
+            System.out.println(begin);
+            y1.setHeading(begin);
+            y1.turnToVMG();
+            for (int i = 0; i < 50; i++) {
+                y1.runAutoPilot();
+                System.out.println(y1.getHeading());
+            }
+            y1.disableAutoPilot();
+            assertEquals(values.get(begin), y1.getHeading(), 5.0);
+        }
+
+    }
+
 
     @AfterClass
     public static void tearDown() {