import java.awt.*;
import java.io.*;
import java.util.*;
import java.util.List;
import java.awt.image.*;
import javax.imageio.ImageIO;
import javax.swing.*;

/**
 * CMCMarkerTrackingEX.java
 * 
 * A class to find markers in pictures made by LED white light. Adapted from CMCMatkerTracking for 3D purposes.
 * 
 * @author Will Levine, Matt Kracum, Kohei Shinkawa, Coryn Pavelsky
 * @version 3-11-2010
 * */

public class CMCMarkerTrackingEx implements MarkerTracking {
	String inFolderW = "/3DComps/0";
	String inFolderL = inFolderW + "/LC1";
	String inFolderC = inFolderW + "/CC1";
	String inFolderR = inFolderW + "/RC1";
	
	String outFile = "02.text";
	
	//Our threshold value for black
	public static final int BLACK = 200;
	//130 (best), 120, 180 for Dance from 2T1
	//130, 160 tests for 2Box
	//130 for kickchick
	//130, 160 swing test1
	//160 lean
	//160, 180 2t2boring
	//130, 110, 100, 80, 200 (best, but bad) 2t1WalkCenter
	//130 for 0
	
	//Holds the coordinates of the markers found in one picture.  Each entry representing a marker's coordinates is a Mkr object, which hold the XYZ coords
	public Mkr[] markersL = new Mkr[13];
	public Mkr[] markersC = new Mkr[13];
	public Mkr[] markersR = new Mkr[13];
	public Mkr[] markersW = new Mkr[13];
	//Max number of guesses allowed before the guessed point is not displayed due to its inaccuracy
	public static final int MAX_GUESS = 5;
	
	public static final double MAX_GUESS_DIST = 3.01;

	/**
	 * Constructor
	 */
	public CMCMarkerTrackingEx(){ }

	/**
	 * Builds array containing the picture files as File objects.  We iterate through this array later to find the coordinates of each of the markers in each picture.
	 */
	private static void addFilesRecursively(File file, Collection<File> all) {
	    final File[] children = file.listFiles();
	    if (children != null) {
	        for (File child : children) {
	        	if(child.getName().endsWith(".jpg")) all.add(child);
	            addFilesRecursively(child, all);
	        }
	    }
	}
	
