package org.mote.wiimote.whiteboard.calibration;

import java.util.HashSet;
import java.util.Map;
import java.util.Set;

import org.mote.wiimote.whiteboard.ds.IRDot;
import org.mote.wiimote.whiteboard.ds.Wiimote;
import org.mote.wiimote.whiteboard.preferences.WWPreferences;

public class PointClusterer {

	static {
		if (WWPreferences.WIIMOTES > 2)
			throw new RuntimeException("Clustering for more than 2 Wiimotes not implemented yet.");
	}

	public static IRDot[][] cluster(Map<Wiimote, IRDot[]> data) {
		IRDot[][] cluster = new IRDot[4][];

		if (data.size() == 1) {
			IRDot[] values = data.values().iterator().next();
			int j = 0;
			for (int i = 0; i < 4; i++)
				if (values[i] != null)
					cluster[j++] = new IRDot[] { values[i] };
			return cluster;
		}

		Wiimote[] wiimotes = new Wiimote[data.keySet().size()];
		data.keySet().toArray(wiimotes);

		Set<IRDot> used = new HashSet<IRDot>();
		int c = 0;
		
		for (int a = 0; a < 2; a++) {
			
			for (int i = 0; i < 4; i++) {				
				IRDot dot1 = data.get(wiimotes[a])[i];
				if (dot1 == null || used.contains(dot1))
					continue;
				
				IRDot minDot = null;
				double minDist = Double.POSITIVE_INFINITY;
				
				for (int j = 0; j < 4; j++) {
					IRDot dot2 = data.get(wiimotes[(a+1)%2])[j];
					if (dot2 == null || used.contains(dot2))
						continue;
					
					double dist = dot1.distance(dot2);
					if (dist < minDist) {
						minDot = dot2;
						minDist = dist;
					}
				}
				
				used.add(dot1);
				if (minDot == null) {
					cluster[c++] = new IRDot[] { dot1 };
				} else {
					cluster[c++] = new IRDot[] { dot1, minDot };
					used.add(minDot);
				}
				
			}
		}		

		return cluster;
	}

}
