#include "AutoControlPoints.h"
//------------------------------------------------------------------------------------------------------------------------------------------
//CLASS: AutoControlPoints -----------------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------------------------------------------
//Constructor
AutoControlPoints::AutoControlPoints()
{
	 _pathsize = 0;
	 _numspline = 100;
}
//Destructor
AutoControlPoints::~AutoControlPoints()
{
}
//------------------------------------------------------------------------------------------------------------------------------------------
int AutoControlPoints::GetSizeVector(std::vector<double>*Vector)
   {
	   if(Vector != NULL)
	   {
			return _SizeVectorIn = Vector->size();
	   }
	   else
	   {
			return _SizeVectorIn = -1;
	   }
   }
//------------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::PointLeft ( std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ,
								double* lex, double* ley, double* lez )
{
	double LeftX = 1000;
	int pos = 0;
	int size = GetSizeVector(InX);
	if(size != -1)
	{
		for(int i=0; i< size; i++)
		{
			if(  (*InX)[i] < LeftX  )
			{
				LeftX = (*InX)[i];
				pos = i;
			}
		}
		*lex = (*InX)[pos];
		*ley = (*InY)[pos];
		*lez = (*InZ)[pos];
	}
}
//------------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::PointRight( std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ,
								double* rix, double* riy, double* riz )
{
	  double RightX = 0;
	  int pos = 0;
	  int size = GetSizeVector(InX);
	  if(size != -1)
	  {
		for(int i=0; i< size; i++)
		{
		  if(  (*InX)[i] > RightX  )
		  {
			  RightX = (*InX)[i];
			  pos = i;	
		  }
		}
		*rix = (*InX)[pos];
		*riy = (*InY)[pos];
		*riz = (*InZ)[pos];
	  }
}
//------------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::PointHigh ( std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ,
								double* hix, double* hiy, double* hiz )
{
	double HighY=-1;
	int pos = 0;
	int size = InX->size();
	if(size > 0){
		HighY = (*InY)[0];
		for(int i = 1; i < size; i++){
			if((*InY)[i] < HighY){
				HighY = (*InY)[i];
				pos = i;
			}
		}
		*hix = (*InX)[pos];
		*hiy = (*InY)[pos];
		*hiz = (*InZ)[pos];
	}
	
/*JCP 29-09-08
	int size = GetSizeVector(InX);
	if(size != -1)
	{
		for(int i=0; i< _SizeVectorIn; i++)
		{
			if(  (*InY)[i] < HighY  )
			{
				HighY = (*InY)[i];
				pos = i;
			}
		}
		*hix = (*InX)[pos];
		*hiy = (*InY)[pos];
		*hiz = (*InZ)[pos];
	}
JCP 29-09-08*/
	
}

//------------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::PointLow (	std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ,
									double *lox, double *loy, double *loz)
{
	double LowY = 0;
	int pos = 0;
	int size = GetSizeVector(InX);
	if(size != 0)
	{
		for(int i=0; i< _SizeVectorIn; i++)
		{
			if(  (*InY)[i] > LowY  )
			{
				LowY = (*InY)[i];
				pos = i;
			}
		} 
		*lox = (*InX)[pos];
		*loy = (*InY)[pos];
		*loz = (*InZ)[pos];
	}
}
//------------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::TwoPoints ( std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ )
{
	double hiX=0,hiY=0,hiZ=0;
	PointHigh ( InX,InY,InZ,&hiX,&hiY,&hiZ );
	
	double leX=0,leY=0,leZ=0;
	PointLeft ( InX,InY,InZ,&leX,&leY,&leZ );
	
	double loX=0,loY=0,loZ=0;
	PointLow  ( InX,InY,InZ,&loX,&loY,&loZ );

	double riX=0,riY=0,riZ=0;
	PointRight( InX,InY,InZ,&riX,&riY,&riZ );

	double distHiLo = sqrt( pow(hiX-loX,2) + pow(hiY-loY,2) );
	double distRiLe = sqrt( pow(riX-leX,2) + pow(riY-leY,2) );
		
	_controlpointsX.clear();
	_controlpointsY.clear();
	_controlpointsZ.clear();
	if(distHiLo >= distRiLe)
	{
		_controlpointsX.push_back(hiX);
		_controlpointsY.push_back(hiY);
		_controlpointsZ.push_back(hiZ);
			
		_controlpointsX.push_back(loX);
		_controlpointsY.push_back(loY);
		_controlpointsZ.push_back(loZ);
	}
	else
	{
		_controlpointsX.push_back(riX);
		_controlpointsY.push_back(riY);
		_controlpointsZ.push_back(riZ);

		_controlpointsX.push_back(leX);
		_controlpointsY.push_back(leY);
		_controlpointsZ.push_back(leZ);
	}
}
//------------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::CircleCenter(std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ, double *cx, double *cy, double *r)
{
	double hiX=0,hiY=0,hiZ=0;
	PointHigh ( InX,InY,InZ,&hiX,&hiY,&hiZ );

	double leX=0,leY=0,leZ=0;
	PointLeft ( InX,InY,InZ,&leX,&leY,&leZ );
		
	double loX=0,loY=0,loZ=0;
	PointLow  ( InX,InY,InZ,&loX,&loY,&loZ );

	double riX=0,riY=0,riZ=0;
	PointRight( InX,InY,InZ,&riX,&riY,&riZ );

	*cx = (riX+leX)/2;
	*cy = (hiY+loY)/2;
	*r = sqrt(pow(leX-*cx,2)+pow(hiY-*cy,2)) + 7;
}
//------------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::CirclePoints(double cx, double cy, double r, double grad, double *x, double *y)
{
	double alpha = (2*3.14159265*grad)/360;

	*x = cx + (r*cos(alpha));
	*y = cy + (r*sin(alpha));
}
//------------------------------------------------------------------------------------------------------------------------------------------
 void AutoControlPoints::ChargeSpline( )
 {
	 int size = _controlpointsX.size();
	 if(size != 0)
	 {
		_mContourModel = new manualContourModel();
		if( _controlpointsX.size() == 2 )
		{
			_mContourModel->SetCloseContour(false);
		}
		if( _controlpointsX.size() > 2 )
		{
			_mContourModel->SetCloseContour(true);
		}

		_mContourModel->DeleteAllPoints();
		_mContourModel->SetNumberOfPointsSpline(_numspline);
		for(int i=0; i<size; i++)
		{
			_mContourModel->AddPoint(_controlpointsX[i],_controlpointsY[i],_controlpointsZ[i]);
		}
		_mContourModel->UpdateSpline();
		int numspline = _mContourModel->GetNumberOfPointsSpline();
		double x,y,z;
		_chargecontrolpointsX.clear();
		_chargecontrolpointsY.clear();
		_chargecontrolpointsZ.clear();
		for(int j=0; j<numspline; j++)
		{
			_mContourModel->GetSpline_i_Point(j,&x,&y,&z);
			_chargecontrolpointsX.push_back(x);
			_chargecontrolpointsY.push_back(y);
			_chargecontrolpointsZ.push_back(z);
		}
		_pathsize = _mContourModel->GetPathSize();
//printf("\nPATH SIZE = %f",_pathsize);

		 std::ofstream file1;
		file1.open( "4_SplinePoints.txt" );

		for(int i = 0; i < numspline; i++){
			file1<<"X= "<<_chargecontrolpointsX[i] << "\tY= "<<_chargecontrolpointsY[i] << "\tZ= "<<_chargecontrolpointsZ[i]<<std::endl;
		}
		file1.close();
	 }
	
 }
 //Given the coordinates of two points, it calculates the slope
