#ifdef _CH_
#pragma package <opencv>
#endif

#ifndef _EiC
#include "cv.h"
#include "cvaux.h"
#include "highgui.h"
#endif

#include "components.h"
#include <time.h>
#include "nipples.h"

#define getrandom( min, max ) ((rand() % (int)(((max) + 1) - (min))) + (min))


components::components()
{
	pixels=0;
	label=0;
	imWidth=0;
	imHeight=0;
	curves=NULL;
	pointCounter=NULL;
	validLabel=NULL;
	X=new double[5];//Array of 
	realElypse=NULL;
	offsetX=0;
	offsetY=0;
	Xrotdone=false;	//used in error finding for ellipses
	Xrot=new double[5];
	/*loci1X=0;	//loci of ellipse in actual image,
	loci1Y=0;	//used to find distance from point to ellipse
	loci2X=0;
	loci2Y=0;*/
	h=0;		//center of ellipse
	k=0;
	a2=0;
	b2=0;
	q=0;
}

components::~components()
{
	pixels=0;
	if (curves!=NULL) {
		for (int r=0; r<label; r++) 
			delete [] curves[r];
		delete [] curves;
	}
	if (pointCounter!=NULL) {
		delete [] pointCounter;
	}
	if (validLabel!=NULL)
		delete [] validLabel;
	delete [] X;
	delete [] Xrot;
	label=0;
	imWidth=0;
	imHeight=0;
	offsetX=0;
	offsetY=0;
	/*loci1X=0;	//loci of ellipse in actual image,
	loci1Y=0;	//used to find distance from point to ellipse
	loci2X=0;
	loci2Y=0;*/
	
}

