package Location;

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

import Utils.*;

/**
 * Simple Location finder that returns the first known APs location from the list of received MAC addresses
 * @author Bernd
 *
 */
public class CustomLocationFinder implements LocationFinder{

	private HashMap<String, Position> knownLocations; //Contains the known locations of APs. The long is a MAC address.
	private Position lastPosition = new Position(0, 0);
	private PositionBuffer positionHistory = new PositionBuffer(10);

	public CustomLocationFinder(){
		knownLocations = Utils.getKnownLocations(); //Put the known locations in our hashMap
	}

	@Override
	public Position locate(MacRssiPair[] data) {
		//printMacs(data); //print all the received data
		return getFirstKnownFromList(data); //return the first known APs location
	}

	/**
	 * Returns the position of the first known AP found in the list of MacRssi pairs
	 * @param data
	 * @return
	 */
	private Position getFirstKnownFromList(MacRssiPair[] data){
		List<MacRssiPair> locations = selectKnownPairs(data);
		locations = selectStrongestPairs(locations, 3);

		if(locations.size() == 3) {
			MacRssiPair pairA = locations.get(0);
			MacRssiPair pairB = locations.get(1);
			MacRssiPair pairC = locations.get(2);
			Position posA = knownLocations.get(pairA.getMacAsString());
			Position posB = knownLocations.get(pairB.getMacAsString());
			Position posC = knownLocations.get(pairC.getMacAsString());
			double factorAB = getLerpFactor(pairA, pairB);
			double factorAC = getLerpFactor(pairA, pairC);
			double factorBC = getLerpFactor(pairB, pairC);
			Position posAB = lerpPosition(posA, posB, factorAB);
			Position posAC = lerpPosition(posA, posC, factorAC);
			Position posBC = lerpPosition(posAB, posAC, factorBC);

			System.out.println("[Locating with 3 signals]");
			System.out.printf("%s %s%n", pairA, posA);
			System.out.printf("%s %s%n", pairB, posB);
			System.out.printf("%s %s%n", pairC, posC);
			System.out.println("Factors");
			System.out.printf("AB: %f%n", factorAB);
			System.out.printf("AC: %f%n", factorAC);
			System.out.printf("BC: %f%n", factorBC);

			//lastPosition = posBC;
			positionHistory.add(posBC);
		} else if(locations.size() > 0) {
			MacRssiPair pair = locations.get(0);
			String mac = pair.getMacAsString();

			System.out.println("[Locating with 1 signal]");
			System.out.println(pair);

			//lastPosition = knownLocations.get(mac);
			positionHistory.add(knownLocations.get(mac));
		} else {
			System.out.println("[No data, returning last known position]");
		}

		//return lastPosition;
		return positionHistory.average();
	}

	private List<MacRssiPair> selectKnownPairs(MacRssiPair[] data) {
		List<MacRssiPair> result = new ArrayList<MacRssiPair>();

		for(MacRssiPair pair : data) {
			if(knownLocations.containsKey(pair.getMacAsString())) {
				result.add(pair);
			}
		}

		return result;
	}

	private List<MacRssiPair> selectStrongestPairs(List<MacRssiPair> data, int count) {
		List<MacRssiPair> result = new ArrayList<MacRssiPair>();

		for(int i = 0; i < count && count < data.size(); i++) {
			MacRssiPair pair = getStrongestPair(data);

			result.add(pair);
			data.remove(pair);
		}

		return result;
	}

	private MacRssiPair getStrongestPair(List<MacRssiPair> data) {
		int max = Integer.MIN_VALUE;
		MacRssiPair result = null;

		for(MacRssiPair pair : data) {
			if(pair.getRssi() >= max) {
				result = pair;
				max = pair.getRssi();
			}
		}

		return result;
	}

	/**
	 * Outputs all the received MAC RSSI pairs to the standard out
	 * This method is provided so you can see the data you are getting
	 * @param data
	 */
	private void printMacs(MacRssiPair[] data) {
		for (MacRssiPair pair : data) {
			System.out.println(pair);
		}
	}

	private static double getLerpFactor(MacRssiPair a, MacRssiPair b) {
		// diff 0       	  = factor 0.5
		// huge diff (eg 100) = factor 0
		MacRssiPair strongest = a.getRssi() > b.getRssi() ? a : b;
		MacRssiPair weakest = strongest == a ? b : a;

		double exp = 0.25d;
		int difference = -weakest.getRssi() + strongest.getRssi();
		double factor = 0.5d - (Math.pow(difference / 100d, exp) * 0.5d);

		// Linear blend
		//double factor = 0.5d - ((difference / 100d) * 0.5d);

		return factor;
	}

	private static Position lerpPosition(Position a, Position b, double f) {
		double x = (1.0d - f) * a.getX() + f * b.getX();
		double y = (1.0d - f) * a.getY() + f * b.getY();

		return new Position(x, y);
	}
}

class PositionBuffer {
	private int size;
	private Position[] positions;

	public PositionBuffer(int size) {
		this.size = size;
		this.positions = new Position[size];

		for(int i = 0; i < size; i++) {
			positions[i] = new Position(0, 0);
		}
	}

	public void add(Position position) {
		System.arraycopy(positions, 0, positions, 1, size - 1);
		positions[0] = position;
	}

	public Position average() {
		double x = 0.0d;
		double y = 0.0d;

		for(Position pos : positions) {
			x += pos.getX();
			y += pos.getY();
		}

		return new Position(x / size, y / size);
	}
}