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

package mock.model;

import shared.model.Bearing;

import java.util.HashMap;
import java.util.Map;
import java.util.Set;
import java.util.TreeMap;

/**
 * Created by fwy13 on 4/09/17.
 */
public class NewPolars {


    //true wind speed, <true wind angle, best boat angle>
    private static Map<Double, TreeMap<Double, Double>> polars = new TreeMap<>();

    public static NewPolars newPolars = null;

    public NewPolars(){
        newPolars = this;
    }

    public static 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 TreeMap<>());
        }
        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(){
//        double maxTWS = 0;
//        for (double key: polars.keySet()){
//            if (maxTWS < key){
//                maxTWS = key;
//            }
//        }
        /*for (double windSpeed: polars.keySet()){
            if (!polars.get(windSpeed).containsKey(180d)){
                polars.get(windSpeed).put(180d, windSpeed);
            }
        }*/
        TreeMap<Double, Double> prevTWS = null;
        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;
            }
            double previousTWA = -1;
            TreeMap<Double, Double> iterableTWS = new TreeMap<>(tws);
            for (double twa: iterableTWS.keySet()){
                if (previousTWA == -1){
                    previousTWA = twa;
                    continue;
                }
                double twaDiff = twa - previousTWA;
                double speedDiff = iterableTWS.get(twa) - iterableTWS.get(previousTWA);
                double prevSpeed  = iterableTWS.get(previousTWA);
                double diff = speedDiff/twaDiff;
                for (double i = previousTWA; i < twa; i ++){
                    double mult = i - previousTWA;
                    double newSpeed = diff * mult + prevSpeed;
                    //System.out.println(newSpeed);
                    System.out.println(String.format("tws %f, twa %f, bs %f", windSpeed, i, newSpeed));
                    tws.put(i, newSpeed);
                }
                previousTWA = twa;
            }
        }
    }

    private static 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;
    }

    /**
     * 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) + 360) % 360) / 90 + 1;
    }

    private static double getBestSpeedInQuadrant(int quad, Map<Double, Double> set){
        double min = quad* 90;
        double max = (quad + 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
     * @param trueWindSpeed
     * @param boatAngle
     * @return
     */
    public static 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());

        int quad = getQuadrant(boatAngle.degrees());
        double bestAngle = getBestSpeedInQuadrant(quad, polars.get(closestSpeed));

        Bearing vmgAngle = Bearing.fromDegrees((bestAngle) % 360);
        double boatSpeed = polars.get(closestSpeed).get(bestAngle);

        return new VMG(boatSpeed, Bearing.fromDegrees(vmgAngle.degrees() + trueWindAngle.degrees()));
    }

    public static double calculateSpeed(Bearing trueWindAngle, double trueWindSpeed, Bearing boatAngle){
        //speed
        double closestSpeed = getClosest(trueWindSpeed, polars.keySet());

        double angleDiff = Math.abs(trueWindAngle.degrees() - boatAngle.degrees()) % 180;
        double closestAngle = getClosest(angleDiff, polars.get(closestSpeed).keySet());

        double boatSpeed = polars.get(closestSpeed).get(closestAngle);

        return boatSpeed;


    }

}