package seng302.Model;

import javafx.util.Pair;

import java.util.ArrayList;
import java.util.HashMap;

/**
 * Created by hba56 on 10/05/17.
 */

/**
 * Encapsulates an entire polar table. Has a function to calculate VMG.
 */
public class Polars {

    ///Internal store of data. Maps pair<windSpeed, windAngle> to boatSpeed.
    private HashMap<Pair<Double, Double>, Double> polarValues = new HashMap<>();

    /**
     * Ctor.
     */
    public Polars() {
    }


    /**
     * Adds an estimated velocity to the polar table object, for a given (windSpeed, windAngle) pair. That is, stores a mapping from (windSpeed, windAngle) to (boatVelocity).
     * @param trueWindSpeed The true wind speed of the estimate.
     * @param trueWindAngle The true wind angle of the estimate.
     * @param boatSpeed The boat speed of the estimate.
     */
    public void addEstimate(double trueWindSpeed, double trueWindAngle, double boatSpeed){
        Pair newKey = new Pair(trueWindSpeed, trueWindAngle);
        polarValues.put(newKey, boatSpeed);
    }


    /**
     * Calculates the VMG for a given wind angle, wind speed, and angle to destination.
     * @param trueWindAngle The current true wind angle.
     * @param trueWindSpeed The current true wind speed.
     * @param destinationAngle The angle between the boat and the destination point.
     * @return
     */
    public VMG calculateVMG(double trueWindAngle, double trueWindSpeed, double destinationAngle) {

        //Currently a fairly simple implementation where we find the wind speed that is less than or equal to the current wind speed (the lower bound), and then find the specific angle (with no interpolation) that gives the best VMG.

        double polarWindSpeed = 0;

        //Find the lower bound wind speed from the polar table.
        for (Pair<Double, Double> key : this.polarValues.keySet()) {

            double currentPolarSpeed = key.getKey();

            //If the speed from the table we are looking at is greater than the last speed, and less than or equal to the current wind speed.
            if ((currentPolarSpeed > polarWindSpeed) && (currentPolarSpeed <= trueWindSpeed)) {
                polarWindSpeed = currentPolarSpeed;
            }
        }

        //We create a list of wind angles because we need (speed, angle) pairs to look into the map.
        ArrayList<Double> windAngles = new ArrayList<>();
        for (Pair<Double, Double> key : this.polarValues.keySet()) {

            //Don't add angles multiple times.
            double angle = key.getValue();
            if (!windAngles.contains(angle)) {
                windAngles.add(angle);
            }
        }

        //Find the angle with the best VMG.
        //TODO need to differentiate between windward and leeward.
        double bestVMGAngle = 0;
        double bestVMGVelocity = 0;
        for (double tackAngle : windAngles) {
            Pair<Double,Double> key = new Pair<Double,Double>(polarWindSpeed, tackAngle);
            if (this.polarValues.containsKey(key)) {
                //This is the velocity from the polar table at this wind speed/angle.
                double estVelocity = this.polarValues.get(key);
                double angleBetweenDestAndTack = tackAngle - destinationAngle;
                //This is the estimated velocity towards the target (e.g., angling away from the target reduces velocity).
                double vmgTemp = Math.cos(angleBetweenDestAndTack) * estVelocity;

                //Check that the velocity is better.
                if (vmgTemp > bestVMGVelocity) {
                    bestVMGVelocity = vmgTemp;
                    bestVMGAngle = tackAngle;
                }

            }

        }

        System.out.println("VMG speed = " + bestVMGVelocity + " , VMG angle = " + bestVMGAngle);//TEMP DEBUG REMOVE

        //Create the VMG object and return it.
        return new VMG(bestVMGVelocity, bestVMGAngle);


    }


    /**
     * Returns the hashmap used to store polar data.
     * @return A hashmap containing estimated boat speeds for a given (windSpeed, windAngle) pair.
     */
    public HashMap<Pair<Double, Double>, Double> getPolarValues() {
        return polarValues;
    }
}