/*input (below) is assumed to be canny edge space*/
IplImage *components::Components(IplImage *input, IplImage *skin, std::vector<ellipse*> &ellipses) {	//from edges. 
	
	IplImage *skinmap(IplImage *skin);	//creates binary image from skin image. usefull for skin % determintaion per component later

	imWidth=input->width;
	imHeight=input->height;
	pixels=imWidth*imHeight;
	CvSize size;
	size=cvGetSize(input);

	IplImage *skinbinary=skinmap(skin);	//binary image of the skin and non skin regions

	IplImage *src = NULL;		//grayscale representation of input
	int dms = input->nChannels;
	if (dms>1){
		src = cvCreateImage( size, IPL_DEPTH_8U, 1);
		cvCvtColor( input, src, CV_BGR2GRAY);
	}
	else{
		src=cvCloneImage(input);	
	}

	//short **skinstatus;
	skinstatus=new short*[imWidth];
	for (int y=0; y< imWidth; y++) 
		skinstatus[y]= new short[imHeight];

	int a=0;
	for (y=0; y<imWidth; y++) {
		for (int e=0; e<imHeight; e++) {
			a=((uchar*)(src->imageData + src->widthStep*e))[y];
			if (a==0)
				skinstatus[y][e]=1;		//not curve, is background
			else 
				skinstatus[y][e]=0;		//curve
		}
	}

	int **complabel;
	int **features;
	features=new int *[pixels];
	complabel=new int *[imWidth];
	for (int i=0; i<imWidth; i++) {
		complabel[i]=new int[imHeight];
	}

	for (i=0; i<pixels; i++)
		features[i]=new int[2];

	for (i=0; i<pixels; i++) {
		for  (int j=0; j<2; j++) {
			features[i][j]=0;
		}
	}

	label=1;
	int *comparea=new int[pixels]; //area per component
	//minlabel=new int[1000];
	int *labelisSkin=new int[pixels];
	int *featloc=new int[pixels];
	int featlocCount=0;

	int *minX=new int[pixels];
	int *minY=new int[pixels];
	int *maxX=new int[pixels];
	int *maxY=new int[pixels];
	//int *nonConvexArea=new int[1000];

	for (i=0; i<pixels; i++) {
		comparea[i]=0;
		labelisSkin[i]=0;
		maxX[i]=0;
		maxY[i]=0;
		minX[i]=1000000;
		minY[i]=1000000;
	}

	int maxarea=0;  //component with greatest area		  
	int *minlabel=new int[pixels];
	for (int q=0;q<pixels;q++) {
		minlabel[q]=q;
		minX[q]=imWidth;
		minY[q]=imHeight;
	}

	for (int r=0;r<imWidth;r++)
		skinstatus[r][0]=2;
	for (int z=0;z<imHeight;z++)
		skinstatus[0][z]=2;
	for (int g=0;g<imWidth;g++){
		//skinstatus[imWidth-1][g]=2;
		skinstatus[g][imHeight-1]=2;		// Rob - allows you to dellocate memory that you couldn't before..
	}

    int xm1y=0;
	int xm1ym1=0;
    int xym1=0;
	int xp1ym1=0;  
	int temp1=0;		
	int temp2=0;

	for (i=0; i<imWidth; i++) {
		for (int j=0; j<imHeight; j++) {
			complabel[i][j]=0;
		}
	}

	for (y=1;y<imHeight;y++) {	//skin components
		for (int x=1;x<(imWidth-1);x++) {
			xm1y=skinstatus[x-1][y];
			xm1ym1=skinstatus[x-1][y-1];
			xym1=skinstatus[x][y-1];
			xp1ym1=skinstatus[x+1][y-1];
			
			if (xm1y==skinstatus[x][y])
				complabel[x][y]=complabel[x-1][y];
			if (xm1ym1==skinstatus[x][y]) {
				if (complabel[x][y]==0)
					complabel[x][y]=complabel[x-1][y-1];
				else {
					temp1=complabel[x][y];
					temp2=complabel[x-1][y-1];
					sameComp(&temp1,&temp2, minlabel);
				}
			}if (xym1==skinstatus[x][y]) {
				if (complabel[x][y]==0)
					complabel[x][y]=complabel[x][y-1];
				else {
					temp1=complabel[x][y];
					temp2=complabel[x][y-1];
					sameComp(&temp1,&temp2, minlabel);
				}
			}if (xp1ym1==skinstatus[x][y]) { 
				if (complabel[x][y]==0)
					complabel[x][y]=complabel[x+1][y-1];
				else {
					temp1=complabel[x][y];
					temp2=complabel[x+1][y-1];
					sameComp(&temp1,&temp2, minlabel);
				}
			}if (complabel[x][y]==0){
				complabel[x][y]=label;
				label++;
			}
		}  //for x
	}  //for y

	for (int p=1;p<pixels;p++) {
		int t=p;
		do {
			t=minlabel[t];
		}while(t!=minlabel[t]);
		minlabel[p]=t;
	}                         //for p

	for (y=1;y<imHeight;y++) {	
		for (int x=1;x<imWidth-1;x++) {
			complabel[x][y]=minlabel[complabel[x][y]];	//finds lowest equal label
		}
	}
	//colour components
	printf("Number of components: %i\n",label);

	for (y=1;y<imHeight;y++) {			//computes area, this is important
		for (int x=1;x<imWidth-1;x++) {
			comparea[complabel[x][y]]++;			//add 1 to area
			if (skinstatus[x][y]==0)
				labelisSkin[complabel[x][y]]=1;
	
			if (x<minX[complabel[x][y]])
				minX[complabel[x][y]]=x;
			if (y<minY[complabel[x][y]])
				minY[complabel[x][y]]=y;
			if (x>maxX[complabel[x][y]])
				maxX[complabel[x][y]]=x;
			if (y>maxY[complabel[x][y]])
				maxY[complabel[x][y]]=y;
		}
	}

	//find largest piece of skin
	maxarea=0;
    int maxlabel=0;					//label of largest skin component
	for (int h=0;h<pixels;h++) {
		if ((maxarea<comparea[h])&&(labelisSkin[h]==1)){
			maxarea=comparea[h];
			maxlabel=h;
		}
	}
	
	/*printf("largest area is %i pixels large\n", maxarea);
	printf("it occurs at label %i\n", maxlabel);
	printf("minX and minY are: %i %i\n", minX[maxlabel], minY[maxlabel]);
	printf("maxX and maxY are: %i %i\n", maxX[maxlabel], maxY[maxlabel]);
	
	CvPoint p1;
	CvPoint p2;
	p1.x=minX[maxlabel];
	p1.y=minY[maxlabel];
	p2.x=maxX[maxlabel];
	p2.y=maxY[maxlabel];
	//cvRectangle( input, p1, p2, CV_RGB(255,255,255), 2 );*/
	
	
	IplImage* r_plane = cvCreateImage( size, 8, 1 );	//helpers (3 channels)
    IplImage* g_plane = cvCreateImage( size, 8, 1 );
    IplImage* b_plane = cvCreateImage( size, 8, 1 );
	for (int x=0; x<imWidth; x++) {
		for (int y=0; y<imHeight; y++) {
			if (((uchar*)(skinbinary->imageData + skinbinary->widthStep*y))[x]==0) {
				((uchar*)(r_plane->imageData + r_plane->widthStep*y))[x]=0;
				((uchar*)(g_plane->imageData + g_plane->widthStep*y))[x]=0;
				((uchar*)(b_plane->imageData + b_plane->widthStep*y))[x]=0;
			}
			/*else {
				((uchar*)(r_plane->imageData + r_plane->widthStep*y))[x]=0;	//100
				((uchar*)(g_plane->imageData + g_plane->widthStep*y))[x]=0;
				((uchar*)(b_plane->imageData + b_plane->widthStep*y))[x]=0;
			}*/
		}
	}

	curves = new point*[label];		//curves dimentions= label X (num element per label)
	pointCounter=new int[label];	//array of counters. Determines place of next point in each array in curves[][];
	validLabel=new short[label];
	int init=0;			//helps with curves initialization
	for (i=0; i<label; i++) {
		if (comparea[i]<1) {
			curves[i]=new point[2];	//comparea[i];
			init=2;
		} else {
			curves[i]=new point[comparea[i]];	//comparea[i];
			init=comparea[i];
		}
		for (int j=0; j<init; j++) {
			curves[i][j].x=0;		//init all points to 0;
			curves[i][j].y=0;
		}
		pointCounter[i]=0;
		validLabel[i]=0;
	}

	//int color1;
	//int color2;
	//int color3;
	for (int labs=0; labs<label; labs++) {	//colour each component a different colour.
		//color1 = rand();
		//color2 = rand();
		//color3 = rand();
		for (int x=1; x<imWidth-1; x++) {
			for (int y=1; y<imHeight; y++) {
				if ((complabel[x][y]==labs)&&(skinstatus[x][y]==0)){
					((uchar*)(r_plane->imageData + r_plane->widthStep*y))[x]=255;//color1
					((uchar*)(g_plane->imageData + g_plane->widthStep*y))[x]=255;//color2
					((uchar*)(b_plane->imageData + b_plane->widthStep*y))[x]=255;//color3
					curves[labs][pointCounter[labs]].x=x;
					curves[labs][pointCounter[labs]].y=y;
					validLabel[labs]=1;
					//printf("labs= %i (%i,%i)\n",labs, x, y);
					//if ((x<0)||(y<0))
					//	x++;
					pointCounter[labs]++;	//number of points per used label
				}
			}
		}
	}

	

	//call elyses here**************
	elypses(minX, minY, maxX, maxY, input, skinstatus, complabel, skinbinary, ellipses);

	IplImage *out=cvCreateImage( size, 8, 3 );
	cvCvtPlaneToPix(b_plane, g_plane, r_plane, 0, out);
	
	// - Rob - Removed since not needed (will use milos' elypses method instead)
	//fitElipse(out, skinbinary);

	cvReleaseImage(&r_plane);
	cvReleaseImage(&g_plane);
	cvReleaseImage(&b_plane);

	delete [] minX;
	delete [] minY;
	delete [] maxX;
	delete [] maxY;
	delete [] labelisSkin;
	delete [] comparea;
	delete [] featloc;
	delete [] minlabel;
	delete [] validLabel;
	validLabel=NULL;
	
	// - Rob (Swapped the following 2 lines.. was loosing reference before deleted)
	delete [] pointCounter;
	pointCounter=NULL;
	
	//delete [] nonConvexArea;
	
	for (i=0; i<imWidth; i++) {
		delete [] complabel[i];
	}
	delete [] complabel; 
	
	for (i=0; i<imWidth; i++)		//Rob -- Fixed:     Milos: no idea why it doesn't accept it without the -2
		delete [] skinstatus[i];
	delete [] skinstatus;
	skinstatus=NULL;

	for (i=0; i<pixels; i++) 
		delete [] features[i];
	delete [] features;
	
	////////////////
	for (r=0; r<label; r++) {		//Rob -- Fixed: why can't I deallocate the array?   -- YOU can now.. ROB!  (See above)
		delete [] curves[r];
		//if (curves[r]!=NULL)			
	}
	delete [] curves;
	curves=NULL;
	///////////

	cvReleaseImage(&src);
	cvReleaseImage(&skinbinary);
	label=0; 
	pixels=0;
	return out;
}

void components::sameComp(int *A, int *B, int *minlabel) {
	int be=minlabel[*B];
	do {
		be=minlabel[be];
	}while (be!=minlabel[be]);
	
	int Ae=minlabel[*A];
	do {
		Ae=minlabel[Ae];
	}while (Ae!=minlabel[Ae]);
	
	if (Ae<be)
		minlabel[be]=Ae;
	else 
		minlabel[Ae]=be;
	
	minlabel[*A]=minlabel[Ae];
	minlabel[*B]=minlabel[be];
}