	/**
	 * Creates a double array of double values to store the coordinates.  Each sub array represents a picture.  Each of these sub arrays contains the XYZ
	 * coords of each of the markers in the picture.  These are of the form: [XYZ(head), XYZ(shoulder1), XYZ(shoulder2), etc...].  If a cluster is not found, XYZ of
	 * that cluster is guessed based on previous data on that marker's motion. RETURNS: 'coordinates', a double array of double values.
	 */
	public double[][] getCoordinates() {
		
		//Get all of the picture files, sort them by name, and put them into an array
		final ArrayList<File> FILESL = new ArrayList<File>();
		addFilesRecursively(new File(inFolderL), FILESL);
		System.out.println(FILESL);
		Collections.sort((List<File>)FILESL, new FileComparator());
		System.out.println(FILESL);
		Object[] tempL = FILESL.toArray();
		
		final ArrayList<File> FILESC = new ArrayList<File>();
		addFilesRecursively(new File(inFolderC), FILESC);
		System.out.println(FILESC);
		Collections.sort((List<File>)FILESC, new FileComparator());
		System.out.println(FILESC);
		Object[] tempC = FILESC.toArray();
		
		final ArrayList<File> FILESR = new ArrayList<File>();
		addFilesRecursively(new File(inFolderR), FILESR);
		System.out.println(FILESR);
		Collections.sort((List<File>)FILESR, new FileComparator());
		System.out.println(FILESR);
		Object[] tempR = FILESR.toArray();
		
		//Go through the images one by one finding the coordinates of the markers in each
		for(int i = 0; i < tempL.length; i++){
			System.out.println("File name: "+ tempL[i]);
			if(tempL[i].toString().endsWith(".jpg") && tempC[i].toString().endsWith(".jpg") && tempR[i].toString().endsWith(".jpg")){
				if(i == 0) {
					double[] firstPicL = getCoordinates(tempL[i].toString());
					double[] firstPicC = getCoordinates(tempC[i].toString());	
					double[] firstPicR = getCoordinates(tempR[i].toString());	
					//Adds the coordinates of the makers in the first frame to the array as Mkr objects
					for(int k = 0; k < 13; k++){
						markersL[k] = new Mkr(firstPicL[k*3], firstPicL[k*3+1], firstPicL[k*3+2]);
						markersC[k] = new Mkr(firstPicC[k*3], firstPicC[k*3+1], firstPicC[k*3+2]);
						markersR[k] = new Mkr(firstPicR[k*3], firstPicR[k*3+1], firstPicR[k*3+2]);
					}
				}
				else{
					double[] picL = getCoordinates(tempL[i].toString());
					double[] picC = getCoordinates(tempC[i].toString());
					double[] picR = getCoordinates(tempR[i].toString());
					processCoords(picL, picC, picR, markersL, markersC, markersR);
					}
				}
			}
		
		 //Finds Head, shoulders, hands...etc and sorts them in the array accordingly
		sortMarkers(markersL);
		sortMarkers(markersC);
		sortMarkers(markersR);

		//Sets the pointers to the first frame, where each marker represents the correct body part, so that we can go through the markers
		//and match later ones to their proper body part
		for(int i =0; i < 13; i++){
			markersL[i] = markersL[i].getHeader();
			markersC[i] = markersC[i].getHeader();
			markersR[i] = markersR[i].getHeader();
			
		}
		
		//twoDDouble is a temporary arraylist version of the double array of doubles that we will return in the end
		ArrayList<double[]> twoDDouble = new ArrayList<double[]>();
		//While there are pictures that haven't been added to twoDDouble, add the next one's coordinates to coords, which will then be added to twoDDouble
		while(markersL[0] != null){
			//Temp array to hold the XYZ values of markers in a given picture
			double[] coords = new double[39];
			//Add the markers from this picture to coords
			for(int k=0; k < 13; k++){
				//If right is missing
				if(markersL[k].getGuess() == 0 && markersC[k].getGuess() == 0 && markersR[k].getGuess() != 0){
					double[] p1 = {markersL[k].getX(), markersL[k].getY()};
					double[] p2 = {markersC[k].getX(), markersC[k].getY()};
					double[] wc = CMCMath.calculateWorldCoord2(p1, 2, p2, 1);
					coords[k*3] = wc[0];
					coords[k*3+1] = wc[1];
					coords[k*3+2] = wc[2];
					markersL[k] = markersL[k].getNext();
					markersC[k] = markersC[k].getNext();
					markersR[k] = markersR[k].getNext();
				}
				//If left is missing
				else if(markersL[k].getGuess() != 0 && markersC[k].getGuess() == 0 && markersR[k].getGuess() == 0){
					double[] p1 = {markersR[k].getX(), markersR[k].getY()};
					double[] p2 = {markersC[k].getX(), markersC[k].getY()};
					double[] wc = CMCMath.calculateWorldCoord2(p1, 0, p2, 1);
					coords[k*3] = wc[0];
					coords[k*3+1] = wc[1];
					coords[k*3+2] = wc[2];
					markersL[k] = markersL[k].getNext();
					markersC[k] = markersC[k].getNext();
					markersR[k] = markersR[k].getNext();
				}
				//If center is missing
				else if(markersL[k].getGuess() == 0 && markersC[k].getGuess() != 0 && markersR[k].getGuess() == 0){
					double[] p1 = {markersL[k].getX(), markersL[k].getY()};
					double[] p2 = {markersR[k].getX(), markersR[k].getY()};
					double[] wc = CMCMath.calculateWorldCoord2(p1, 2, p2, 0);
					coords[k*3] = wc[0];
					coords[k*3+1] = wc[1];
					coords[k*3+2] = wc[2];
					markersL[k] = markersL[k].getNext();
					markersC[k] = markersC[k].getNext();
					markersR[k] = markersR[k].getNext();
				}
				//If multiple are missing or all are found just pretend they all aren't guesses
				else{
					double[] p1 = {markersL[k].getX(), markersL[k].getY()};
					double[] p2 = {markersC[k].getX(), markersC[k].getY()};
					double[] p3 = {markersR[k].getX(), markersR[k].getY()};
					double[] wc = CMCMath.calculateWorldCoord3(p3, p2, p1);
					coords[k*3] = wc[0];
					coords[k*3+1] = wc[1];
					coords[k*3+2] = wc[2];
					markersL[k] = markersL[k].getNext();
					markersC[k] = markersC[k].getNext();
					markersR[k] = markersR[k].getNext();
				}
			}
			//Add coords to twoDDouble
			twoDDouble.add(coords);
		}
		
		//Move the contents of twoDDouble into a double array of doubles, coordinates.  Return coordinates
		double[][] coordinates = new double[twoDDouble.size()][twoDDouble.get(0).length];
		System.out.println(coordinates.length);
		for(int i =0; i<twoDDouble.size(); i++){
			coordinates[i] = twoDDouble.get(i);
		}
		
		return coordinates;
	}
	
