Got VMG quad detection working, however, the speeds are completley wrong. #story[1186]

racevisionGame/src/main/java/mock/model/Polars.java

@@ -23,7 +23,7 @@ public class Polars {
     private HashMap<Double, List<Bearing>> polarAngles = new HashMap<>();
 
     //true wind speed, <true wind angle, best boat angle>
-    private HashMap<Double, HashMap<Double, Double>> polars = new HashMap<>();
+    private Map<Double, HashMap<Double, Double>> polars = new TreeMap<>();
 
 
 
@@ -87,7 +87,7 @@ public class Polars {
         polars.get(tws).put(twa, bs);
     }
 
-    public double getClosest(double value, Set<Double> set){
+    private static double getClosest(double value, Set<Double> set){
         double closestVal = 0;
         double smallestDiff = Double.MAX_VALUE;
         for (double d: set){
@@ -100,6 +100,39 @@ public class Polars {
         return closestVal;
     }
 
+    /**
+     * Determines which quadrant degrees are in
+     *           0/360 Degrees
+     * Quadrant 4 | Quadrant 1
+     * -----------------------
+     * Quadrant 3 | Quadrant 2
+     * @param degrees
+     * @return
+     */
+    private static int getQuadrant(double degrees){
+        return ((int) degrees % 360) / 90 + 1;
+    }
+
+    private static double getBestSpeedInQuadrant(int quad, HashMap<Double, Double> set){
+        double quadVal = (double)((quad - 1) % 2);// this is as the hash map only has values 0 - 180
+        double min = quadVal* 90;
+        double max = (quadVal + 1) * 90;
+        double maxAngle = 0;
+        double maxSpeed = 0;
+        for (Double s: set.keySet()){
+            if (s >= min && s < max){
+                if (set.get(s) > maxSpeed){
+                    maxAngle = s;
+                    maxSpeed = set.get(s);
+                }
+            }
+        }
+        if (quad > 2){
+            maxAngle += 180;
+        }
+        return maxAngle;
+    }
+
     /**
      *
      * @param trueWindAngle
@@ -111,13 +144,15 @@ public class Polars {
         //speed
         double closestSpeed = getClosest(trueWindSpeed, polars.keySet());
 
-        //not using true vmg using general concept
+        //not using true vmg, using general concept
         double angle = Math.abs(trueWindAngle.degrees() - boatAngle.degrees());
 
-        double closestAngle = getClosest(angle, polars.get(closestSpeed).keySet());
+        int quad = getQuadrant(boatAngle.degrees());
+        double bestAngle = getBestSpeedInQuadrant(quad, polars.get(closestSpeed));
+
+        Bearing vmgAngle = Bearing.fromDegrees((bestAngle + trueWindSpeed) % 360);
+        double boatSpeed = Math.abs(trueWindSpeed * Math.cos(vmgAngle.radians())) * 4;
 
-        Bearing vmgAngle = Bearing.fromDegrees(closestAngle);
-        double boatSpeed = Math.abs(closestSpeed * Math.cos(vmgAngle.radians())) * 4;
         return new VMG(boatSpeed, vmgAngle);
     }