IplImage *skinmap(IplImage *skin) {	//creates binary image from skin image. usefull for skin % determintaion per component later

	CvSize size;
	size=cvGetSize(skin);
	IplImage *skinbinary;
	skinbinary = cvCreateImage( size, IPL_DEPTH_8U, 1);
	int dms = skin->nChannels;
	if (dms>1)
		cvCvtColor( skin, skinbinary, CV_BGR2GRAY);
	cvThreshold( skinbinary, skinbinary, 1, 100, CV_THRESH_BINARY );

	/*cvNamedWindow("binary",1);
	cvShowImage( "binary", skinbinary );
	cvWaitKey(0);
	cvDestroyWindow("binary");*/

	return skinbinary;
}

//main function (elypses). determines whether curves are elypses or not
void components::elypses(int *minX, int *minY, int *maxX, int*maxY, IplImage *input, short **skinstatus, int **complabel, IplImage *skinbinary, std::vector<ellipse*>& ellipses) {	//find elypses based on curves from Components (above)
	
	struct point test[5]={0};	//5 test points chosen at random from each curve. Initialized to 0;
	int p1=0;	//p1 is random number, used for clarity 
	int numberOfTries=300;	//number of rounds of ellipse fitting
	int EllipsesFound=0;
	
	double **Gauss=Matrix(5,6);
	realElypse=new short[label];	//list of proclaimed ellipses
	double **Averages=Matrix(5,numberOfTries);	//helps determine average value of each of 5 variables (ROSIN)
	int AveragesCounter=0;	//determines how many actual solutions exist per curve: Max is 50
	
	double **Test=Matrix(5,numberOfTries);	//test matrix.. 
	
	

	/*double **testmat=Matrix(3, 4);
	testmat[0][0]=1;
	testmat[0][1]=0;
	testmat[0][2]=1;
	testmat[0][3]=-1;
	
	testmat[1][0]=1;
	testmat[1][1]=1;
	testmat[1][2]=1;
	testmat[1][3]=-2;
	
	testmat[2][0]=1;
	testmat[2][1]=-1;
	testmat[2][2]=1;
	testmat[2][3]=-1;
	
	GaussElim(testmat, 3, 4);
	
	for (int x=0; x<3; x++) {
		for (int y=0; y<4; y++) {
			printf("%9.3f ", testmat[x][y]);
		}
		printf("\n");
	}
	
	printf("%9.3f %9.3f %9.3f %9.3f %9.3f\n\n", X[0], X[1], X[2], X[3], X[4]);
	cvWaitKey(0);*/
	
	for (int i=1; i<label; i++) {	//for each curve;	problems with label 0, considered skin
		int cu1=curves[i][0].x;
		int cu2=curves[i][0].y;
		realElypse[i]=0;
		srand(20);
		//cvNamedWindow("testpoints", 1);

		//if valid curve (used label) and curve has more than 10 pxls
		if ((validLabel[i]==1)&&(pointCounter[i]>20)) {	//if skin and #points>20		
			if (skinstatus[cu1][cu2]==0) {	//curve not the background (exclude background)
				calcOffset(i);	//introduce origin offset to decrease calculation error later
				
				for (int k=0; k<numberOfTries; k++) {	//try to find an elypse 50 times
					fillGauss(Gauss, i, minX, minY, maxX, maxY);

					//start manipulation of Gaussian array
					bool f;
					f=GaussElim(Gauss, 5, 6);
					if (f) {		//if there exists a unique solution to Gauss matrix...
									/*for (int x=0; x<5; x++) {
									for (int y=0; y<6; y++) {
									printf("%9.3f ", Gauss[x][y]);
									}
									printf("\n");
									}
									
									  printf("%9.3f %9.3f %9.3f %9.3f %9.3f\n\n", X[0], X[1], X[2], X[3], X[4]);
						cvWaitKey(0);*/
						
						//try error fitting each elipse
						bool earlyelipse;
						earlyelipse=checkElypse2(curves, i, X, minX[i], maxX[i], minY[i], maxY[i], skinbinary);
						Xrotdone=false;
						if (earlyelipse) {
							//printf("early elipse found after %i tries\n",k);
							k=numberOfTries;	//skip the rest of the testing for this curve
							AveragesCounter=0;	// - Rob / Milos .. Changed to =0 instead of -1.. -1 would cause memory corruption.. (Out of bounds memory writing in Averages[t][-1]...=X[T]
						
							// Rob - We have an ellipse, now memorize it
							ellipse* e = new ellipse(X, h, k, offsetX, offsetY);
							ellipses.push_back(e);
						}
					
						if (AveragesCounter > 0){							
							for (int t=0; t<5; t++) {							
								Averages[t][AveragesCounter]=X[t];
							}
							AveragesCounter++;
						}
						
					}
				}
				if (AveragesCounter>0) {
					//findAverages(Averages, X, AveragesCounter);
					if (realElypse[i]==1) {
						findMedians(Averages, X, AveragesCounter);
						printf("medians for label %i\n", i);
						printf("%9.6f %9.6f %9.6f %9.6f %9.6f\n", X[0], X[1], X[2], X[3], X[4]);
						bool isElypse;
						isElypse=checkElypse2(curves, i, X, minX[i], maxX[i], minY[i], maxY[i], skinbinary);
						if (isElypse){
							EllipsesFound++;	//just ellipse counter, not important
							
							// We have an ellipse, now memorize it
							ellipse* e = new ellipse(X, h, k, offsetX, offsetY);
							ellipses.push_back(e);
						}
					}					
					AveragesCounter=0;
					ClearMatrix(Averages, 5, numberOfTries);
					Xrotdone=false;
				}
				
				offsetX=0;
				offsetY=0;
			}
		}
	}


	delete [] realElypse;

	paintElypses(skinbinary);
	
	DeleteMatrix(Gauss, 5);
	
	DeleteMatrix(Averages, 5);

	DeleteMatrix(Test, 5);

	//for (i=0; i<5; i++)	
	//	delete [] Gauss[i];
	//delete [] Gauss;
	
	//for (i=0; i<5; i++)		//go crazy here ROB!!! mem leaks
	//	delete [] Averages[i];
	//delete [] Averages;
	

	//cvDestroyWindow("testpoints");
	//printf("done elypses\n");
	printf("components::elypses - Number of ellipses found is %i\n", ellipses.size());
}