	/**
	 * Sort markers by their relative coordinates
	 * right now, this assumes that the initial picture was taken from the front
	 */
	public void sortMarkers(Mkr[] marks){
		
		//If the right should was found before the left, switch them
		if(marks[1].getHeader().getX() > marks[2].getHeader().getX()){
			Mkr temp = marks[2];
			marks[2] = marks[1];
			marks[1] = temp;
		}
		//Knees
		if(marks[9].getHeader().getX() > marks[10].getHeader().getX()){
			Mkr temp = marks[10];
			marks[10] = marks[9];
			marks[9] = temp;
		}
		//Feet
		if(marks[11].getHeader().getX() > marks[12].getHeader().getX()){
			Mkr temp = marks[12];
			marks[12] = marks[11];
			marks[11] = temp;
		}
		//Elbows
		if(marks[3].getHeader().getX() > marks[4].getHeader().getX()){
			Mkr temp = marks[4];
			marks[4] = marks[3];
			marks[3] = temp;
		}
		
		//Sorting the hands and hips by X value.  Slightly more complex due to 4 points
		boolean done = false;
		while(! done){
			done = true;
			for(int i = 0; i < 3; i++){
				if(marks[i+5].getHeader().getX() > marks[i+6].getHeader().getX()){
					Mkr temp = marks[i+6];
					marks[i+6] = marks[i+5];
					marks[i+5] = temp;
					done = false;
				}
			}
		}
		Mkr temp = marks[6];
		marks[6] = marks[8];
		marks[8] = temp;
		temp = marks[7];
		marks[7] = marks[8];
		marks[8] = temp;

				
	}
	
	/**
	 * Finds the XYZ coords of each marker in a picture (called by double[][] getCoordinates for each pic).  Returns the array of the 13 coordinates in XYZ format.
	 */
	public double[] getCoordinates(String fileName) {
		//Read the image into java
		BufferedImage bim = null;
		try {
			 bim = ImageIO.read(new File(fileName));
		} catch (IOException e) {
			e.printStackTrace();
		}

		//The double array that will contain the marker coords for 1 picture
		double[] clust = new double[39];
		//Find 13 points, [-1,-1,-1] is added at the end of clust if a point isn't found.  When we find a point, the cluster on the picture i blacked out, so we must
		//set the old image object, bim, to th new image
		for(int i = 0; i<13; i++){
			Object[] temp = findCluster(bim);
			double[] center = (double[]) temp[0];
			bim = (BufferedImage) temp[1];
			int j = i*3;
			clust[j] = center[0];
			clust[j+1] = center[1];
			clust[j+2] = center[2];
		}
		return clust;
	}	
	
	/**
	 * Calculate the distance between two markers for the purpose of matching the markers of two frames to each other: head marker to head marker because
	 * the second marker is closest to the head from the first picture
	 */
	private double getMkrDistance(Mkr newMkr, Mkr lastMkr){
		double leeway = 1; //.017
		
		double difX = (Math.abs(newMkr.getX() - lastMkr.getX()) + 1)*leeway;
		double difY = (Math.abs(newMkr.getY() - lastMkr.getY()) + 1)*leeway;
		double difZ = (Math.abs(newMkr.getZ() - lastMkr.getZ()) + 1)*leeway;
		
		
		double distance = Math.pow(difX, 2) + Math.pow(difY, 2) + Math.pow(difZ, 2);
		
		return distance;
	}
	
	/**
	 * Finds and returns the index of the predicted coordinates of the point in another picture closest to the newMkr point.  Use this to keep track of 
	 * which markers in a new picture correspond to which markers in the previous picture (points).
	 */
	private int findBestMatch(Mkr newMkr, Mkr[] points){
		double bestDistance = 10000000;
		int index = -1;
		for(int i=0; i < points.length; i++){
			double distance = getMkrDistance(newMkr, points[i].predictPoint());
			if(distance < bestDistance){
				bestDistance = distance;
				index = i;
			}	
		}

		return index;
	}
	
