VMG now works with some edge cases issues #story[1182]

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

@@ -2,6 +2,7 @@ package mock.model;
 
 import shared.model.Bearing;
 
+import java.util.HashMap;
 import java.util.Map;
 import java.util.Set;
 import java.util.TreeMap;
@@ -28,7 +29,9 @@ public class NewPolars {
         if (!polars.containsKey(tws)){
             polars.put(tws, new TreeMap<>());
         }
-        polars.get(tws).put(twa, bs);
+        polars.get(tws).putIfAbsent(twa, bs);
+        polars.get(tws).putIfAbsent(360d - twa, bs);
+        System.out.println(String.format("tws %f, twa %f, bs %f", tws, twa, bs));
     }
 
     public static void linearInterpolatePolars(){
@@ -38,8 +41,15 @@ public class NewPolars {
 //                maxTWS = key;
 //            }
 //        }
+        /*for (double windSpeed: polars.keySet()){
+            if (!polars.get(windSpeed).containsKey(180d)){
+                polars.get(windSpeed).put(180d, windSpeed);
+            }
+        }*/
         TreeMap<Double, Double> prevTWS = null;
-        for (TreeMap<Double, Double> tws: polars.values()){
+        TreeMap<Double, TreeMap<Double, Double>> iterablePolars = new TreeMap<>(polars);
+        for (double windSpeed: iterablePolars.keySet()){
+            TreeMap<Double, Double> tws = iterablePolars.get(windSpeed);
             if (prevTWS == null){
                 prevTWS = tws;
                 continue;
@@ -58,9 +68,11 @@ public class NewPolars {
                 for (double i = previousTWA; i < twa; i ++){
                     double mult = i - previousTWA;
                     double newSpeed = diff * mult + prevSpeed;
-                    System.out.println(newS);
+                    //System.out.println(newSpeed);
+                    System.out.println(String.format("tws %f, twa %f, bs %f", windSpeed, i, newSpeed));
                     tws.put(i, newSpeed);
                 }
+                previousTWA = twa;
             }
         }
     }
@@ -88,13 +100,12 @@ public class NewPolars {
      * @return
      */
     private static int getQuadrant(double degrees){
-        return ((int) degrees % 360) / 90 + 1;
+        return ((((int) degrees % 360) + 360) % 360) / 90 + 1;
     }
 
     private static double getBestSpeedInQuadrant(int quad, Map<Double, Double> set){
-        double quadVal = (double)((quad - 1) % 2);// this is as the hash map only has values 0 - 180
+        double min = quad* 90;
-        double min = quadVal* 90;
+        double max = (quad + 1) * 90;
-        double max = (quadVal + 1) * 90;
         double maxAngle = 0;
         double maxSpeed = 0;
         for (Double s: set.keySet()){