Added new polar function, currently it does not distinguish between up wind and downwind and doesn't find it in the correct quadrant #story[1186]

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

@@ -3,10 +3,7 @@ package mock.model;
 import javafx.util.Pair;
 import shared.model.Bearing;
 
-import java.util.ArrayList;
-import java.util.HashMap;
-import java.util.List;
-import java.util.Map;
+import java.util.*;
 
 /**
  * Encapsulates an entire polar table. Has a function to calculate VMG.
@@ -25,6 +22,8 @@ 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<>();
 
 
 
@@ -78,7 +77,49 @@ public class Polars {
 
     }
 
+    public void addPolars(double trueWindSpeed, Bearing trueWindAngle, double boatSpeed){
+        double tws = trueWindSpeed;
+        double bs = boatSpeed;
+        double twa = trueWindAngle.degrees();
+        if (!polars.containsKey(tws)){
+            polars.put(tws, new HashMap<>());
+        }
+        polars.get(tws).put(twa, bs);
+    }
 
+    public double getClosest(double value, Set<Double> set){
+        double closestVal = 0;
+        double smallestDiff = Double.MAX_VALUE;
+        for (double d: set){
+            double diff = Math.abs(value - d);
+            if (diff < smallestDiff){
+                closestVal = d;
+                smallestDiff = diff;
+            }
+        }
+        return closestVal;
+    }
+
+    /**
+     *
+     * @param trueWindAngle
+     * @param trueWindSpeed
+     * @param boatAngle
+     * @return
+     */
+    public VMG setBestVMG(Bearing trueWindAngle, double trueWindSpeed, Bearing boatAngle){
+        //speed
+        double closestSpeed = getClosest(trueWindSpeed, polars.keySet());
+
+        //not using true vmg using general concept
+        double angle = Math.abs(trueWindAngle.degrees() - boatAngle.degrees());
+
+        double closestAngle = getClosest(angle, polars.get(closestSpeed).keySet());
+
+        Bearing vmgAngle = Bearing.fromDegrees(closestAngle);
+        double boatSpeed = Math.abs(closestSpeed * Math.cos(vmgAngle.radians())) * 4;
+        return new VMG(boatSpeed, vmgAngle);
+    }
 
     /**
      * Calculates the VMG for a given wind angle, wind speed, and angle to destination. Will only return VMGs that have a true bearing (angle) within a given bound - this is to ensure that you can calculate VMGs without going out of bounds.