	/**
	 * After assigning the points, fill the enmpty slots with their guessing points*/
	private void fillUnassigned(int frameNumL, Mkr[] marksL, int frameNumC, Mkr[] marksC, int frameNumR, Mkr[] marksR){
		for(int i=0; i < 13; i++){
			//If left is missing
			if(marksL[i].getFrameNum() != frameNumL && marksC[i].getFrameNum() == frameNumC && marksR[i].getFrameNum() == frameNumR){
				Mkr guessPntL = new Mkr();
				Mkr tempL = marksL[i].findLastKnown();
				double[] p1 = {marksR[i].getX(), marksR[i].getY()};
				double[] p2 = {marksC[i].getX(), marksC[i].getY()};
				double[] wc = CMCMath.calculateWorldCoord2(p1, 0, p2, 1);
				double[] p0 = CMCMath.calculateCameraCoords(wc, 2);
				guessPntL.setX(p0[0]);
				guessPntL.setY(p0[1]);
				guessPntL.setZ(tempL.getZ());
				guessPntL.setGuess(0);
				marksL[i] = guessPntL;
			}
			//If right is missing
			else if(marksL[i].getFrameNum() == frameNumL && marksC[i].getFrameNum() == frameNumC && marksR[i].getFrameNum() != frameNumR){
				Mkr guessPntR = new Mkr();
				Mkr tempR = marksR[i].findLastKnown();
				double[] p1 = {marksL[i].getX(), marksL[i].getY()};
				double[] p2 = {marksC[i].getX(), marksC[i].getY()};
				double[] wc = CMCMath.calculateWorldCoord2(p1, 2, p2, 1);
				double[] p0 = CMCMath.calculateCameraCoords(wc, 2);
				guessPntR.setX(p0[0]);
				guessPntR.setY(p0[1]);
				guessPntR.setZ(tempR.getZ());
				guessPntR.setGuess(0);
				marksR[i] = guessPntR;
			}
			//If center is missing
			else if(marksL[i].getFrameNum() == frameNumL && marksC[i].getFrameNum() == frameNumC && marksC[i].getFrameNum() != frameNumC){
				Mkr guessPntC = new Mkr();
				Mkr tempC = marksC[i].findLastKnown();
				double[] p1 = {marksL[i].getX(), marksL[i].getY()};
				double[] p2 = {marksR[i].getX(), marksR[i].getY()};
				double[] wc = CMCMath.calculateWorldCoord2(p1, 2, p2, 0);
				double[] p0 = CMCMath.calculateCameraCoords(wc, 2);
				guessPntC.setX(p0[0]);
				guessPntC.setY(p0[1]);
				guessPntC.setZ(tempC.getZ());
				guessPntC.setGuess(0);
				marksC[i] = guessPntC;
			}
			
			// Find the ones that have not been assigned new marks and either fill the void with a predicted value or disable it.
			else{
				fillUnassignedHelper(frameNumL, marksL);
				fillUnassignedHelper(frameNumC, marksC);
				fillUnassignedHelper(frameNumR, marksR);
			}
		}
	}
	/**
	 * Helper function for fillUnassigned
	 * */
	private void fillUnassignedHelper(int frameNum, Mkr[] marks){
		for(int i=0; i < 13; i++){
			if(marks[i].getFrameNum() != frameNum){
				Mkr guessPnt = new Mkr();
				guessPnt.setPrev(marks[i]);	
				marks[i].setNext(guessPnt);
			
				// If the point has been guessed too many times in a row, make sure it is disabled and go back to last known.
				if(marks[i].getGuess() > MAX_GUESS || marks[i].isDisable()){
					System.out.println("Disabling guess: Too many guesses");
					Mkr temp = marks[i].findLastKnown();
					guessPnt.setX(temp.getX());
					guessPnt.setY(temp.getY());
					guessPnt.setZ(temp.getZ());
					guessPnt.setGuess(temp.getGuess()+1);
					guessPnt.setDisable(true);
				}
    		
    		// If the predicted point is too far from the last known point, disable it and revert back to last known point.
	    		else if(getMkrDistance(marks[i].predictPoint(), marks[i].findLastKnown()) > MAX_GUESS_DIST){
	    			System.out.println("Disabling guess: Too far reaching");
	    			Mkr temp = marks[i].findLastKnown();
	    			guessPnt.setX(temp.getX());
	    			guessPnt.setY(temp.getY());
	    			guessPnt.setZ(temp.getZ());
	    			guessPnt.setGuess(temp.getGuess()+1);
	    			guessPnt.setDisable(true);
		    	}
	    		
	    		// Build the predicted value.
	    		else{	
	       			guessPnt.setX(marks[i].predictPoint().getX());
	    			guessPnt.setY(marks[i].predictPoint().getY());
	    			guessPnt.setZ(marks[i].predictPoint().getZ());
	    			guessPnt.setGuess(marks[i].getGuess()+1);
	    		}
				
    		// Set the missing marker to the predicted value or the unknown value, depending on what guessPnt is.
			marks[i] = guessPnt;
			}
		}
	}
	