bool components::GaussElim(double **g, int width, int height) {	//g is a matrix size 5 X 6

	bool sol=true;
	double max=-99999999999;
	int maxRow=0;
	double m=0;
	double backsum=0;
	int b;

	for (int i=0; i<width-1; i++) {		//step 2
		for (int j=i; j<height-1; j++) {	//find largest pivot: step 3
			double t=g[j][i];
			if (t>max) {
				max=t;
				maxRow=j;
			}
		}

		if (g[maxRow][i]==0){		//step 4
			//printf("no unique solution\n");
			sol=false;
			goto exit;
		}
		
		if (i!=maxRow) {	//step 5
			double *tmp=g[i];
					g[i]=g[maxRow];
					g[maxRow]=tmp;
		}

		for (j=i+1; j<width; j++) {		//step 6
			m=g[j][i]/g[i][i];		//step 7
			for (int row=0; row<height; row++) {		//step 8
				g[j][row]=g[j][row]-m*g[i][row];
			}
		}

		if (g[width-1][width-1]==0) {			//step 9
			//printf("no unique solution exits\n");
			sol=false;
			goto exit;
		}
		maxRow=0;
		max=-99999999999;
		m=0;
	}

	/*start backwards substitution*/
	X[width-1]=-g[width-1][height-1]/g[width-1][height-2];	//step 10
	
	for (b=width-2; b>=0; b--) {	//step 11
		for (int k=b+1; k<width; k++) {
			backsum+=g[b][k]*X[k];
		}
		X[b]=(-g[b][height-1]-backsum)/(g[b][b]);
		if ((X[b]>1000000)||(X[b]<-1000000)) {
			sol=false;
			goto exit;
		}
		backsum=0;
	}
	
	if ((X[0]==0)&&(X[1]==0)&&(X[2]==0)&&(X[3]==0)&&(X[4]==0)) {
		sol=false;
		goto exit;
	}
	//backsum=0;
	exit:
	return sol;
}

void components::fillGauss(double **Gauss, int label, int *minX, int *minY, int *maxX, int*maxY) {
	
	/*struct point is allocated on frame??? (is deallocated automatically)*/
	struct point test[5]={0}; 	//5 test points chosen at random from each curve. Initialized to 0;
	int p1=0;	//p1 is random number, used for clarity 
	int xspread=maxX[label]-minX[label];
	int yspread=maxY[label]-minY[label];
	double mindist=0;			//minimal distance between two randomly chosen points. (Rosin)
	if (xspread>=yspread)
		mindist=xspread/8;	//divide by 6 instead of 5 to avoid errors.
	else
		mindist=yspread/8;	

	/*take 5 points at random with mindist spacing*/
	for (int y=0; y<5; y++) {

		if (y==0) {	
			//p1=(int)((double)(rand()*pointCounter[label])/RAND_MAX);	//choses random numer in interval 0..pointcounter[i]
			int p1=getrandom( 0, pointCounter[label]-1 );
			//int p1=cvRandNext( &rng_state )%(pointCounter[label]-1);
			test[y].x=curves[label][p1].x-offsetX;
			test[y].y=curves[label][p1].y-offsetY;
		}
		int tx,ty;
		double pdist=0;
		if (y==1) {//check that next point is mindist away from previous point
			while (pdist<mindist) {
				int p1=getrandom( 0, pointCounter[label]-1 );
				//int p1=cvRandNext( &rng_state )%(pointCounter[label]-1);
				tx=curves[label][p1].x-offsetX;
				ty=curves[label][p1].y-offsetY;
				pdist=sqrt((double)((tx-test[y-1].x)*(tx-test[y-1].x)+(ty-test[y-1].y)*(ty-test[y-1].y)));
			}
			test[y].x=tx;
			test[y].y=ty;
			pdist=0;
		}
		int tx1, ty1;
		double pdis1=0, pdis2=0, pdis3=0, pdis4=0;
		if (y==2) {
			while (pdist<mindist) {
				int p1=getrandom( 0, pointCounter[label]-1 );
				//int p1=cvRandNext( &rng_state )%(pointCounter[label]-1);
				tx1=curves[label][p1].x-offsetX;
				ty1=curves[label][p1].y-offsetY;
				pdis1=sqrt((double)((tx1-test[y-1].x)*(tx1-test[y-1].x)+(ty1-test[y-1].y)*(ty1-test[y-1].y)));
				pdis2=sqrt((double)((tx1-test[y-2].x)*(tx1-test[y-2].x)+(ty1-test[y-2].y)*(ty1-test[y-2].y)));
				pdist=min(pdis1, pdis2);
			}
			test[y].x=tx1;
			test[y].y=ty1;
			pdist=0;
		}

		if (y==3) {
			while (pdist<mindist) {
				int p1=getrandom( 0, pointCounter[label]-1 );
				//int p1=cvRandNext( &rng_state )%(pointCounter[label]-1);
				tx1=curves[label][p1].x-offsetX;
				ty1=curves[label][p1].y-offsetY;
				pdis1=sqrt((double)((tx1-test[y-1].x)*(tx1-test[y-1].x)+(ty1-test[y-1].y)*(ty1-test[y-1].y)));
				pdis2=sqrt((double)((tx1-test[y-2].x)*(tx1-test[y-2].x)+(ty1-test[y-2].y)*(ty1-test[y-2].y)));
				pdis3=sqrt((double)((tx1-test[y-3].x)*(tx1-test[y-3].x)+(ty1-test[y-3].y)*(ty1-test[y-3].y)));
				pdist=min(pdis1, pdis2);
				pdist=min(pdist, pdis3);
			}
			test[y].x=tx1;
			test[y].y=ty1;
			pdist=0;
		}

		if (y==4) {
			while (pdist<mindist) {
				int p1=getrandom( 0, pointCounter[label]-1 );
				//int p1=cvRandNext( &rng_state )%(pointCounter[label]-1);
				tx1=curves[label][p1].x-offsetX;
				ty1=curves[label][p1].y-offsetY;
				pdis1=sqrt((double)((tx1-test[y-1].x)*(tx1-test[y-1].x)+(ty1-test[y-1].y)*(ty1-test[y-1].y)));
				pdis2=sqrt((double)((tx1-test[y-2].x)*(tx1-test[y-2].x)+(ty1-test[y-2].y)*(ty1-test[y-2].y)));
				pdis3=sqrt((double)((tx1-test[y-3].x)*(tx1-test[y-3].x)+(ty1-test[y-3].y)*(ty1-test[y-3].y)));
				pdis4=sqrt((double)((tx1-test[y-4].x)*(tx1-test[y-4].x)+(ty1-test[y-4].y)*(ty1-test[y-4].y)));
				pdis1=min(pdis1, pdis2);
				pdis2=min(pdis3, pdis4);
				pdist=min(pdis1, pdis2);
			}
			test[y].x=tx1;
			test[y].y=ty1;
		}
	}
		//printf("point %i is (%i, %i)\n", y, test[y].x, test[y].y);

	/*//draw chosen points to check their random distribution
	IplImage *check;
	check=cvCreateImage(cvSize(imWidth, imHeight), IPL_DEPTH_8U, 3 );
	IplImage* r_plane = cvCreateImage( cvSize(imWidth, imHeight), 8, 1 );
    IplImage* g_plane = cvCreateImage( cvSize(imWidth, imHeight), 8, 1 );
    IplImage* b_plane = cvCreateImage( cvSize(imWidth, imHeight), 8, 1 );

	for (int j=0; j<imWidth; j++) {
		for (int k=0; k<imHeight; k++) {
			((uchar*)(r_plane->imageData + r_plane->widthStep*k))[j]=0;
			((uchar*)(g_plane->imageData + g_plane->widthStep*k))[j]=0;
			((uchar*)(b_plane->imageData + b_plane->widthStep*k))[j]=0;
		}
	}

	//draw points
	((uchar*)(r_plane->imageData + r_plane->widthStep*(test[0].y+offsetY)))[test[0].x+offsetX]=255;//white
	((uchar*)(g_plane->imageData + g_plane->widthStep*(test[0].y+offsetY)))[test[0].x+offsetX]=255;
	((uchar*)(b_plane->imageData + b_plane->widthStep*(test[0].y+offsetY)))[test[0].x+offsetX]=255;

	((uchar*)(r_plane->imageData + r_plane->widthStep*(test[1].y+offsetY)))[test[1].x+offsetX]=255;//red
	((uchar*)(g_plane->imageData + g_plane->widthStep*(test[1].y+offsetY)))[test[1].x+offsetX]=0;
	((uchar*)(b_plane->imageData + b_plane->widthStep*(test[1].y+offsetY)))[test[1].x+offsetX]=0;

	((uchar*)(r_plane->imageData + r_plane->widthStep*(test[2].y+offsetY)))[test[2].x+offsetX]=0;//green
	((uchar*)(g_plane->imageData + g_plane->widthStep*(test[2].y+offsetY)))[test[2].x+offsetX]=255;
	((uchar*)(b_plane->imageData + b_plane->widthStep*(test[2].y+offsetY)))[test[2].x+offsetX]=0;

	((uchar*)(r_plane->imageData + r_plane->widthStep*(test[3].y+offsetY)))[test[3].x+offsetX]=0;//blue
	((uchar*)(g_plane->imageData + g_plane->widthStep*(test[3].y+offsetY)))[test[3].x+offsetX]=0;
	((uchar*)(b_plane->imageData + b_plane->widthStep*(test[3].y+offsetY)))[test[3].x+offsetX]=100;

	((uchar*)(r_plane->imageData + r_plane->widthStep*(test[4].y+offsetY)))[test[4].x+offsetX]=255;
	((uchar*)(g_plane->imageData + g_plane->widthStep*(test[4].y+offsetY)))[test[4].x+offsetX]=255;
	((uchar*)(b_plane->imageData + b_plane->widthStep*(test[4].y+offsetY)))[test[4].x+offsetX]=255;

	cvCvtPlaneToPix(b_plane, g_plane, r_plane, 0, check);

	cvNamedWindow("testpoints", 1);
	cvShowImage( "testpoints", check );
	cvWaitKey(0);
	cvDestroyWindow("testpoints");
	
	cvReleaseImage(&r_plane);
	cvReleaseImage(&g_plane);
	cvReleaseImage(&b_plane);
	cvReleaseImage(&check);*/

	/*fill Gaussian array*/
	for (int q=0; q<5; q++) {	//for each equation
		for (int e=0; e<6; e++) {	//for each variable
			if (e==0)
				Gauss[q][e]=test[q].x*test[q].x;
			else if (e==1)
				Gauss[q][e]=test[q].x*test[q].y;
			else if (e==2)
				Gauss[q][e]=test[q].y*test[q].y;
			else if (e==3)
				Gauss[q][e]=test[q].x;
			else if (e==4)
				Gauss[q][e]=test[q].y;
			else			//if e==6
				Gauss[q][e]=-1000;	//F=1000
		}
	}	
}

