package pokus.sd;

import java.util.Iterator;
import java.util.Random;
import java.util.Vector;

import org.spaceroots.mantissa.optimization.CostFunction;

//trida provede ohodnoceni vysledku funkce ziskaneho metodou Simplex Downhill

public class HostsPositionCostFunction implements CostFunction {

	/** Pocitadlo iteraci. */
	private int count = 0;

	/**
	 * @param computedLm
	 *            Vektor vypoctenych parametru
	 */
	public double cost(double[] computedLm) {
		double errSum = 0;
		++count;
		/*
		 * Resi vzdalenosti v matici sousednosti (vypocty oznacene krizkem) 
		 *  1  2  3  4 
		 * +--+--+--+--+ 
		 *1|  |  |  |  | 
		 * +--+--+--+--+ 
		 *2|X |  |  |  | 
		 * +--+--+--+--+
		 *3|X |X |  |  | 
		 * +--+--+--+--+ 
		 *4|X |X |X |  | 
		 * +--+--+--+--+
		 */

		HostPositionEstimation estimation = new HostPositionEstimation();
		Landmark[] lm = new Landmark[estimation.getLandmarks().length];
		lm = estimation.getLandmarks();
		
		for (int i = 0; i < lm.length; i++) {
			// PING hodnota 
			double ping = estimation.getPingValue()[i];
			//double ping = getPing(estimation.getPingValue()[i]);
			
			// landmark
			double lat1 = lm[i].getLatitude();
			double lng1 = lm[i].getLongitude();
			
			// host
			double lat2 = computedLm[0];
			double lng2 = computedLm[1];

			double distance = getDistanceRtt(lat1, lng1, lat2, lng2)*1000;

			// celkova kvadraticka odchylka zmerenych a vypoctenych hodnot
			errSum += error(ping, distance);
		}
		System.out.println(count + ": " + errSum);
		return errSum;
	}

	public double getDistanceRtt(double lat1, double lng1, double lat2, double lng2) {
		double c = 299792.458;	// m/ms
		//double cOpt = c*0.7;
		Conversion conv = new Conversion();
		double dist = conv.getDistance2(lat1, lng1, lat2, lng2);
		
		return 2*(dist/c);
	}
	
	private double getPing(double rtt) {
		double c = 299792.458;	// m/ms
		double cOpt = c*(7/10);
		
		return (rtt/2)*cOpt;
	}

	/**
	 * Celkova chyba mezi namerenou a vypoctenou vzdalenosti.
	 * 
	 * @param ping
	 *            Hodnota prevzata z operace PING. Nejcasteji se jedna o RTT.
	 * @param distance
	 *            Vzdalenost mezi prvky
	 * 
	 * @return kvadraticka odchylka.
	 */
	protected double error(double ping, double distance) {
		return (ping - distance) * (ping - distance);
	}

	public int getCount() {
		return count;
	}

	public void resetCount(int count) {
		this.count = 0;
	}
}