	/**
	 * Count how many of the same number at the index are in the list
	 */
	private  int count(int[] list, int index){
		int count = 0;
		for(int i=0; i < list.length; i++){
			if(list[i] == index && list[i] != -1) count++;
		}
		return count;
	}
	
	/**
	 * Check and see so that no two points have been assigned to a same point
	 * Re-assign points not yet assigned to those that are close enough
	 */
	private int[] checkIndex(int[] index, Mkr[] points, Mkr[] marks){
		ArrayList<Integer> pointsTrack = new ArrayList<Integer>(); // Keeps track of points that have not been assigned.
		ArrayList<Integer> markersTrack = new ArrayList<Integer>(); // Keeps track of markers that have not been assigned.
		
		for(int i=0; i < index.length; i++){
			markersTrack.add(i);
			if(index[i] != -1){
				pointsTrack.add(i);
			}
		}
		
		for(int i=0; i < index.length; i++){
			if(count(index, index[i]) > 1){
				if(index[i] == -1){
					//Do nothing
				}
				else if(marks[index[i]].getGuess() > 0){
					index[i] = -1;
				}
				else if(findBestMatch(marks[index[i]], points) != i){
					index[i] = -1;
				}
				else{
					markersTrack.remove(markersTrack.indexOf(index[i]));
					pointsTrack.remove(pointsTrack.indexOf(i));
				}
			}
			else if(index[i] != -1){
				markersTrack.remove(markersTrack.indexOf(index[i]));
				pointsTrack.remove(pointsTrack.indexOf(i));
			}
		}
		
		while(pointsTrack.size() > 0){
			Mkr[] temp = new Mkr[markersTrack.size()];
			for(int i=0; i< markersTrack.size(); i++){
				if(!marks[markersTrack.get(i)].isDisable()){
					temp[i] = marks[markersTrack.get(i)];
				}
				else{
					temp[i] = marks[markersTrack.get(i)].findLastKnown();
				}
			}
			int pos = findBestMatch(points[pointsTrack.get(0)], temp);
			
			if(getMkrDistance(points[pointsTrack.get(0)], temp[pos]) < MAX_GUESS_DIST)	{
				index[pointsTrack.get(0)] = markersTrack.get(pos);
				markersTrack.remove(pos);
				System.out.println("Re-assigned "+pos);
			}
			
			// If we have guessed too many times, cancel the output to prevent a runaway point.
			else{
				System.out.println("Cannot assign "+pos+" the nearest point too far");
			}
			pointsTrack.remove(0);
		}

		return index;
	}
	