bool components::checkElypse2(point **curves, int index, double *X, int minX, int maxX, int minY, int maxY, IplImage *skinbinary) {
	double pointsOnElypse=0;
	int x,y;
	double error=0;
	double error13=0;
	int skincount=0;

	//if ((X[1]<0.001)&&(X[1]>-0.001))	//if b is very small, the elipse is flat!
	//	return false;

	if ((X[1]*X[1]-4*X[0]*X[2]) >= 0)	//if hyperbola or parabola reject
		return false;

	//count skin pixels per ROI of elipse
	for (int i=minX; i<maxX; i++) {
		for (int j=minY; j<maxY; j++) {
			if (((uchar*)(skinbinary->imageData + skinbinary->widthStep*j))[i]>0)
				skincount++;
		}
	}

	double area=(double)skincount/(double)((maxX-minX)*(maxY-minY));
	if (area<0.7)	//at least 70% skin within the ellipse
		return false;

	//if (index==211) {	//donja sisa kod carmele
 	//	printf(" \n");
	//}

	//declare test window for drawing the ellipse using the EOF13 method.
	IplImage *el;
	el = cvCreateImage( cvSize(imWidth, imHeight), IPL_DEPTH_8U, 1);
	cvSetZero(el);
	//cvNamedWindow("DrawEllipse", 1);
	//cvShowImage("DrawEllipse", el);

	for (i=0; i<pointCounter[index]; i++) {		//for all points in curves[index]
		x=curves[index][i].x-offsetX;
		y=curves[index][i].y-offsetY;
		error=X[0]*x*x + X[1]*x*y + X[2]*y*y + X[3]*x + X[4]*y +1000;
		error13=EOF13(x, y, X, el);
		if (error13<0.05)	//determine variable threshold
			pointsOnElypse++;
		
		if (error13<0) {		//center of ellipse is not in the image (ellipse is too large)
			cvReleaseImage(&el);
			//cvDestroyWindow("DrawEllipse");
			return false;
		}

		//if (error<2500)	//error threshold was 3	
		//	pointsOnElypse++;
	}
	

	if ((pointsOnElypse/(double)pointCounter[index])>=0.9) {	//if 90% of points are near elypse, then curve is elypse
		realElypse[index]=1;
		printf("label %i is an ellipse\n\n", index);
		printf("a2 %f b2 %f\n", a2, b2);
		q=q*180/3.1419;
		printf("q is %f\n", q);
		printf("b2-4ac is %f\n", (X[1]*X[1]-4*X[0]*X[3]));
		
		//paint each component
		//paintElypses(skinbinary);
		//cvWaitKey(0);
		cvNamedWindow("DrawEllipse", 1);
		cvShowImage("DrawEllipse", el);
		//if (index==211)
			cvWaitKey(0);
		cvReleaseImage(&el);
		cvDestroyWindow("DrawEllipse");

		//EOF13(0,0);
		return true;
	}else {
		cvReleaseImage(&el);
		//cvDestroyWindow("DrawEllipse");
		return false;
	}
}