double AutoControlPoints::Slope(double x0, double y0, double x1, double y1)
{
	double m = (y1-y0)/(x1-x0);
	return m;
}
//----------------------------------------------------------------------------------------------------------------------------------------
//Given the coordinates of two points, it calculates the normal and it's slope 
double AutoControlPoints::Normal(double x0, double y0, double* m, double xi)
{
	double y;
	*m = -(1/(*m));
	y = ((*m)*(xi - x0)) + y0;
	return y;
}
//----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::Intersection(double x01, double y01, double x02, double y02, double mn, double m2, double* x, double* y)
{
	*x = ( y02-y01-(m2*x02)+(mn*x01) )/(mn-m2);
	*y = m2*(*x-x02)+y02;
}
//-----------------------------------------------------------------------------------------------------------------------------------------
/**
**This methods finds the points where each radius of the circle intersect the contour
**/
void AutoControlPoints::InterCircle(std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ)
{
	double cx,cy,r;
	CircleCenter(InX,InY,InZ,&cx,&cy,&r);

	//GENERATE THE CIRCLE'S POINTS
	int i;
	double grad,x,y,n;
	std::vector<double>tempX;
	std::vector<double>tempY;
	tempX.clear();
	tempY.clear();
	n = 1;
	grad = 0;
	for(i=0; i<360/n; i++)
	{
		CirclePoints(cx,cy,r,grad,&x,&y);
		tempX.push_back(x);
		tempY.push_back(y);
		grad = grad + n;
	}

	//FIND THE INTERSECTIONS BETWEEN THE CIRCLE AND THE CONTOUR
	int j,jj;
	bool interRad = false;
	double m1, /*mn=0,*/ m2,xinter,yinter,xmin,ymin,min,dist;
	_intercircleX.clear();
	_intercircleY.clear();
	_intercircleDist.clear();
	_interbewteencircleX.clear();
	_interbewteencircleY.clear();
	_interbewteencircleDist.clear();
	_interbewteencirclePos.clear();

//EED 22 Sep 2008
//	FILE *fd, *fexp;
//	fd = fopen("C:/bbtk_JS/data/tempCircle.txt","w");
//	fexp = fopen("C:/bbtk_JS/data/InterCircle.txt","w");
//	fprintf(fexp,"\npos	min		xmin	ymin	xcir	ycir");

	//std::ofstream file1;
    //file1.open( "Temp.txt" );	
	
	for(i=0; i<tempX.size(); i++)
	{
//		fprintf(fd,"\n Para X = %f, Y = %f",tempX[i],tempY[i]);
		_circleX.push_back(tempX[i]);
		_circleY.push_back(tempY[i]);
		m1 = Slope(tempX[i],tempY[i],cx,cy);//slope of the radius
		min = 9999;
		for(j=0; j<InX->size(); j++)
		{
			jj = (j+1)%(InX->size());
			m2 = Slope((*InX)[j],(*InY)[j],(*InX)[jj],(*InY)[jj]);//Slope of the vector between the adjacent points
			Intersection(tempX[i],tempY[i],(*InX)[j],(*InY)[j],m1,m2,&xinter,&yinter);

//JCP 26-09-2008
			//If the point of intersection is between two points of the contour
			if( ((xinter>=(*InX)[j]) && (xinter<=(*InX)[jj]))||((xinter<=(*InX)[j]) && (xinter>=(*InX)[jj]) ))
			{
				dist = sqrt(pow(tempX[i]-xinter,2) + pow(tempY[i]-yinter,2));
				if(dist<min)
				{
					min = dist;
					xmin = xinter;
					ymin = yinter;
				}
			}
//JCP 26-09-2008
/*JCP 26-09-2008
			if((*InX)[j]<=(*InX)[jj])
			{
				if( (xinter>=(*InX)[j]) && (xinter<=(*InX)[jj]) )		//Intersection entre le cercle et le contour
				{
					dist = sqrt(pow(tempX[i]-xinter,2) + pow(tempY[i]-yinter,2));
					if(dist<min)
					{
						min = dist;
						xmin = xinter;
						ymin = yinter;
					}
				}
			}
			if((*InX)[j]>(*InX)[jj])
			{
				if( (xinter<=(*InX)[j]) && (xinter>=(*InX)[jj]) )		//Intersection entre le cercle et le contour
				{
					dist = sqrt(pow(tempX[i]-xinter,2) + pow(tempY[i]-yinter,2));
					if(dist<min)
					{
						min = dist;
						xmin = xinter;
						ymin = yinter;
					}
				}
			}
JCP 26-09-2008*/
		}



//		fprintf(fd,"\n => x_int = %f, y_int = %f, dist_int = %f",xmin,ymin,min);
//		fprintf(fexp,"\n%d	%f	%f	%f	%f	%f",i,min,xmin,ymin,tempX[i],tempY[i]);
//JCP 26-09-08 If the distance of the intersection is bigger than the radio we have to invert the segment
		if(min>=r)
		{
			interRad = true;
			_interbewteencirclePos.push_back(i);
			_interbewteencircleX.push_back(xmin);
			_interbewteencircleY.push_back(ymin);
			_interbewteencircleDist.push_back( sqrt(pow(cx-xmin,2)+pow(cy-ymin,2)) );

			//	file1<<i<<std::endl;
		}else{
//JCP 26-09-08		if(min<r)
//JCP 26-09-08		{
			_intercircleX.push_back(xmin);
			_intercircleY.push_back(ymin);
			_intercircleDist.push_back(min);
		//	file1<<"\t"<<i<<std::endl;
		}
	}
	//file1.close();
//	fclose(fd);
//	fclose(fexp);

	//WHEN THERE IS RADIAL INTERSECTION
	vectorFunctions *vecf = new vectorFunctions();

//EED 22 Sep 2008
//	FILE *fdata;
//	fdata = fopen("C:/bbtk_JS/data/autoCPdata.txt","w");
	if(interRad == true)
	{
		std::vector<double> tempXX;
		std::vector<double> tempYY;
		std::vector<double> tempDD;
		tempXX.clear();
		tempYY.clear();
		tempDD.clear();
//Copy of the first points in the array until the first intersection is found		
		for(i=0; i<_interbewteencirclePos[0]; i++)
		{
			tempXX.push_back(_intercircleX[i]);
			tempYY.push_back(_intercircleY[i]);
			tempDD.push_back(_intercircleDist[i]);
//			fprintf(fdata,"\n%f	%f	%f",_intercircleDist[i],_intercircleX[i],_intercircleY[i]);
		}
		int sizep = _interbewteencirclePos.size();
//Copy all the points where there is an intersection with the center but inverted
//JCP 26-09-08		for(i=_interbewteencirclePos[sizep-1],j=sizep-1; i>=_interbewteencirclePos[0]; i--,j--)
		for(i=sizep-1; i >= 0;i--)
		{
//JCP 26-09-08			tempXX.push_back(_interbewteencircleX[j]);
//JCP 26-09-08			tempYY.push_back(_interbewteencircleY[j]);
//JCP 26-09-08			tempDD.push_back(_interbewteencircleDist[j]);
			tempXX.push_back(_interbewteencircleX[i]);
			tempYY.push_back(_interbewteencircleY[i]);
			tempDD.push_back(_interbewteencircleDist[i]);
//			fprintf(fdata,"\n%f	%f	%f",_interbewteencircleDist[j],_interbewteencircleX[j],_interbewteencircleY[j]);
		}
		for(i=_interbewteencirclePos[0]; i<_intercircleX.size(); i++)
		{
			tempXX.push_back(_intercircleX[i]);
			tempYY.push_back(_intercircleY[i]);
			tempDD.push_back(_intercircleDist[i]);
//			fprintf(fdata,"\n%f	%f	%f",_intercircleDist[i],_intercircleX[i],_intercircleY[i]);
		}
		
		_intercircleX.clear();
		_intercircleY.clear();
		_intercircleDist.clear();
		vecf->copyVector(&tempXX,&_intercircleX);
		vecf->copyVector(&tempYY,&_intercircleY);
		vecf->copyVector(&tempDD,&_intercircleDist);
	}	
//	fclose(fdata);

	//DELETE!!

	std::ofstream file1;
    file1.open( "1_Intersection.txt" );

	for(int i = 0; i < _intercircleX.size(); i++){
		file1<<"X= "<<_intercircleX[i] << "\tY= "<<_intercircleY[i] << "\tDist= "<<_intercircleDist[i]<<std::endl;
	}
	file1.close();
		

	delete vecf;
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::maxminLocal()
{
	int i;
	_posmaxlocal.clear();
	_posminlocal.clear();
	_posminmaxlocal.clear();
	_maxlocalX.clear();
	_maxlocalY.clear();
	_minlocalX.clear();
	_minlocalY.clear();
	_minmaxlocalX.clear();
	_minmaxlocalY.clear();
	
	if(_intercircleDist.size() != 0)
	{
//JCP 26 - 09 - 08 This change was du to the posibility of having a maximum or a minimum value in the limits of 
//JCP 26 - 09 - 08 the array
		double lastdist, currentdist, nextdist;
		for(i=0; i < _intercircleDist.size(); i++)
		{
			//FOR MAXIMUM LOCAL
//JCP 26-09-08
			currentdist = _intercircleDist[i];
			if(i == 0){
				lastdist = _intercircleDist[_intercircleDist.size()-1];
				nextdist = _intercircleDist[i+1];
			}else if (i == _intercircleDist.size()-1){
				lastdist = _intercircleDist[i-1];
				nextdist = _intercircleDist[0];
			}else{
				lastdist = _intercircleDist[i-1];				
				nextdist = _intercircleDist[i+1];
			}
			

//JCP 26-09-08			if( (_intercircleDist[i-1]<_intercircleDist[i]) && (_intercircleDist[i]>_intercircleDist[i+1]))
			if(lastdist < currentdist && currentdist > nextdist)
			{
				_posmaxlocal.push_back(i);
				_maxlocalX.push_back(_intercircleX[i]);
				_maxlocalY.push_back(_intercircleY[i]);
				_minmaxlocalX.push_back(_intercircleX[i]);
				_minmaxlocalY.push_back(_intercircleY[i]);
				_posminmaxlocal.push_back(i);
			}
			//FOR MINIMUM LOCAL
//JCP 26-09-08			if( (_intercircleDist[i-1]>_intercircleDist[i]) && (_intercircleDist[i]<_intercircleDist[i+1]))
			if(lastdist > currentdist && currentdist < nextdist)
			{
				_posminlocal.push_back(i);
				_minlocalX.push_back(_intercircleX[i]);
				_minlocalY.push_back(_intercircleY[i]);
				_minmaxlocalX.push_back(_intercircleX[i]);
				_minmaxlocalY.push_back(_intercircleY[i]);
				_posminmaxlocal.push_back(i);
			}
		}
	}

	vectorFunctions	*vecf	= new vectorFunctions();
	std::vector<double> tempZ;
	tempZ.clear();
	
	vecf->copyVector(&_minlocalX,&_controlpointsX);	
	vecf->copyVector(&_minlocalY,&_controlpointsY);
	for(i=0; i<_minlocalX.size(); i++)
	{
		tempZ.push_back(_controlpointsZ[0]);
	}
	vecf->copyVector(&tempZ,&_controlpointsZ);

//JCP 26-09-08	
	std::ofstream file1;
    file1.open( "2_MaxMin.txt" );

	for(int i = 0; i < _controlpointsX.size(); i++){
		file1<<"X= "<<_controlpointsX[i] << "\tY= "<<_controlpointsY[i] << "\tZ= "<<_controlpointsZ[i]<<std::endl;
	}
	file1.close();
//JCP 26-09-08
	delete vecf;
}
//-----------------------------------------------------------------------------------------------------------------------------------------
//ELIMINATES THE POINTS WITH A DISTANCE < val
void AutoControlPoints::fixBetweenPoints(double val)
{
	int size = _controlpointsX.size();
	double dist;
	if(size != 0)
	{
		std::vector<double> tempX;
		std::vector<double> tempY;
		std::vector<double>	tempZ;
		tempX.clear();
		tempY.clear();
		tempZ.clear();

		int ii;
		vectorFunctions *vecf = new vectorFunctions();
		for(int i=0; i<size; i++)
		{
			ii = (i+1)%size;
			dist = sqrt(pow(_controlpointsX[i]-_controlpointsX[ii],2)+pow(_controlpointsY[i]-_controlpointsY[ii],2));
			if(dist>val)
			{
				tempX.push_back(_controlpointsX[i]);
				tempY.push_back(_controlpointsY[i]);
				tempZ.push_back(_controlpointsZ[i]);
			}
		}
		_controlpointsX.clear();
		_controlpointsY.clear();
		_controlpointsZ.clear();

		vecf->copyVector(&tempX,&_controlpointsX);
		vecf->copyVector(&tempY,&_controlpointsY);
		vecf->copyVector(&tempZ,&_controlpointsZ);

		std::ofstream file1;
		file1.open( "3_PointsFixed.txt" );

		for(int i = 0; i < _controlpointsX.size(); i++){
			file1<<"X= "<<_controlpointsX[i] << "\tY= "<<_controlpointsY[i] << "\tZ= "<<_controlpointsZ[i]<<std::endl;
		}
		file1.close();

		delete vecf;
	}
}
//-----------------------------------------------------------------------------------------------------------------------------------------
//ALL THE INTERSECTIONS
void AutoControlPoints::InterBetweenContours(std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ)
{
	_intervectorX.clear();
	_intervectorY.clear();
	ChargeSpline();

	int i,ii,j,jj;
	double m1,mn,m2,xinter,yinter;
	if(_chargecontrolpointsX.size() > 1) //These condition exists because there is a method for find the initial control points 
	{
//EED 22 Sep 2008
//		FILE *fd;
//		fd = fopen("C:/bbtk_JS/data/interBetweenContours.txt","w");
		for(i=0; i<_chargecontrolpointsX.size(); i++)
		{
			ii = (i+1)%(_chargecontrolpointsX.size());

			m1 = Slope(_chargecontrolpointsX[i],_chargecontrolpointsY[i],_chargecontrolpointsX[ii],_chargecontrolpointsY[ii]);
			mn = m1;
			Normal(_chargecontrolpointsX[i],_chargecontrolpointsY[i],&mn,_chargecontrolpointsX[i]+1);
//			fprintf(fd,"\n Para X = %f, Y = %f",_chargecontrolpointsX[i],_chargecontrolpointsY[i]);

			Vector *vecX = new Vector();
			Vector *vecY = new Vector();
			vecX->set_var(_chargecontrolpointsX[i]);
			vecY->set_var(_chargecontrolpointsY[i]);

			for(j=0; j<InX->size(); j++)
			{
				jj = (j+1)%(InX->size());
				m2 = Slope((*InX)[j],(*InY)[j],(*InX)[jj],(*InY)[jj]);
				Intersection(_chargecontrolpointsX[i],_chargecontrolpointsY[i],(*InX)[j],(*InY)[j],mn,m2,&xinter,&yinter);

				if(((*InX)[j] <= xinter && xinter <= (*InX)[jj]) || (xinter<=(*InX)[j] && xinter>=(*InX)[jj])){
					vecX->set_vec(xinter);
					vecY->set_vec(yinter);
				}
/*JCP 29-09-08
				if( (*InX)[j]<=(*InX)[jj] )
				{
					if( (xinter>=(*InX)[j]) && (xinter<=(*InX)[jj]) )
					{
						//If the point is a CP, the intersection is itself.
						if((xinter==_chargecontrolpointsX[i]) && (yinter==_chargecontrolpointsY[i]))
						{
							vecX->set_vec(xinter);
							vecY->set_vec(yinter);
//							fprintf(fd,"\n => x_int = %f, y_int = %f",xinter,yinter);
						}
						else
						{
							vecX->set_vec(xinter);
							vecY->set_vec(yinter);
//							fprintf(fd,"\n => x_int = %f, y_int = %f",xinter,yinter);
						}
					}
				}
				if( (*InX)[j]>(*InX)[jj] )
				{
					if( (xinter<=(*InX)[j]) && (xinter>=(*InX)[jj]) )
					{
						//If the point is a CP, the intersection is itself.
						if((xinter==_chargecontrolpointsX[i]) && (yinter==_chargecontrolpointsY[i]))
						{
							vecX->set_vec(xinter);
							vecY->set_vec(yinter);
//							fprintf(fd,"\n => x_int = %f, y_int = %f",xinter,yinter);
						}
						else
						{
							vecX->set_vec(xinter);
							vecY->set_vec(yinter);
//							fprintf(fd,"\n => x_int = %f, y_int = %f",xinter,yinter);
						}
					}
				}
JCP 29-09-08*/
			}//FOR2
			_intervectorX.push_back(*vecX);
			_intervectorY.push_back(*vecY);
			//DELETE!!
			delete vecX;
			delete vecY;
		}//FOR1
//		fclose(fd);
	}//IF
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::GetInterBetweenContours(std::vector<Vector>*interVX, std::vector<Vector>*interVY)
{
	interVX->clear();
	interVY->clear();
	int size = _intervectorX.size();
	int i;
	if(size != 0)
	{
		for(i=0; i<size; i++)
		{
			interVX->push_back(_intervectorX[i]);
			interVY->push_back(_intervectorY[i]);
		}
	}
}
//-----------------------------------------------------------------------------------------------------------------------------------------
//ONLY THE LOGICAL INTERSECTIONS
void AutoControlPoints::IntersectionPoints()
{
	if(_intervectorX.size() != 0)
	{		
		_interpointsX.clear();
		_interpointsY.clear();

//EED
//		FILE *fd;
//		fd = fopen("C:/bbtk_JS/data/IntersectionPoints.txt","w");
		double dist,min;
		int i,j,posj;

		posj = -1;
		min = 9999;
/*JCP 30-08-09 
		for(j=0; j<_intervectorX[0].getsize_vec(); j++)
		{
			dist = sqrt( pow( _intervectorX[0].get_vec(j)-_intervectorX[0].get_var(),2 ) + pow( _intervectorY[0].get_vec(j)-_intervectorY[0].get_var(),2 ) );
			if( dist < min )
			{
				min = dist;
				posj = j;
			}
		}
		if(posj != -1)
		{
			_interpointsX.push_back(_intervectorX[0].get_vec(posj));
			_interpointsY.push_back(_intervectorY[0].get_vec(posj));
//			fprintf(fd,"\n Para X = %f, Y = %f",_intervectorX[0].get_var(),_intervectorY[0].get_var());
//			fprintf(fd,"\n => x_int = %f, y_int = %f",_interpointsX[0],_interpointsY[0]);
		}
		if(posj == -1)
		{
			printf("\n\n There is an invalid intersection: Must see AutoControlPoints::IntersectionPoints() method");
		}
JCP 30-08-09 */
		for(i=0; i<_intervectorX.size(); i++){
			min = 9999;
			posj = -1;
			for(j=0; j<_intervectorX[i].getsize_vec(); j++)	{
				dist  = sqrt( pow( _intervectorX[i].get_vec(j)-_intervectorX[i].get_var(),2 ) + pow( _intervectorY[i].get_vec(j)-_intervectorX[i].get_var(),2 ) );
				if( dist < min ){
					min = dist;
					posj = j;
				}
			}
			_interpointsX.push_back(_intervectorX[i].get_vec(posj));
			_interpointsY.push_back(_intervectorY[i].get_vec(posj));		
		}
/*JCP 30-09-08
		for(i=1; i<_intervectorX.size(); i++)
		{
			min = 9999;
			posj = -1;
//			fprintf(fd,"\n Para X = %f, Y = %f",_intervectorX[i].get_var(),_intervectorY[i].get_var());
			for(j=0; j<_intervectorX[i].getsize_vec(); j++)
			{
				//TYPE: LE PLUS PRES VOISIN
				dist  = sqrt( pow( _intervectorX[i].get_vec(j)-_interpointsX[i-1],2 ) + pow( _intervectorY[i].get_vec(j)-_interpointsY[i-1],2 ) );
				//TYPE: LE PLUS PRES DANS LA MÊME DROITE
				//dist = sqrt(pow(_intervectorX[i].get_vec(j)-_intervectorX[i].get_var(),2)+pow(_intervectorY[i].get_vec(j)-_intervectorY[i].get_var(),2));
				
				if( dist < min )
				{
					min = dist;
					posj = j;
				}
			}
			_interpointsX.push_back(_intervectorX[i].get_vec(posj));
			_interpointsY.push_back(_intervectorY[i].get_vec(posj));
//			fprintf(fd,"\n => x_int = %f, y_int = %f",_interpointsX[i],_interpointsY[i]);
		}
JCP 30-09-08*/
//		fclose(fd);
	}
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::GetIntersectionPoints(std::vector<Vector>*interVX, std::vector<Vector>*interVY)
{
	int size = _interpointsX.size();
	int i;
	if(size != 0)
	{
		Vector *vecX = new Vector();
		Vector *vecY = new Vector();
		interVX->clear();
		interVY->clear();
		for(i=0; i<size; i++)
		{
			vecX->set_var(_controlpointsZ[0]);
			vecX->set_vec(_interpointsX[i]);
			vecY->set_var(_controlpointsZ[0]);
			vecY->set_vec(_interpointsY[i]);
			interVX->push_back(*vecX);
			interVY->push_back(*vecY);
		}
		delete vecX;
		delete vecY;
	}
}
//-----------------------------------------------------------------------------------------------------------------------------------------
//ERROR BETWEEN THE LOGICAL INTERSECTIONS
void AutoControlPoints::ErrorBetweenContours()
{
	_errorpos = -1;
	if(_interpointsX.size() != 0)
	{
		_errorvector.clear();
		int i;
//EED 22 Sep 2008

//		FILE *fd;
//		fd = fopen("C:/bbtk_JS/data/interErrorData.txt","w");
		for(i=0; i<_interpointsX.size(); i++)
		{
			_errorvector.push_back( (sqrt( pow( _interpointsX[i]-_intervectorX[i].get_var(),2 ) + pow( _interpointsY[i]-_intervectorY[i].get_var(),2 ) )/_pathsize)*100 );
//			fprintf(fd,"\n%d	%f",i,_errorvector[i]);
		}
//		fclose(fd);
		double max = -1;
		for(i=0; i<_errorvector.size(); i++)
		{
			if(_interpointsX[i] != -1)
			{
				if(_errorvector[i]>max)
				{
					max = _errorvector[i];
					_errorpos = i;
				}
			}
		}
	}
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::GetErrorBetweenContours( std::vector<double>*vec )
{
	vec->clear();
	vectorFunctions *vf = new vectorFunctions();
	vf->copyVector(&_errorvector,vec);
	delete vf;
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::AddControlPoint(bool activate)
{
	if(_errorpos != -1)
	{
		double xmax = _interpointsX[_errorpos];
		double ymax = _interpointsY[_errorpos];
		double xx	= _intervectorX[_errorpos].get_var();
		double yy	= _intervectorY[_errorpos].get_var();
printf("\n XMAX = %f, YMAX = %f, XX = %f, YY = %f",xmax,ymax,xx,yy);

		int i,ii,j,posA=-1,posB=-1;
		bool findA=false, findB=false;
		//CASE A
		for(i=_errorpos; findA!=true; i++)
		{
			ii = i%_errorvector.size();
			for(j=0; j<_controlpointsX.size(); j++)
			{
				if( ((float)_controlpointsX[j]-1.5<=(float)_intervectorX[ii].get_var()) && ((float)_intervectorX[ii].get_var()<=(float)_controlpointsX[j]+1.5) &&
					((float)_controlpointsY[j]-1.5<=(float)_intervectorY[ii].get_var()) && ((float)_intervectorY[ii].get_var()<=(float)_controlpointsY[j]+1.5) )
				{
					findA = true;
					posA = j;
				}
			}
		}
		//CASE B
		for(i=_errorpos; findB!=true; i--)
		{
			if(_errorpos==-1)
			{
				i = _errorvector.size();
			}
			for(j=0; j<_controlpointsX.size(); j++)
			{
				if( ((float)_controlpointsX[j]-1.5<=(float)_intervectorX[i].get_var()) && ((float)_intervectorX[i].get_var()<=(float)_controlpointsX[j]+1.5) &&
					((float)_controlpointsY[j]-1.5<=(float)_intervectorY[i].get_var()) && ((float)_intervectorY[i].get_var()<=(float)_controlpointsY[j]+1.5) )
				{
					findB = true;
					posB = j;
				}
			}
		}
		if(posA == posB)
		{
			posB = posA-1;
		}
		if(posA<posB)
		{
			posA = posB+1;
			if(posB = _controlpointsX.size()-1)
			{
				posA = 0;
			}
		}
printf("\n POSA = %d, X = %f, Y = %f",posA,_controlpointsX[posA],_controlpointsY[posA]);
printf("\n POSB = %d, X = %f, Y = %f",posB,_controlpointsX[posB],_controlpointsY[posB]);
		_posA = posA;
		_posB = posB;
		int id = -1;
		if(((posA!=-1)&&(posB!=-1)))
		{
			id = posA;
		}
printf("\n ID = %d",id);
		if(id != -1)
		{
			std::vector<double> tempX;
			std::vector<double> tempY;
			std::vector<double> tempZ;
			for(i=0; i<_controlpointsX.size(); i++)
			{
				if(i == id)
				{
					tempX.push_back(xmax);
					tempY.push_back(ymax);
					tempZ.push_back(_controlpointsZ[0]);
				}
				tempX.push_back(_controlpointsX[i]);
				tempY.push_back(_controlpointsY[i]);
				tempZ.push_back(_controlpointsZ[i]);
			}
		
			if(activate == true)
			{
				vectorFunctions *vf = new vectorFunctions();
				vf->copyVector(&tempX,&_controlpointsX);
				vf->copyVector(&tempY,&_controlpointsY);
				vf->copyVector(&tempZ,&_controlpointsZ);
				delete vf;
			}
		
		}
	}//IF-(principal)
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::InterBetweenControl( )
{
	_intervecXX.clear();
	_intervecYY.clear();
	
	int i,ii,j,jj;
	double m1,mn,m2,xinter,yinter;
	if(_chargecontrolpointsX.size() > 1) //These condition exists because there is a method for find the initial control points 
	{
//EED 22 Sep 2008

//		FILE *fd;
//		fd = fopen("C:/bbtk_JS/data/InterBetweenControl.txt","w");
		for(i=0; i<_chargecontrolpointsX.size()-1; i++)
		{
			ii = (i+1)%(_chargecontrolpointsX.size());
			m1 = Slope(_chargecontrolpointsX[i],_chargecontrolpointsY[i],_chargecontrolpointsX[ii],_chargecontrolpointsY[ii]);
			mn = m1;
			Normal(_chargecontrolpointsX[i],_chargecontrolpointsY[i],&mn,_chargecontrolpointsX[i]+1);
//			fprintf(fd,"\n Para X = %f, Y = %f",_chargecontrolpointsX[i],_chargecontrolpointsY[i]);

			Vector *vecX = new Vector();
			Vector *vecY = new Vector();
			vecX->set_var(_chargecontrolpointsX[i]);
			vecY->set_var(_chargecontrolpointsY[i]);

			for(j=0; j<_chargecontrolpointsX.size(); j++)
			{
				jj = (j+1)%(_chargecontrolpointsX.size());
				m2 = Slope(_chargecontrolpointsX[j],_chargecontrolpointsY[j],_chargecontrolpointsX[jj],_chargecontrolpointsY[jj]);
				Intersection(_chargecontrolpointsX[i],_chargecontrolpointsY[i],_chargecontrolpointsX[j],_chargecontrolpointsY[j],mn,m2,&xinter,&yinter);
				if( _chargecontrolpointsX[j]<=_chargecontrolpointsX[jj] )
				{
					if( (xinter>=_chargecontrolpointsX[j]) && (xinter<=_chargecontrolpointsX[jj]) )
					{
						if(((float)xinter==(float)_chargecontrolpointsX[i]) && ((float)yinter==(float)_chargecontrolpointsY[i]))
						{
						}
						else
						{
//							fprintf(fd,"\n => x_int = %f, y_int = %f",xinter,yinter);
							vecX->set_vec(xinter);
							vecY->set_vec(yinter);
						}
					}
				}
				if( _chargecontrolpointsX[j]>_chargecontrolpointsX[jj] )
				{
					if( (xinter<=_chargecontrolpointsX[j]) && (xinter>=_chargecontrolpointsX[jj]) )
					{
						if(((float)xinter==(float)_chargecontrolpointsX[i]) && ((float)yinter==(float)_chargecontrolpointsY[i]))
						{
						}
						else
						{
//							fprintf(fd,"\n => x_int = %f, y_int = %f",xinter,yinter);
							vecX->set_vec(xinter);
							vecY->set_vec(yinter);
						}
					}
				}
			}//FOR2
			_intervecXX.push_back(*vecX);
			_intervecYY.push_back(*vecY);
			
			//DELETE!!
			delete vecX;
			delete vecY;

		}//FOR1
//		fclose(fd);
	}//IF
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::fixBetweenControl()
{
	_interitselfX.clear();
	_interitselfY.clear();
	int i,j;
	float vecx,vecy,varx,vary;
//	FILE *fd;
//	fd = fopen("C:/bbtk_JS/data/InterBetweenControlFix.txt","w");
	for(i=0; i<_intervecXX.size(); i++)
	{
		Vector *vx = new Vector();
		Vector *vy = new Vector();
		vx->set_var(_intervecXX[i].get_var());
		vy->set_var(_intervecYY[i].get_var());
//		fprintf(fd,"\n Para X = %f, Y = %f",_intervecXX[i].get_var(),_intervecYY[i].get_var());
		for(j=0; j<_intervecXX[i].getsize_vec(); j++)
		{
			vecx = _intervecXX[i].get_vec(j);
			varx = _intervecXX[i].get_var();
			vecy = _intervecYY[i].get_vec(j);
			vary = _intervecYY[i].get_var();
			if(  (vecx == varx) && (vecy == vary) )
			{
			}
			else
			{
				vx->set_vec((double)vecx);
				vy->set_vec((double)vecy);
//				fprintf(fd,"\n => x_int = %f, y_int = %f",vecx,vecy);
			}
		}
		_interitselfX.push_back(*vx);
		_interitselfY.push_back(*vy);
		delete vx;
		delete vy;
	}
//	fclose(fd);
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::PossibleIntersections( std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ )
{
	InterBetweenContours(InX,InY,InZ);	//_intervectorX
	//InterBetweenControl();				//_intervecXX
	//fixBetweenControl();				//_interitselfX
	
	std::vector<Vector> tempX;
	std::vector<Vector> tempY;
	tempX.clear();
	tempY.clear();
	int i,j,k;
	//double dist1,dist2;
//EED 22 Sep 2008
//	FILE *fd;
/*
	bool ready;
	fd = fopen("C:/bbtk_JS/data/InterPossibleIntersections.txt","w");


	for(i=0; i<_intervectorX.size(); i++)
	{
		fprintf(fd,"\n Para X = %f, Y = %f",_intervectorX[i].get_var(),_intervectorY[i].get_var());
		Vector *vx = new Vector();
		Vector *vy = new Vector();
		vx->set_var(_intervectorX[i].get_var());
		vy->set_var(_intervectorY[i].get_var());
		ready = false;
		for(j=0; j<_intervectorX[i].getsize_vec() ; j++)
		{
			dist1 = sqrt( pow(_intervectorX[i].get_var()-_intervectorX[i].get_vec(j),2) + pow(_intervectorY[i].get_var()-_intervectorY[i].get_vec(j),2) );
			for(k=0; (k<_interitselfX[i].getsize_vec()) && (ready!=true); k++)
			{
				dist2 = sqrt( pow(_interitselfX[i].get_var()-_interitselfX[i].get_vec(k),2) + pow(_interitselfY[i].get_var()-_interitselfY[i].get_vec(k),2) );
				if(dist2>dist1)
				{
					fprintf(fd,"\n => x_int = %f, y_int = %f",_intervectorX[i].get_vec(j),_intervectorY[i].get_vec(j));
					vx->set_vec(_intervectorX[i].get_vec(j));
					vy->set_vec(_intervectorY[i].get_vec(j));
					ready == true;
				}
			}
		}
		tempX.push_back(*vx);
		tempY.push_back(*vy);
		delete vx;
		delete vy;
	}
	fclose(fd);
	
	_intervectorX.clear();
	_intervectorY.clear();
	Vector *vv = new Vector();
	vv->copyVector(&tempX,&_intervectorX);
	vv->copyVector(&tempY,&_intervectorY);
	
	//vv->printVector(&_intervectorX);
*/
	std::vector<double> arrX; 
	std::vector<double> arrY;
	std::vector<double>::iterator itx;
	std::vector<double>::iterator ity;
	std::vector<double>::iterator itxx;
	std::vector<double>::iterator ityy;
	double distA, distB;
	if(_intervectorX.size() != 0)
	{
//		fd = fopen("C:/bbtk_JS/data/InterPossibleIntersections.txt","w");
		for(i=0; i<_intervectorX.size(); i++)
		{
//			fprintf(fd,"\n Para X = %f, Y = %f",_intervectorX[i].get_var(),_intervectorY[i].get_var());
			if(_intervectorX[i].getsize_vec() > 1)
			{
				arrX.clear();
				arrY.clear();
				for(j=0; j<_intervectorX[i].getsize_vec(); j++)
				{
					arrX.push_back(_intervectorX[i].get_vec(j));
					arrY.push_back(_intervectorY[i].get_vec(j));
				}
//printf("\n arrX Size = %d",arrX.size());
				itx = arrX.begin();
				ity = arrY.begin();
				itxx = arrX.begin()+1;
				ityy = arrY.begin()+1;
				for(j=0; j<arrX.size()-1; j++)
				{
					// I
					if( (*itx > _intervectorX[i].get_var())  && (*ity < _intervectorY[i].get_var()) && 
						(*itxx > _intervectorX[i].get_var()) && (*ityy < _intervectorY[i].get_var()) )
					{
						distA = sqrt( pow(*itx-_intervectorX[i].get_var(),2) + pow(*ity-_intervectorY[i].get_var(),2) );
						distB = sqrt( pow(*itxx-_intervectorX[i].get_var(),2) + pow(*ityy-_intervectorY[i].get_var(),2) );
						if(distA<distB)
						{
							arrX.erase(itxx);
							arrY.erase(ityy);
						}
							if(distA>distB)
						{
							arrX.erase(itx);
							arrY.erase(ity);
							itxx++;
							ityy++;
						}	
					}
					// II
					else if( (*itx < _intervectorX[i].get_var())  && (*ity < _intervectorY[i].get_var()) && 
						(*itxx < _intervectorX[i].get_var()) && (*ityy < _intervectorY[i].get_var()) )
					{
						distA = sqrt( pow(*itx-_intervectorX[i].get_var(),2) + pow(*ity-_intervectorY[i].get_var(),2) );
						distB = sqrt( pow(*itxx-_intervectorX[i].get_var(),2) + pow(*ityy-_intervectorY[i].get_var(),2) );
						if(distA<distB)
						{
							arrX.erase(itxx);
							arrY.erase(ityy);
						}
						if(distA>distB)
						{
							arrX.erase(itx);
							arrY.erase(ity);
							itxx++;
							ityy++;
						}	
					}
					// III
					else if( (*itx < _intervectorX[i].get_var())  && (*ity > _intervectorY[i].get_var()) && 
						(*itxx < _intervectorX[i].get_var()) && (*ityy > _intervectorY[i].get_var()) )
					{
						distA = sqrt( pow(*itx-_intervectorX[i].get_var(),2) + pow(*ity-_intervectorY[i].get_var(),2) );
						distB = sqrt( pow(*itxx-_intervectorX[i].get_var(),2) + pow(*ityy-_intervectorY[i].get_var(),2) );
						if(distA<distB)
						{
							arrX.erase(itxx);
							arrY.erase(ityy);
						}
						if(distA>distB)
						{
							arrX.erase(itx);
							arrY.erase(ity);
							itxx++;
							ityy++;
						}	
					}
					// IV
					else if( ((double)*itx > _intervectorX[i].get_var())  && (*ity > _intervectorY[i].get_var()) && 
						((double)*itxx > _intervectorX[i].get_var()) && (*ityy > _intervectorY[i].get_var()) )
					{
						distA = sqrt( pow(*itx-_intervectorX[i].get_var(),2) + pow(*ity-_intervectorY[i].get_var(),2) );
						distB = sqrt( pow(*itxx-_intervectorX[i].get_var(),2) + pow(*ityy-_intervectorY[i].get_var(),2) );
						if(distA<distB)
						{
							arrX.erase(itxx);
							arrY.erase(ityy);
						}
						if(distA>distB)
						{
							arrX.erase(itx);
							arrY.erase(ity);
							itxx++;
							ityy++;
						}	
					}
					else
					{
						itx++;
						ity++;
						itxx++;
						ityy++;
					}
				}
				_intervectorX[i].resetVec();
				_intervectorY[i].resetVec();
//printf("\n _intervector(%d) Size = %d",i,_intervectorX[i].getsize_vec());
				for(k=0; k<arrX.size(); k++)
				{
//printf("\n arr(%d) X = %f, Y = %f",k,arrX[k],arrY[k]);
//					fprintf(fd,"\n => x_int = %f, y_int = %f",arrX[k],arrY[k]);
					_intervectorX[i].set_vec(arrX[k]);
					_intervectorY[i].set_vec(arrY[k]);
				}
			}
		}
	}

	//DELETE!!
//	delete vv;

}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::ControlInContour(std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ)
{
	int i,j;
	_contIncontpos.clear();	
	bool find = false;
	double range = 2;
	for(i=0; i<_controlpointsX.size(); i++)
	{
		find = false;
		for(j=0; (j<InX->size()) && (find!=true); j++)
		{
			if( ((*InX)[j]-range<=_controlpointsX[i]) && (_controlpointsX[i]<=(*InX)[j]+range) && ((*InY)[j]-range<=_controlpointsY[i]) && (_controlpointsY[i]<=(*InY)[j]+range) )
			{
				_contIncontpos.push_back(j);
				find = true;
			}
		}
	}
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::NearMaxError2Control()
{
	if(_interpointsX.size() != 0)
	{
		AddControlPoint(false);

		double distA = sqrt( pow(_interpointsX[_errorpos]-_controlpointsX[_posA],2) + pow(_interpointsY[_errorpos]-_controlpointsY[_posA],2) );
		double distB = sqrt( pow(_interpointsX[_errorpos]-_controlpointsX[_posB],2) + pow(_interpointsY[_errorpos]-_controlpointsY[_posB],2) );
		double nearp = -1;
		if(distA<distB)
		{
			nearp = distA;
			_posn = _posA;

		}
		else
		{
			nearp = distB;
			_posn = _posB;
		}
	}
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::MoveControlPointInContour(std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ)
{
int i;
/*
//PRINTF---------------------------------------------------------------
printf("\n CONTROL POINTS BEFORE MOVEMENT");
for(i=0; i<_controlpointsX.size(); i++)
{
	printf("\n X = %f, Y = %f",_controlpointsX[i],_controlpointsY[i]);
}
//---------------------------------------------------------------------
*/
	vectorFunctions *vf = new vectorFunctions();
	fixBetweenPoints(5.0);
	PossibleIntersections(InX,InY,InZ);
	IntersectionPoints();
	ErrorBetweenContours();
	//double promIn = vf->promVector(&_errorvector,false);

	std::vector<double> tempX;
	std::vector<double> tempY;
	std::vector<double> tempZ;
	tempX.clear();
	tempY.clear();
	tempZ.clear();
	
	vf->copyVector(&_controlpointsX,&tempX);
	vf->copyVector(&_controlpointsY,&tempY);
	vf->copyVector(&_controlpointsZ,&tempZ);
	_controlpointsX.clear();
	_controlpointsY.clear();
	_controlpointsZ.clear();

	for(i=0; i<tempX.size(); i++)
	{
		if(i==_posn)
		{
			_controlpointsX.push_back( (*InX)[_contIncontpos[_posn]] );
			_controlpointsY.push_back( (*InY)[_contIncontpos[_posn]] );
			_controlpointsZ.push_back( (*InZ)[_contIncontpos[_posn]] );
		}
		else
		{
			_controlpointsX.push_back( tempX[i] );
			_controlpointsY.push_back( tempY[i] );
			_controlpointsZ.push_back( tempZ[i] );
		}
	}

	fixBetweenPoints(5.0);
	PossibleIntersections(InX,InY,InZ);
	IntersectionPoints();
	ErrorBetweenContours();
	double promactualIn = vf->promVector(&_errorvector,false);
	double prom;
	double promactual;
	
	int posact;
	//int pos1;
	//double prom1final;
	int pos2;
	double prom2final;

	//DIRECTION 1
	int direction = 1;
	posact = _posn;
	prom = -1;
	promactual = promactualIn;
	_controlpointsX.clear();
	_controlpointsY.clear();
	_controlpointsZ.clear();
	for(i=0; promactual > prom; i++)
	{
		prom = promactual;
		for(i=0; i<tempX.size(); i++)
		{
			if(i==_posn)
			{
				_controlpointsX.push_back( (*InX)[_contIncontpos[posact]] );
				_controlpointsY.push_back( (*InY)[_contIncontpos[posact]] );
				_controlpointsZ.push_back( (*InZ)[_contIncontpos[posact]] );
			}
			else
			{
				_controlpointsX.push_back( tempX[i] );
				_controlpointsY.push_back( tempY[i] );
				_controlpointsZ.push_back( tempZ[i] );
			}
		}
		if(direction == 1)
		{
			posact = posact+1;
		}
		if(direction == -1)
		{
			posact = posact-1;
		}
		fixBetweenPoints(5.0);
		PossibleIntersections(InX,InY,InZ);
		IntersectionPoints();
		ErrorBetweenContours();
		promactual = vf->promVector(&_errorvector,false);
//printf("\n The point in the position %d, has moved %d times",_posn,i);
	}
	pos2 = posact;
	prom2final = promactual;

	delete vf;
}
//--------------------------------------------------------------------------------------------------------------------------------
double AutoControlPoints::MoveAndAverage(int dir, std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ)
{
	/*
//PRINTF---------------------------------------------------------------
	int i;
	printf("\n CONTROL POINTS BEFORE MOVEMENT");
	for(i=0; i<_controlpointsX.size(); i++)
	{
		printf("\n X = %f, Y = %f",_controlpointsX[i],_controlpointsY[i]);
	}
	for(i=0; i<_contIncontpos.size(); i++)
	{
		printf("\n contIncont pos = %d",_contIncontpos[i]);
	}
*/
//IMPLEMENTATION-------------------------------------------------------
	if( (_contIncontpos.size() != 0) && (_controlpointsX.size() != 0) )
	{
		vectorFunctions *vf = new vectorFunctions();
		std::vector<double>::iterator itx;
		std::vector<double>::iterator ity;
		std::vector<double> tempX;
		std::vector<double> tempY;
		tempX.clear();
		tempY.clear();
		vf->copyVector(&_controlpointsX,&tempX);
		vf->copyVector(&_controlpointsY,&tempY);
		int i,j /*,pos = 0*/ ;  // JPRx
		double prom1=0,promactual1=1;
		//double prom2=0,promactual2=1; // JPRx
		int h = 0;
		int hh = 1;
		if(_contIncontpos[h]>_contIncontpos[hh])
		{
			itx = _controlpointsX.begin();
			ity = _controlpointsY.begin();
			for(i=_contIncontpos[h],j=_contIncontpos[h]; (i>_contIncontpos[hh]) && (promactual1>prom1); i--)
			{
				if(j == InX->size())
				{
					j = 0;
				}
				prom1 = promactual1;
				*itx = (*InX)[j];
				*ity = (*InY)[j];
				printf("\n itx = %f, ity = %f", *itx,*ity);
				fixBetweenPoints(5.0);
				PossibleIntersections(InX,InY,InZ);
				IntersectionPoints();
				ErrorBetweenContours();
				promactual1 = vf->promVector(&_errorvector,false);
				j++;
			}
		}
		if(_contIncontpos[h]<_contIncontpos[hh])
		{
			itx = _controlpointsX.begin();
			ity = _controlpointsY.begin();
			for(i=_contIncontpos[h],j=_contIncontpos[h]; (i<_contIncontpos[hh]) && (promactual1>prom1); i++)
			{
				if(j == -1)
				{
					j = InX->size()-1;
				}
				prom1 = promactual1;
				*itx = (*InX)[j];
				*ity = (*InY)[j];
				printf("\n itx = %f, ity = %f", *itx,*ity);
				fixBetweenPoints(5.0);
				PossibleIntersections(InX,InY,InZ);
				IntersectionPoints();
				ErrorBetweenContours();
				promactual1 = vf->promVector(&_errorvector,false);
				j--;
			}
		}
		delete vf;
	}
	return 99999;
}
//--------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::MoveControlPoints(std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ)
{
	ControlInContour(InX,InY,InZ);
	NearMaxError2Control();
	MoveAndAverage(1,InX,InY,InZ);
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::GetNewPoints( std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ )
{
	vectorFunctions *vf = new vectorFunctions();
	double prom,maxerror;
	
	InterCircle(InX,InY,InZ);
	maxminLocal();
	fixBetweenPoints(5.0);
	PossibleIntersections(InX,InY,InZ);
	IntersectionPoints();
	ErrorBetweenContours();
	prom = vf->promVector(&_errorvector,false);
	vf->maxVector(&_errorvector,&maxerror);
printf("\n");
	if( maxerror>3.0 )
	{
		_controlpointsX.clear();
		_controlpointsY.clear();
		_controlpointsZ.clear();
		vf->copyVector(&_minmaxlocalX,&_controlpointsX);
		vf->copyVector(&_minmaxlocalY,&_controlpointsY);
		for(int i=0; i<_minmaxlocalX.size(); i++)
		{
			_controlpointsZ.push_back( (*InZ)[0] );
		}
		fixBetweenPoints(5.0);
		PossibleIntersections(InX,InY,InZ);
		IntersectionPoints();
		ErrorBetweenContours();
		prom = vf->promVector(&_errorvector,false);
		vf->maxVector(&_errorvector,&maxerror);
printf("\n");
	}

	std::vector<double> cpX;
	std::vector<double> cpY;
	std::vector<double> cpZ;
	cpX.clear();
	cpY.clear();
	cpZ.clear();
	vf->copyVector(&_controlpointsX,&cpX);
	vf->copyVector(&_controlpointsY,&cpY);
	vf->copyVector(&_controlpointsZ,&cpZ);
	int i;

	for(i=0; (i<10)&&(maxerror>0.5)&&(prom>0.15); i++ )
	{
		AddControlPoint(true);
		fixBetweenPoints(5.0);
		PossibleIntersections(InX,InY,InZ);
		IntersectionPoints();
		ErrorBetweenContours();
		prom = vf->promVector(&_errorvector,false);
		vf->maxVector(&_errorvector,&maxerror);
		printf("\n %d ",i);
	}

	if(i == 10)
	{
		_controlpointsX.clear();
		_controlpointsY.clear();
		_controlpointsZ.clear();
		int inicontrolpoints = cpX.size();
		double inipercentage = (inicontrolpoints*100)/InX->size();
		int h=0;
		if(inicontrolpoints<10)
		{
			int points = (int)((inipercentage*3*InX->size())/100);
			for (int i=0; i<InX->size(); i++, h++)
			{
				if( h == points )
				{
					_controlpointsX.push_back( (*InX)[i] );
					_controlpointsY.push_back( (*InY)[i] );
					_controlpointsZ.push_back( (*InZ)[i] );
					h = 0;
				}
			}
		}
		if(inicontrolpoints>=10)
		{
			int points = (int)((inipercentage*2*InX->size())/100);
			for (int i=0; i<InX->size(); i++, h++)
			{
				if( h == points )
				{
					_controlpointsX.push_back( (*InX)[i] );
					_controlpointsY.push_back( (*InY)[i] );
					_controlpointsZ.push_back( (*InZ)[i] );
					h = 0;
				}
			}
		}
	}
/*
	fixBetweenPoints(5.0);
	PossibleIntersections(InX,InY,InZ);
	IntersectionPoints();
	ErrorBetweenContours();
	prom = vf->promVector(&_errorvector,false);
	vf->maxVector(&_errorvector,&maxerror);


printf("\n Error Average  = %f",prom);
printf("\n Error Max  = %f",maxerror);
	AddControlPoint(false);
*/
/*
	//if( (prom>1) || (maxerror>2))
	if( prom>0.2 )
	{
printf("\n Error Average is grater than 1 !!");
		MoveControlPoints(InX,InY,InZ);
	}
*/
	delete vf;
}
//------------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::GetInitialNewPoints(std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ )
{
	vectorFunctions *vf = new vectorFunctions();
	double prom,maxerror;
	
	InterCircle(InX,InY,InZ);
	maxminLocal();
	fixBetweenPoints(5.0);
	PossibleIntersections(InX,InY,InZ);
	IntersectionPoints();
	ErrorBetweenContours();
	prom = vf->promVector(&_errorvector,false);
	vf->maxVector(&_errorvector,&maxerror);
	
	if( maxerror>3.0 )
	{
		_controlpointsX.clear();
		_controlpointsY.clear();
		_controlpointsZ.clear();
		vf->copyVector(&_minmaxlocalX,&_controlpointsX);
		vf->copyVector(&_minmaxlocalY,&_controlpointsY);
		for(int i=0; i<_minmaxlocalX.size(); i++)
		{
			_controlpointsZ.push_back( (*InZ)[0] );
		}
		fixBetweenPoints(5.0);
		PossibleIntersections(InX,InY,InZ);
		IntersectionPoints();
		ErrorBetweenContours();
		prom = vf->promVector(&_errorvector,false);
		vf->maxVector(&_errorvector,&maxerror);
	}

	std::vector<double> cpX;
	std::vector<double> cpY;
	std::vector<double> cpZ;
	cpX.clear();
	cpY.clear();
	cpZ.clear();
	vf->copyVector(&_controlpointsX,&cpX);
	vf->copyVector(&_controlpointsY,&cpY);
	vf->copyVector(&_controlpointsZ,&cpZ);

	double promini = prom; 

	int i;
	for(i=0; (i<10)&&(maxerror>0.5)&&(prom>0.15); i++ )
	{
		AddControlPoint(true);
		fixBetweenPoints(5.0);
		PossibleIntersections(InX,InY,InZ);
		IntersectionPoints();
		ErrorBetweenContours();
		prom = vf->promVector(&_errorvector,false);
		vf->maxVector(&_errorvector,&maxerror);
	}

	if( i==10 || prom > promini)
	{
		_controlpointsX.clear();
		_controlpointsY.clear();
		_controlpointsZ.clear();
		vf->copyVector(&cpX,&_controlpointsX);
		vf->copyVector(&cpY,&_controlpointsY);
		vf->copyVector(&cpZ,&_controlpointsZ);
	}
	delete vf;
}
//------------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::CalculeControlPoints(std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ)
{

	_controlpointsX.clear();
	_controlpointsY.clear();
	_controlpointsZ.clear();
	_controlpointsZ.push_back((*InZ)[0]);
	GetNewPoints( InX,InY,InZ );
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::CalculeInitialControlPoints(std::vector<double>*InX, std::vector<double>*InY, std::vector<double>*InZ)
{

	_controlpointsX.clear();
	_controlpointsY.clear();
	_controlpointsZ.clear();
	_controlpointsZ.push_back((*InZ)[0]);
	GetInitialNewPoints( InX,InY,InZ );
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::GetControlPoints(std::vector<double>*OutX, std::vector<double>*OutY, std::vector<double>*OutZ)
{
	vectorFunctions *vf = new vectorFunctions();
	OutX->clear();
	OutY->clear();
	OutZ->clear();
	vf->copyVector(&_controlpointsX,OutX);
	vf->copyVector(&_controlpointsY,OutY);
	vf->copyVector(&_controlpointsZ,OutZ);
	delete vf;
}
//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::GetInitialControlPoints(std::vector<double>*OutX, std::vector<double>*OutY, std::vector<double>*OutZ)
{
	vectorFunctions *vf = new vectorFunctions();
	OutX->clear();
	OutY->clear();
	OutZ->clear();
	vf->copyVector(&_controlpointsX,OutX);
	vf->copyVector(&_controlpointsY,OutY);
	vf->copyVector(&_controlpointsZ,OutZ);
	delete vf;
}

//-----------------------------------------------------------------------------------------------------------------------------------------
void AutoControlPoints::SetNumSplineInterpolation(int num)
{
	_numspline = num;
}
//-----------------------------------------------------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------------------------------------------------------