	/**
	 * Cleans up the coords double array.
	 * Assign newly found points to the previous makers in the form of doubly linked list 
	 */
	private void processCoords(double[] coordinatesL, double[] coordinatesC, double[] coordinatesR, Mkr[] marksL, Mkr[] marksC, Mkr[] marksR){			
			int[] indexL = new int[13];
			int[] indexC = new int[13];
			int[] indexR = new int[13];
			Mkr[] mkrL = new Mkr[13];
			Mkr[] mkrC = new Mkr[13];
			Mkr[] mkrR = new Mkr[13];
			
			// Goes through each of the coordinates in the list for one picture, adds these new markers to the mkr array.
			// Adds to index the index of the marker from the previous picture that best matches the new marker.
	        for(int i=0; i< 13; i++){
	        	Mkr newMkrL = new Mkr(coordinatesL[i*3], coordinatesL[i*3+1],coordinatesL[i*3+2]);
	        	Mkr newMkrC = new Mkr(coordinatesC[i*3], coordinatesC[i*3+1],coordinatesC[i*3+2]);
	        	Mkr newMkrR = new Mkr(coordinatesR[i*3], coordinatesR[i*3+1],coordinatesR[i*3+2]);
	        	mkrL[i] = newMkrL;
	        	mkrC[i] = newMkrC;
	        	mkrR[i] = newMkrR;
	        	if(newMkrL.getX() < 0){
	        		indexL[i] = -1;
	        	}
	        	else{
	        		// Sets, at the point that the marker was found in the picture, the index of the point in the previous picture that best matches newMkr.
	        		indexL[i] = findBestMatch(newMkrL, marksL);
	        	}
	        	
	        	if(newMkrC.getX() < 0){
	        		indexC[i] = -1;
	        	}
	        	else{
	        		// Sets, at the point that the marker was found in the picture, the index of the point in the previous picture that best matches newMkr.
	        		indexC[i] = findBestMatch(newMkrC, marksC);
	        	}
	        	
	        	if(newMkrR.getX() < 0){
	        		indexR[i] = -1;
	        	}
	        	else{
	        		// Sets, at the point that the marker was found in the picture, the index of the point in the previous picture that best matches newMkr.
	        		indexR[i] = findBestMatch(newMkrR, marksR);
	        	}
	        }
	        
	        //Print out the indices of the best match for the previous picture
	        /*System.out.println(index[0]+" "+index[1]+" "+index[2]+" "+index[3]+" "+index[4]+" "+index[5]+" "+index[6]+" "+index[7]+" "+index[8]+" "+
	        		index[9]+" "+index[10]+" "+index[11]+" "+index[12]);*/
	        //Assigns, reassigns, or removes points.  Takes care of matching up points between pictures(?).  insures no 2 points pointing
	        //to the same place.
	        indexL = checkIndex(indexL, mkrL, marksL);
	        indexC = checkIndex(indexC, mkrL, marksC);
	        indexR = checkIndex(indexR, mkrL, marksR);
	        /*System.out.println(index[0]+" "+index[1]+" "+index[2]+" "+index[3]+" "+index[4]+" "+index[5]+" "+index[6]+" "+index[7]+" "+index[8]+" "+
	        		index[9]+" "+index[10]+" "+index[11]+" "+index[12]);*/
	        
	        // Moves up the linked list to get to the next picture.
	        int curNumL=0;
	        int curNumC=0;
	        int curNumR=0;
	        for(int i=0; i< 13; i++){
	        	if(indexL[i] != -1){
	        		mkrL[i].setPrev(marksL[indexL[i]]);
	        		marksL[indexL[i]].setNext(mkrL[i]);
	        		marksL[indexL[i]] = mkrL[i];
	        		curNumL = marksL[indexL[i]].getFrameNum();
	        	}
	        	if(indexC[i] != -1){
	        		mkrC[i].setPrev(marksC[indexC[i]]);
	        		marksC[indexC[i]].setNext(mkrC[i]);
	        		marksC[indexC[i]] = mkrC[i];
	        		curNumC = marksC[indexC[i]].getFrameNum();
	        	}
	        	if(indexR[i] != -1){
	        		mkrR[i].setPrev(marksR[indexR[i]]);
	        		marksR[indexR[i]].setNext(mkrR[i]);
	        		marksR[indexR[i]] = mkrR[i];
	        		curNumR = marksR[indexR[i]].getFrameNum();
	        	}
	        }
	        //System.out.println(curNum);
	        fillUnassigned(curNumL, marksL, curNumC, marksC, curNumR, marksR);
	}
	