void components::paintElypses(IplImage *skinbinary) {
	
	IplImage *ely;
	ely=cvCreateImage(cvSize(imWidth, imHeight), IPL_DEPTH_8U, 3 );
	IplImage* r_plane = cvCreateImage( cvSize(imWidth, imHeight), 8, 1 );
    IplImage* g_plane = cvCreateImage( cvSize(imWidth, imHeight), 8, 1 );
    IplImage* b_plane = cvCreateImage( cvSize(imWidth, imHeight), 8, 1 );

	for (int j=0; j<imWidth; j++) {
		for (int k=0; k<imHeight; k++) {
			if (((uchar*)(skinbinary->imageData + skinbinary->widthStep*k))[j]==0) {
				((uchar*)(r_plane->imageData + r_plane->widthStep*k))[j]=0;
				((uchar*)(g_plane->imageData + g_plane->widthStep*k))[j]=0;
				((uchar*)(b_plane->imageData + b_plane->widthStep*k))[j]=0;
			}
			else {	//paint all skin regions grey
				((uchar*)(r_plane->imageData + r_plane->widthStep*k))[j]=100;
				((uchar*)(g_plane->imageData + g_plane->widthStep*k))[j]=100;
				((uchar*)(b_plane->imageData + b_plane->widthStep*k))[j]=100;
			}
			if(skinstatus[j][k]==0) {		//paint every curve pale green
				((uchar*)(r_plane->imageData + r_plane->widthStep*k))[j]=0;
				((uchar*)(g_plane->imageData + g_plane->widthStep*k))[j]=200;
				((uchar*)(b_plane->imageData + b_plane->widthStep*k))[j]=200;
			}
		}
	}
	//CvFont *font1;
	//cvInitFont(font1, CV_FONT_VECTOR0, 1.0, 1.0, 0, 1 );
	int x,y;
	for (int i=0; i<label; i++) {
		if (realElypse[i]==1) {	//if elypse, draw it
			int a;
			for (a=0; a<pointCounter[i]; a++) {		//draw every point
				x=curves[i][a].x;
				y=curves[i][a].y;
				((uchar*)(r_plane->imageData + r_plane->widthStep*y))[x]=255;
				((uchar*)(g_plane->imageData + g_plane->widthStep*y))[x]=255;
				((uchar*)(b_plane->imageData + b_plane->widthStep*y))[x]=255;
			}
			char t1[4];
			sscanf(t1, "%d", &i);
			
		}
	}

	cvCvtPlaneToPix(b_plane, g_plane, r_plane, 0, ely);
	
	cvNamedWindow("elypses", 1);
	cvShowImage( "elypses", ely );
	//cvWaitKey(0);
	//cvDestroyWindow("elypses");
	
	cvReleaseImage(&r_plane);
	cvReleaseImage(&g_plane);
	cvReleaseImage(&b_plane);
	cvReleaseImage(&ely);
}

void components::fitElipse(IplImage *im, IplImage *skinbinary) {
	CvMemStorage* stor;
    CvSeq* cont;
    CvBox2D32f* box;
    CvPoint* PointArray;
    CvPoint2D32f* PointArray2D32f;
	//cvNamedWindow("Result", 1);

	CvSize size;
	size=cvGetSize(im);			
	IplImage *image04;		//grayscale representation of input
	image04 = cvCreateImage( size, IPL_DEPTH_8U, 1);
	int dms = im->nChannels;
	if (dms>1)
		cvCvtColor( im, image04, CV_BGR2GRAY);
	else
		image04=cvCloneImage(im);	
    
    // Create dynamic structure and sequence.
    stor = cvCreateMemStorage(0);
    cont = cvCreateSeq(CV_SEQ_ELTYPE_POINT, sizeof(CvSeq), sizeof(CvPoint) , stor);
    
    // Threshold the source image. This needful for cvFindContours().
    //cvThreshold( image04, image04, 130, 255, CV_THRESH_BINARY );
    
    // Find all contours.
    cvFindContours( image04, stor, &cont, sizeof(CvContour), 
                    CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
    
    // Clear images. IPL use.
    //cvZero(image02);
    cvZero(image04);
    int elipsecounter=0; //counts number of confirmed elipses (breasts)

    // This cycle draw all contours and approximate it by ellipses.
    for(;cont;cont = cont->h_next)
    {   
        int i; // Indicator of cycle.
        int count = cont->total; // This is number of points in contour
        CvPoint center;
        CvSize size;
        
        // Number point must be more than or equal to 6 (for cvFitEllipse_32f).        
        if( count < 6 )
            continue;
        
        // Alloc memory for contour point set.    
        PointArray = (CvPoint*)malloc( count*sizeof(CvPoint) );
        PointArray2D32f= (CvPoint2D32f*)malloc( count*sizeof(CvPoint2D32f) );
        
        // Alloc memory for ellipse data.
        box = (CvBox2D32f*)malloc(sizeof(CvBox2D32f));
        
        // Get contour point set.
        cvCvtSeqToArray(cont, PointArray, CV_WHOLE_SEQ);
        
        // Convert CvPoint set to CvBox2D32f set.
        for(i=0; i<count; i++)
        {
            PointArray2D32f[i].x = (float)PointArray[i].x;
            PointArray2D32f[i].y = (float)PointArray[i].y;
        }
        
        // Fits ellipse to current contour.
        cvFitEllipse(PointArray2D32f, count, box);
        
        // Draw current contour.
        cvDrawContours(image04, cont, CV_RGB(0,0,255), CV_RGB(0,0,255), 0, 1, 8);
        
        // Convert ellipse data from float to integer representation.
        center.x = cvRound(box->center.x);
        center.y = cvRound(box->center.y);
        size.width = cvRound(box->size.width*0.5);
        size.height = cvRound(box->size.height*0.5);
        box->angle = -box->angle;
        
		int skincount=0;
		
		//check elipse to make sure it's a good fit
		if ((size.width>5)&&(size.height>5)) {
			//check skin percintage within box, if >80%, OK
			
			double rat=(double)size.width/(double)size.height;
			if (rat>1)
				rat=1/rat;
			if (rat < 0.5)
				continue;


			//count skin pixels per ROI of elipse
			for (int i=center.x-(size.width/2); i<center.x+(size.width/2); i++) {
				for (int j=center.y-(size.height/2); j<center.y+(size.height/2); j++) {
					if (((uchar*)(skinbinary->imageData + skinbinary->widthStep*j))[i]>0)
						skincount++;
				}
			}

			double area=(double)skincount/(double)((size.width)*(size.height));
			if (area<0.8)	//if less than 80%, do not draw...
				continue;

			// Draw ellipse.
			cvEllipse(im, center, size, box->angle, 0, 360, CV_RGB(10,200,200), 1);  //CV_RGB(0,100,100)
			cvEllipse(image04, center, size, box->angle, 0, 360, 200, 1);  
			elipsecounter++;
			skincount=0;
		}
        
        // Free memory.          
        free(PointArray);
        free(PointArray2D32f);
        free(box);
		
		// - Rob
		cvClearMemStorage(stor);
		cvClearSeq(cont);		
    }
		
	
	// Free mem - Rob
	cvReleaseImage(&image04);	
    
	// - Rob .. get rid of the mem storage
	cvReleaseMemStorage(&stor);

	// Show image. HighGUI use.
    //cvShowImage( "Result", image04 );
	printf("Number of elipses found by fitelipse method is %i\n", elipsecounter);
}

void components::calcOffset(int index) {
	double offx, offy;
	float c;

	offx = offy = 0;
    /* shift all points to zero */
    for( int i = 0; i < pointCounter[index]; i++ )
    {
        offx += curves[index][i].x;
        offy += curves[index][i].y;
    }
    c = 1.f / pointCounter[index];
    offx *= c;
    offy *= c;
	offsetX=(int)offx;
	offsetY=(int)offy;
}

//returns percentage D2/D1 (how close percentually x1, y1 is to ellipse
double components::EOF13(double x1, double y1, double *X, IplImage *el) {	//X array is global (A B C D E)
	if (X[1]!=0) { //if B!=0 , rotation is needed
		if (!Xrotdone) {	//if rotation has not yet been calculated
			q=( atan2(X[1], (X[0]-X[2])) )/2;	//angle of rotation
			double cosq=cos(q);
			double cos2q=cosq*cosq;
			double sinq=sin(q);
			double sin2q=sin(q);

			//determine rotated equation of ellipse
			Xrot[0]=X[0]*cos2q + X[1]*cosq*sinq + X[2]*sin2q;	// A'
			Xrot[1]=0;											// B' = 0 ???
			Xrot[2]=X[0]*sin2q - X[1]*cosq*sinq + X[2]*cos2q;	// C'
			Xrot[3]=X[3]*cosq +	X[4]*sinq;						// D'
			Xrot[4]=-X[3]*sinq + X[4]*cosq;						// E'
			
			//determine center of ellipse (h, k)
			h=(-Xrot[3]/(2*Xrot[0]));		//-D'/(2A')
			k=(-Xrot[4]/(2*Xrot[2]));		//-E'/(2C')

			//check if center is in the image, if not, ellipse is rejected
			if ((h+offsetX>=imWidth)||(h+offsetX<0)||(k+offsetY>=imHeight)||(k+offsetY<0))
				return -1;

			//determine major and minor axes a and b
			a2=sqrt((Xrot[3]*Xrot[3])/(4*Xrot[0]*Xrot[0]) + 
					  (Xrot[4]*Xrot[4])/(4*Xrot[0]*Xrot[2]) -
					  (1000/Xrot[0]));

			b2=sqrt((Xrot[3]*Xrot[3])/(4*Xrot[0]*Xrot[2]) + 
					  (Xrot[4]*Xrot[4])/(4*Xrot[2]*Xrot[2]) -
					  (1000/Xrot[2]));

			//CHECK TO SEE WHAT A2 AND B2 REALLY DO!!!!!!!!!
			if ((a2<5)||(b2<5)) // if curve is too flat, reject
				return -1;

			/*//determine c; c^2=a2-b2, a>b>0
			double c=sqrt(fabs(a2-b2));

			//determine foci f1rot and f2rot
			double f1rotX=c+h;
			double f1rotY=k;
			double f2rotX=h-c;
			double f2rotY=k;

			//determine loci: actual foci of ellipse in image
			loci1X=(c+h)*cosq-k*sinq;	//globals loci1X...
			loci1Y=(c+h)*sinq+k*cosq;
			loci2X=(h-c)*cosq-k*sinq;
			loci2Y=(h-c)*sinq+k*cosq;*/

			Xrotdone=true;

			/*CvPoint center;
			CvSize size;
			if (a2<0)
				a2=a2*(-1);
			if (b2<0)
				b2=b2*(-1);
			center.x = cvRound(h+offsetX);
			center.y = cvRound(k+offsetY);
			size.width = cvRound(a2);
			size.height = cvRound(b2);
			cvEllipse(el, center, size, 0, 0, 360, 200, 1);*/  
		
		}
	} else {	//B==0
		h=(-X[3]/(2*X[0]));		//-D/(2A)
		k=(-X[4]/(2*X[2]));		//-E/(2C)
	}

	//equation of line from x1, y1 to center of ellipse (h, k)
	double U=x1-h;
	double V=y1-k;
	
	double a=X[0]*U*U + X[1]*U*V + X[2]*V*V; 
	double b=2*X[0]*U*x1 + X[1]*(V*x1+U*y1) + 2*X[2]*V*y1 + X[3]*U + X[4]*V;
	double c=X[0]*x1*x1 + X[1]*x1*y1 + X[2]*y1*y1 + X[3]*x1 + X[4]*y1 - 1000; //F=-1000
	
	//at^2 + bt + c = 0     solve for t to find intersects with ellipse 
	double rootb2m4ac=0;
	if (b*b>4*a*c)
		rootb2m4ac=sqrt(b*b-4*a*c);
	double t1=(-b+rootb2m4ac)/(2*a);	//first solution
	double t2=(-b-rootb2m4ac)/(2*a);	//second solution to quadratic equation
	
	double inter1x=x1 + U*t1;	//points of intersection on ellipse
	double inter1y=y1 + V*t1;
	double inter2x=x1 + U*t2;
	double inter2y=y1 + V*t2;

	//draw x1, y1 and intersects
	((uchar*)(el->imageData + (int)(y1+offsetY)*el->widthStep))[(int)(x1+offsetX)]=150;

	if ((inter1x+offsetX<imWidth)&&(inter1x+offsetX>=0)&&(inter1y+offsetY>=0)&&(inter1y+offsetY<imHeight)&&
		(inter2x+offsetX<imWidth)&&(inter2x+offsetX>=0)&&(inter2y+offsetY>=0)&&(inter2y+offsetY<imHeight)) {	//point is in the image
		((uchar*)(el->imageData + (int)(inter1y+offsetY)*el->widthStep))[(int)(inter1x+offsetX)]=255;
		((uchar*)(el->imageData + (int)(inter2y+offsetY)*el->widthStep))[(int)(inter2x+offsetX)]=255;
	}

	//find and return distance to closest intersect
	double distance1=sqrt((x1-inter1x)*(x1-inter1x) + (y1-inter1y)*(y1-inter1y));
	double distance2=sqrt((x1-inter2x)*(x1-inter2x) + (y1-inter2y)*(y1-inter2y));
	double intersectDis=sqrt((inter1x-inter2x)*(inter1x-inter2x) + (inter1y-inter2y)*(inter1y-inter2y));

	if ((distance1<-1000000)||(distance1>1000000)||(distance2<-1000000)||(distance2>1000000)){
		//printf(" /n");	//error catching routine... don't worry about it
	}

	//find % how close x1, y1 is to ellipse 
	double D1=0;
	double prc=0;	//percentage of closeness D1/D2
	if (distance1>=distance2) {
		D1=distance2;
	} else {
		D1=distance1;
	}
	prc=D1/intersectDis;

	return prc;
}

//returns true if given point x1, y1 is in elipse, Xnew is double array size 5
bool components::isInElipse(double *Xnew, CvPoint point) {

	double x1 = point.x - Xnew[7]; // offset X
	double y1 = point.y - Xnew[8]; // offset Y

	//equation of line from x1, y1 to center of ellipse (h, k)
	double U=x1-Xnew[5];//h
	double V=y1-Xnew[6];//k
	
	double a=Xnew[0]*U*U + Xnew[1]*U*V + Xnew[2]*V*V; 
	double b=2*Xnew[0]*U*x1 + Xnew[1]*(V*x1+U*y1) + 2*Xnew[2]*V*y1 + Xnew[3]*U + Xnew[4]*V;
	double c=Xnew[0]*x1*x1 + Xnew[1]*x1*y1 + Xnew[2]*y1*y1 + Xnew[3]*x1 + Xnew[4]*y1 - 1000; //F=-1000
	
	//at^2 + bt + c = 0     solve for t to find intersects with ellipse 
	double rootb2m4ac=0;
	if (b*b>4*a*c)
		rootb2m4ac=sqrt(b*b-4*a*c);
	double t1=(-b+rootb2m4ac)/(2*a);	//first solution
	double t2=(-b-rootb2m4ac)/(2*a);	//second solution to quadratic equation
	
	double inter1x=x1 + U*t1;	//points of intersection on ellipse
	double inter1y=y1 + V*t1;
	double inter2x=x1 + U*t2;
	double inter2y=y1 + V*t2;

	//find distances between the original point and intersects
	double distance1=sqrt((x1-inter1x)*(x1-inter1x) + (y1-inter1y)*(y1-inter1y));
	double distance2=sqrt((x1-inter2x)*(x1-inter2x) + (y1-inter2y)*(y1-inter2y));
	double intersectDis=sqrt((inter1x-inter2x)*(inter1x-inter2x) + (inter1y-inter2y)*(inter1y-inter2y));

	//find % how close x1, y1 is to ellipse 
	if ((distance1+distance2)<(1.05*intersectDis))	//1.05 is an experimental parameter you can make it larger or smaller depending on results
		return true;
	else
		return false;
}

int components::isTit(std::vector<ellipse*>& listOfElipses, std::vector<struct Nipple*>& nippleCenters, IplImage *originalImage) {
	/*listOfElipses is a list of size (n x 5+2) where n is the number of elipses that 
	  are considered elipses by the algorithm before this method is run*/
	/*centers is a list of points that you generate, Rob, from your list of nipples found
	  in each image*/
	/*original image is the original image on which we will draw these tits once the algorithm
	  has found them. I don't think that this original image is directly accessible from
	  this method, but it isn't hard to work around that*/

	/******************************************************************************

	ONE MORE VERY IMPORTANT THING, YOU MUST EXTRACT h and k ONCE AN ELIPSE IS FOUND
	YOU CAN APPEND THEM TO THE X ARRAY AND MAKE IT SIZE 7 (5 + 2). THIS IS NEEDED TO 
	AVOID RECALCULATING THEM HERE

	*******************************************************************************/
	
	int numTits = 0;

	for (int i=0; i<listOfElipses.size(); ++i) { //for each elipse
		for (int j=0; j<nippleCenters.size(); j++) {	//for each nipple
			bool e=false;
			
			// Extract current Ellipse
			ellipse * ell = ((ellipse*)listOfElipses[i]);
			
			// Extract current Nipple
			struct Nipple * nip =  ((struct Nipple*)nippleCenters[j]);
			
			// Is the nipple near the ellipse?
			e=isInElipse(ell->data, nip->CenterOfRectangle()  );			
			if (e){
				numTits++;
				//draw a box around the nipple in the original image here	

				nip->drawCenterPoint(originalImage);
				
			}
		}
	}

	printf("components::isTit - #%d tits found!\n",numTits);
	return numTits;	//actually just return the number of tits.
}


void components::findAverages(double **Avs, double *res, int count){
	double av=0;
	for (int i=0; i<5; i++) {
		for (int j=0; j<count; j++) {
			av+=Avs[i][j];
		}
		av=av/((double)count);
		res[i]=av;
		av=0;
	}
}

void components::findMedians(double **Avs, double *res, int count){	

	int av=count/2;
	for (int i=0; i<5; i++) {
		//double *a=Avs[i];
		iquicksort(Avs[i], 0, count);
		
		//print Avs
		/*printf("column %i, count = %i\n\n", i, count);
		for (int y=0; y<count; y++) {
			printf("%9.9f\n", Avs[i][y]);
		}

		printf("\n");*/

		res[i]=Avs[i][av];
	}
}

double **components::Matrix(int x, int y) {
	double **M;
	M=new double *[x];
	for (int r=0; r<x; r++) {
		M[r]=new double[y];
	}
	
	ClearMatrix(M,x,y);
	
	return M;
}

void components::DeleteMatrix(double **M, int nSize){
	// Release memory of Dynamic Array
  for(int i=0; i< nSize; i++){
    delete[] M[i]; 
  }
  delete[] M;
  M=NULL;
}

void components::ClearMatrix(double **M, int x, int y){
	for (int r=0; r<x; r++) {
		for (int k=0; k<y; k++) {
			M[r][k]=0;
		}
	}
}

void iswap(double *a, int p, int q) {
        double temp;

        temp  = a[p];
        a[p]  = a[q];
        a[q]  = temp;
}

int isplit(double *a, int f, int l) {
        double    x;
        int      u, sp;
        void  iswap(double *a, int p, int q);

        x  = a[f];
        sp = f;
        for (u = f + 1; u <= l; ++u){
                if( a[u] < x){
                        ++sp;
                        iswap(a, u, sp);
                }
        }

        iswap(a, f, sp);
        return(sp);
}

void components::iquicksort(double *a, int fst, int lst) {
        int     splitpt;
        int     isplit(double *a, int f, int l);

        if (fst < lst) {
                splitpt = isplit(a, fst, lst);
                iquicksort(a, fst, splitpt - 1);
                iquicksort(a, splitpt +1, lst);
        }
}

#ifdef _EiC
main(1,"components.cpp");
#endif