	/**
	 * Finds a cluster of anything not black in the given image, using the averages of xmin, xmax, ymin, ymax.
	 * Returns the coords of the center of the cluster and the average RGB values of the cluster, as an array of doubles.
	 */
	private Object[] findCluster(BufferedImage bim){
		int height = bim.getHeight();
		int width = bim.getWidth();
		int clusterSize = 0;
		int xmin = -1;
		int ymin = -1;
		int xmax = -1;
		int ymax = -1;
		
		// Use done to make sure that it only finds one cluster.
		Boolean done = false;
		
		for(int y =0; y < height; y++){
			if (done){
				break;
			}
            for(int x = 0; x< width; x++){
            	int bits = bim.getRGB(x, y);
                Color cl = new Color(bits, false);
                // If pixel is above threshold...
                if (MatchColor(cl)){
                	//... use findClusterHelper to find the data we want on the cluster and set done=true
                	ArrayList<Object> result = findClusterHelper(bim, x, y, 1);
                	bim = (BufferedImage)result.get(0);
            		clusterSize = ((Number)result.get(1)).intValue();
            		xmin = ((Number)result.get(2)).intValue();
            		xmax = ((Number)result.get(3)).intValue();
            		ymin = ((Number)result.get(4)).intValue();
            		ymax = ((Number)result.get(5)).intValue();
                	done = true;
                	break;
	            }
			}
		}

		double[] center = new double[3];
		
		// If the marker isn't found, be ready to return -1s.
		if(clusterSize == 0){
		 center[0] = -1.0;
		 center[1] = -1.0;
		 center[2] = -1.0;
		}
		
		// If the marker is found, be ready to return the data.
		else{
		 center[0] = (((double)(xmax+xmin))/(2*((double)width)));
		 center[1] = (((double)(ymax+ymin))/(2*((double)height)));
		 center[2] = -1;
		 }
		
		Object[] combo = new Object[2];
		combo[0] = center;
		combo[1] = bim;
		
        return combo;
	}
	
	/**
	 * Recursively 'helps' to 'findClusters'.  This operates on one cluster.  Returns arraylist of max and min x and y, pixel count, and the image
	 * with the cluster blacked out.
	 */
	private ArrayList<Object> findClusterHelper(BufferedImage bim, int x , int y, int counter){
    	bim.setRGB(x, y, Color.black.hashCode());
    	
    	int xmin = x;
    	int xmax = x; 	
    	int ymin = y;
    	int ymax = y;
    	
    	ArrayList<Object> result = null;
    	
		if( ((x-1) > 0) && MatchColor(new Color(bim.getRGB(x-1,y)))){
			result = findClusterHelper(bim, x-1, y, 1);
			
			bim = (BufferedImage) result.get(0);
			counter += ((Number)result.get(1)).intValue();
			int newxmin= ((Number)result.get(2)).intValue();
			int newxmax= ((Number)result.get(3)).intValue();
			int newymin= ((Number)result.get(4)).intValue();
			int newymax= ((Number)result.get(5)).intValue();

			if (xmin > newxmin) xmin = newxmin;
			if (xmax < newxmax) xmax = newxmax;
			if (ymax < newymax) ymax = newymax;
			if (ymin > newymin) ymin = newymin;
			
		}
		
		if((bim.getWidth() >= (x +1)) && MatchColor(new Color(bim.getRGB(x+1,y)))){
			result = findClusterHelper(bim, x+1, y, 1);
			
			bim = (BufferedImage) result.get(0);
			counter += ((Number)result.get(1)).intValue();
			int newxmin= ((Number)result.get(2)).intValue();
			int newxmax= ((Number)result.get(3)).intValue();
			int newymin= ((Number)result.get(4)).intValue();
			int newymax= ((Number)result.get(5)).intValue();

			if (xmin > newxmin) xmin = newxmin;
			if (xmax < newxmax) xmax = newxmax;
			if (ymax < newymax) ymax = newymax;
			if (ymin > newymin) ymin = newymin;
			
		}
		if(((y-1) > 0) && MatchColor(new Color(bim.getRGB(x,y-1)))){
			result = findClusterHelper(bim, x, y-1, 1);
			
			bim = (BufferedImage) result.get(0);
			counter += ((Number)result.get(1)).intValue();
			int newxmin= ((Number)result.get(2)).intValue();
			int newxmax= ((Number)result.get(3)).intValue();
			int newymin= ((Number)result.get(4)).intValue();
			int newymax= ((Number)result.get(5)).intValue();

			if (xmin > newxmin) xmin = newxmin;
			if (xmax < newxmax) xmax = newxmax;
			if (ymax < newymax) ymax = newymax;
			if (ymin > newymin) ymin = newymin;
			
		}
		
		
		if((bim.getHeight() >= (y +1)) && MatchColor(new Color(bim.getRGB(x,y+1)))){
			result = findClusterHelper(bim, x, y+1, 1);
			bim = (BufferedImage) result.get(0);
		
			counter += ((Number)result.get(1)).intValue();
			int newxmin= ((Number)result.get(2)).intValue();
			int newxmax= ((Number)result.get(3)).intValue();
			int newymin= ((Number)result.get(4)).intValue();
			int newymax= ((Number)result.get(5)).intValue();

			if (xmin > newxmin) xmin = newxmin;
			if (xmax < newxmax) xmax = newxmax;
			if (ymax < newymax) ymax = newymax;
			if (ymin > newymin) ymin = newymin;
			
		}
		
		ArrayList<Object> finalResult= new ArrayList<Object>();
		finalResult.add(bim);
		finalResult.add(counter);
		finalResult.add(xmin);
		finalResult.add(xmax);
		finalResult.add(ymin);
		finalResult.add(ymax);

		return finalResult;
	}
	
	/**
	 * Displays specified image from BufferedImage.  Used for debugging purposes
	 */
	private void display(BufferedImage bim, String fileName){
		ImageIcon image= new ImageIcon((Image)bim); //cast cast cast...
		JLabel pic = new JLabel(image);
		
		JFrame f = new JFrame(fileName);
        f.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
        f.setSize(image.getIconWidth(), image.getIconHeight());
        f.setLocation(200,200);
        f.add(pic);
        f.setVisible(true);
        
        f.dispose();
	}

	/**
	 * Determines if a pixel's given rgb values are higher than the global threshold's rgb values
	 */
	private boolean MatchColor(Color cl){
		int r = cl.getRed();
		int g = cl.getGreen();
		int b = cl.getBlue();
		
		if ((r > BLACK) && (g > BLACK) && (b > BLACK)){
			return true;
		}
	
		return false;
	}

	/**
	 * Gets the folder name where our pictures are.
	 */
	public String getInFolder() {
		return inFolderW;
	}

	/**
	 * Gets the file name where we are outputting the coords of each marker in each picture.  This is more an alternative to using the double array of coords.
	 */
	public String getOutFile() {
		return outFile;
	}
	
	/**
	 * Sets the name of the folder we are going to be using to find our pictures.
	 */
	public void setInFolder(String foldername) {
		inFolderW = foldername;

	}

	/**
	 * Sets the name of the file we are going to be using to store the coords of the markers in each of the images.
	 */
	public void setOutFile(String filename) {
		outFile = filename;
	}

	/**
	 * Writes the coords of each of the markers in each of the pictures and the colors they're associated with to a NEW file.
	 */
	public void writeToFile() {
		double[][] coords = getCoordinates();
		Writer writer = null;
		try {
			writer = new BufferedWriter(new FileWriter(outFile));
			writer.write(inFolderW+"\r\n");
			for(int i =0; i < coords.length ; i++){
				for(int k =0; k < coords[i].length; k++){
					writer.write(Double.toString(coords[i][k]));
					if(k != coords[i].length-1) writer.write(",");
				}
				writer.write("\r\n");
			}
		writer.close();
		writer = null;
		} catch (IOException e) {           
			System.out.println("FAILED");
			e.printStackTrace();
		}
	}
	
	/**
	 * Appends the coords of each of the markers in each of the pictures and the colors they're associated with to an OLD file. Creates a new file if there is no old file.
	 */
	public void writeToFileAppend() throws IOException {
		double[][] coords = getCoordinates();
		Writer writer = null;
		try {
			writer = new BufferedWriter(new FileWriter(outFile, true));
			writer.write(inFolderW+"\r\n");
			for(int i =0; i < coords.length ; i++){
				for(int k =0; k < coords[i].length; k++){
					writer.write(Double.toString(coords[i][k]));
					if(k != coords[i].length-1) writer.write(",");
				}
				writer.write("\r\n");
			}
		writer.close();
		writer = null;
		} catch (IOException e) {
			System.out.println("FAILED");
			e.printStackTrace();
		}
	}

	/**
	 * @deprecated*/
	public void setAviFile(){
		// Deprecated.
	}
	
	/**
	 * Main method.  Runs the show.
	 */
	public static void main(String[] args) throws IOException{
		CMCMarkerTrackingEx cmcMT = new CMCMarkerTrackingEx();
		cmcMT.writeToFile();
		System.out.println("Done");
	}
}
