//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
#include "bbcreaMaracasVisuManualContourModel_Box.h"
#include "bbcreaMaracasVisuPackage.h"

#include <creaContoursFactory.h>


namespace bbcreaMaracasVisu
{

BBTK_ADD_BLACK_BOX_TO_PACKAGE(creaMaracasVisu,ManualContourModel_Box)
BBTK_BLACK_BOX_IMPLEMENTATION(ManualContourModel_Box,bbtk::AtomicBlackBox);
//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 


void ManualContourModel_Box::ProcessBySegment(	
			int Type, 
			int &iGeneral, int sizeSegment,
			std::vector<double> *lstInX,std::vector<double> *lstInY, std::vector<double> *lstInZ,
			std::vector<double> *lstOutX,std::vector<double> *lstOutY, std::vector<double> *lstOutZ,
			std::vector<int>	*lstIndexsOut, bool open )
{
	creaContoursFactory f;
	manualContourModel 	*m;
	int i,size=iGeneral+sizeSegment;
	double x,y,z;
	m = (manualContourModel*)f.getContourModel( bbGetInputType() );
	m->SetNumberOfPointsSpline( bbGetInputNbPoints() );
	m->SetCloseContour( open );
	for (i=iGeneral;i<size;i++)
	{
		m->AddPoint( (*lstInX)[i] , (*lstInY)[i] , (*lstInZ)[i] );
	} // for
	m->UpdateSpline();
	int sizeContour = bbGetInputNbPoints();
	for (i=0;i<sizeContour;i++)
	{
		m->GetSpline_i_Point(i,&x,&y,&z);
		lstOutX->push_back(x);
		lstOutY->push_back(y);
		lstOutZ->push_back(z);
	} // for
	iGeneral=iGeneral+sizeSegment;
	lstIndexsOut->push_back( sizeContour );
	delete m;
}


void ManualContourModel_Box::RedistributionPoints(	std::vector<double> *lstOutX,
													std::vector<double> *lstOutY, 
													std::vector<double> *lstOutZ,
													std::vector<int> *lstIndexsOut )
{
	std::vector<double> lstRstX;
	std::vector<double> lstRstY;
	std::vector<double> lstRstZ;
	int iLstIndexOut,sizeLstIndexOut=lstIndexsOut->size();
	int ii, iGeneral=0;
	int size;
	for (iLstIndexOut=0;  iLstIndexOut<sizeLstIndexOut; iLstIndexOut++)
	{
//printf("EED ManualContourModel_Box::RedistributionPoints iLstIndexOut=%d   \n", iLstIndexOut);
		lstRstX.clear();
		lstRstY.clear();
		lstRstZ.clear();
		size=(*lstIndexsOut)[iLstIndexOut];
		if (size>2)
		{
			double dist=0,dist2,distSeg,delta;
			double dd,dx,dy,dz;
			int i,k;
			for (i=iGeneral+1; i<iGeneral+size;i++)
			{
				dx=(*lstOutX)[i]-(*lstOutX)[i-1];
				dy=(*lstOutY)[i]-(*lstOutY)[i-1];
				dz=(*lstOutZ)[i]-(*lstOutZ)[i-1];
				dist = dist+sqrt( dx*dx + dy*dy + dz*dz );
			} //for
			delta=dist/(size-1);
			lstRstX.push_back( (*lstOutX)[iGeneral] );
			lstRstY.push_back( (*lstOutY)[iGeneral] );
			lstRstZ.push_back( (*lstOutZ)[iGeneral] );
			for (i=iGeneral+1; i<iGeneral+size;i++)
			{
//printf("EED ManualContourModel_Box::RedistributionPointsi=%d \n", i-iGeneral );

				dist2 = 0;
				for (k=iGeneral+1; k<iGeneral+size;k++)
				{
					dx=(*lstOutX)[k]-(*lstOutX)[k-1];
					dy=(*lstOutY)[k]-(*lstOutY)[k-1];
					dz=(*lstOutZ)[k]-(*lstOutZ)[k-1];
					distSeg = sqrt( dx*dx + dy*dy + dz*dz );
					ii=i-iGeneral;
					if ( dist2+distSeg>=ii*delta) 
					{
						dd=(ii*delta-dist2)/distSeg;
						if (distSeg==0)
						{
							dd=0;
						} // if distSeg == 0
						lstRstX.push_back( (*lstOutX)[k-1] + dd*dx );
						lstRstY.push_back( (*lstOutY)[k-1] + dd*dy );
						lstRstZ.push_back( (*lstOutZ)[k-1] + dd*dz );
						k=iGeneral+size;
					} else {
						if (k==iGeneral+size-1) 
						{
							dd=1;
							lstRstX.push_back( (*lstOutX)[k-1] + dd*dx );
							lstRstY.push_back( (*lstOutY)[k-1] + dd*dy );
							lstRstZ.push_back( (*lstOutZ)[k-1] + dd*dz );
//							printf("EED ManualContourModel_Box::RedistributionPoints iLstIndexOut=%d    i=%d k=%d  dist2+distSeg=%f    ii*delta=%f   dif=%f \n", iLstIndexOut,i-iGeneral, k-iGeneral, dist2+distSeg , ii*delta ,  (dist2+distSeg) - ii*delta);
						}
					}// if dist2 
					dist2=dist2+distSeg;
				} // for k
			} //for i   
			if (lstRstX.size()!=size) 
			{
				printf("EED Warnning!   ManualContourModel_Box::RedistributionPoints  >> This list is not coherent  iLstIndexOut=%d  lstRstX.size()=%d  size=%d\n",iLstIndexOut, lstRstX.size(), size);
			}
			for (i=iGeneral; i<iGeneral+size;i++)
			{
				ii=i-iGeneral;
				(*lstOutX)[i] = lstRstX[ii];
				(*lstOutY)[i] = lstRstY[ii];
				(*lstOutZ)[i] = lstRstZ[ii];
			} // for i
		} // if size>2
		iGeneral=iGeneral+size;
	}// for iLstIndexOut
}


void ManualContourModel_Box::ClockwisePoints(	std::vector<double> *lstInX,
											std::vector<double> *lstInY, 
											std::vector<double> *lstInZ,
											std::vector<int> *lstIndexsIn )
{
	int 	iLstIndexIn,sizeLstIndexIn=lstIndexsIn->size();
	int 	i,iGeneral=0;
	int 	size,size2;
	double 	cx,cy,cz;
	double 	px,py,pz;
	double 	backpx,backpy,backpz;
	double 	ang;
	char   	dir=-1;
	bool 	dirx,diry,dirz;
	int 	flagAng=0;
	float 	backang;
	double 	tmp;

	// For each contour
	for (iLstIndexIn=0;  iLstIndexIn<sizeLstIndexIn; iLstIndexIn++)
	{
		// Step 1. Find gravity center and direction
		size  	= (*lstIndexsIn)[iLstIndexIn];
		if (size>2)  // for contour with more than 2 points
		{
			cx		= 0;
			cy		= 0;
			cz		= 0;
			dirx	= true;
			diry	= true;
			dirz	= true;
			for ( i=0 ; i<size ; i++ )
			{
				px=(*lstInX)[iGeneral+i];
				py=(*lstInY)[iGeneral+i];
				pz=(*lstInZ)[iGeneral+i];
				cx	= px + cx;
				cy	= py + cy;
				cz	= pz + cz;
				if (i!=0) 
				{ 
					if (backpx!=px) { dirx=false; } 
					if (backpy!=py) { diry=false; } 
					if (backpz!=pz) { dirz=false; } 
					backpx=px;
					backpy=py;
					backpz=pz;
				} // if i!=0
				backpx=px; 
				backpy=py; 
				backpz=pz; 
			} // for i 
			cx=cx/size;
			cy=cy/size;
			cz=cz/size;
			if (dirx==true) { dir=1; }  // YZ 
			if (diry==true) { dir=2; }  // XZ 
			if (dirz==true) { dir=0; }  // XZ 
			// Step 2. Find angle diference find 
			flagAng=0;
			for ( i=0 ; i<size ; i++ )
			{
				px = (*lstInX)[iGeneral+i]-cx;
				py = (*lstInY)[iGeneral+i]-cy;
				pz = (*lstInZ)[iGeneral+i]-cz;
				if (dir==0) { ang=atan2( py , px ); } // XY
				if (dir==1) { ang=atan2( pz , py ); } // YZ
				if (dir==2) { ang=atan2( pz , px ); } // XZ
				if (i>0) 
				{ 
					if (backang<ang) 
					{
						flagAng++;
					} else {
						flagAng--;
					}// if backang<ang
				} // if i
				backang=ang; 
			} // for i 

printf("EED ManualContourModel_Box::ClockwisePoints cx=%f cy=%f cz=%f    flagAng=%d   size=%d \n",cx,cy,cz,flagAng, size);
//if (cy<=490) { flagAng = flagAng*(-1); }

			// Step 3. Invert order of points
			if (flagAng<0)
			{
				size2 = size/2;
				for ( i=0 ; i<size2 ; i++ )
				{
					tmp 							= (*lstInX)[iGeneral+i];
					(*lstInX)[iGeneral+i]			= (*lstInX)[iGeneral+size-1-i];
					(*lstInX)[iGeneral+size-1-i]	= tmp;
					tmp 							= (*lstInY)[iGeneral+i];
					(*lstInY)[iGeneral+i]			= (*lstInY)[iGeneral+size-1-i];
					(*lstInY)[iGeneral+size-1-i]	= tmp;
					tmp 							= (*lstInZ)[iGeneral+i];
					(*lstInZ)[iGeneral+i]			= (*lstInZ)[iGeneral+size-1-i];
					(*lstInZ)[iGeneral+size-1-i]	= tmp;
				} // for i
			} // flagAng
		} // size>2
		iGeneral = iGeneral+size;
	} // for iLstIndexIn
}


/*
void ManualContourModel_Box::ShiftValues(	std::vector<double> *lstInX,
											std::vector<double> *lstInY, 
											std::vector<double> *lstInZ,
											std::vector<int> *lstIndexsIn )
{
	int iLstIndexIn,sizeLstIndexIn=lstIndexsIn->size();
	int ii, iGeneral=0;
	int size,size2;
	double dist,distMin;
	int i,iBack;
	int ig;
	double dx,dy,dz;
	std::vector<double> LstTmpX;
	std::vector<double> LstTmpY;
	std::vector<double> LstTmpZ;
	if (sizeLstIndexIn>=2)
	{
	
		for (iLstIndexIn=0;  iLstIndexIn<sizeLstIndexIn-1; iLstIndexIn++)
		{
			size  = (*lstIndexsIn)[iLstIndexIn];
			size2 = (*lstIndexsIn)[iLstIndexIn+1];
			//find min distance and  iBack
			distMin = 10000000;
			iBack	= 0;
			
			
			for (ig=0; ig<size; ig++)
			{
				for ( i=0 ; i<size2 ; i++ )
				{
					dx	= (*lstInX)[iGeneral+ig]-(*lstInX)[iGeneral+size+i];
					dy	= (*lstInY)[iGeneral+ig]-(*lstInY)[iGeneral+size+i];
					dz	= (*lstInZ)[iGeneral+ig]-(*lstInZ)[iGeneral+size+i];
					dist= sqrt( dx*dx + dy*dy + dz*dz );
					if ( dist<distMin ) 
					{
						iBack	= i-ig;
						distMin	= dist;
					}
				} // for i size2
			} // for ig size

if (iBack<0) 
{
	printf("- "); 
	iBack = iBack + size2; 
}
			
printf("EED ManualContourModel_Box::ShiftValues px=%f py=%f pz=%f  iBack=%d/%d  size=%d  distMin=%f \n", (*lstInX)[iGeneral]  , (*lstInY)[iGeneral] , (*lstInZ)[iGeneral], iBack, size2,size, distMin);
			
			if (iBack!=0)
			{
				LstTmpX.clear();
				LstTmpY.clear();
				LstTmpZ.clear();
				for (i=0 ; i<size2 ; i++) 
				{
					ii= (i+iBack)%size2;
					LstTmpX.push_back( (*lstInX)[iGeneral+size+ii] );
					LstTmpY.push_back( (*lstInY)[iGeneral+size+ii] );
					LstTmpZ.push_back( (*lstInZ)[iGeneral+size+ii] );
				} // for i				
				for (i=0 ; i<size2 ; i++) 
				{
					(*lstInX)[iGeneral+size+i] = LstTmpX[i];
					(*lstInY)[iGeneral+size+i] = LstTmpY[i];
					(*lstInZ)[iGeneral+size+i] = LstTmpZ[i];
				} // for i				
			}
			iGeneral=iGeneral+size;
		} // for iLstIndexIn
		
//------------		
		
		iGeneral=0;

		for (iLstIndexIn=0;  iLstIndexIn<sizeLstIndexIn-1; iLstIndexIn++)
		{
			size  = (*lstIndexsIn)[iLstIndexIn];
			size2 = (*lstIndexsIn)[iLstIndexIn+1];
			//find min distance and  iBack
			distMin = 10000000;
			iBack	= 0;
			
			
//			for (ig=0; ig<size; ig++)
//			{
				for ( i=0 ; i<size2 ; i++ )
				{
					dx	= (*lstInX)[iGeneral]-(*lstInX)[iGeneral+size+i];
					dy	= (*lstInY)[iGeneral]-(*lstInY)[iGeneral+size+i];
					dz	= (*lstInZ)[iGeneral]-(*lstInZ)[iGeneral+size+i];
					dist= sqrt( dx*dx + dy*dy + dz*dz );
					if ( dist<distMin ) 
					{
						iBack	= i;
						distMin	= dist;
					}
				} // for i size2
//			} // for ig size

			
printf("EED ManualContourModel_Box::ShiftValues px=%f py=%f pz=%f  iBack=%d/%d  size=%d  distMin=%f \n", (*lstInX)[iGeneral]  , (*lstInY)[iGeneral] , (*lstInZ)[iGeneral], iBack, size2,size, distMin);
			
			if (iBack!=0)
			{
				LstTmpX.clear();
				LstTmpY.clear();
				LstTmpZ.clear();
				for (i=0 ; i<size2 ; i++) 
				{
					ii= (i+iBack)%size2;
					LstTmpX.push_back( (*lstInX)[iGeneral+size+ii] );
					LstTmpY.push_back( (*lstInY)[iGeneral+size+ii] );
					LstTmpZ.push_back( (*lstInZ)[iGeneral+size+ii] );
				} // for i				
				for (i=0 ; i<size2 ; i++) 
				{
					(*lstInX)[iGeneral+size+i] = LstTmpX[i];
					(*lstInY)[iGeneral+size+i] = LstTmpY[i];
					(*lstInZ)[iGeneral+size+i] = LstTmpZ[i];
				} // for i				
			}
			iGeneral=iGeneral+size;
		} // for iLstIndexIn
		
	} // if sizeLstIndexIn
	
}
*/



void ManualContourModel_Box::ShiftValues(	std::vector<double> *lstInX,
											std::vector<double> *lstInY, 
											std::vector<double> *lstInZ,
											std::vector<int> *lstIndexsIn )
{
	int iLstIndexIn,sizeLstIndexIn=lstIndexsIn->size();
	int ii, iGeneral=0;
	int size,size2;
	double dist,distMin;
	int i,iBack;
	int ig;
	double dx,dy,dz;
	std::vector<double> LstTmpX;
	std::vector<double> LstTmpY;
	std::vector<double> LstTmpZ;
	if (sizeLstIndexIn>=2)
	{
	
		for (iLstIndexIn=0;  iLstIndexIn<sizeLstIndexIn-1; iLstIndexIn++)
		{
			size  = (*lstIndexsIn)[iLstIndexIn];
			size2 = (*lstIndexsIn)[iLstIndexIn+1];
			//find min distance and  iBack
			distMin = 10000000;
			iBack	= 0;
			
			
			for (ig=0; ig<size; ig++)
			{
				dist=0;
				for ( i=0 ; i<size2 ; i++ )
				{
					dx	= (*lstInX)[iGeneral+i]-(*lstInX)[iGeneral+size+(i+ig)%size];
					dy	= (*lstInY)[iGeneral+i]-(*lstInY)[iGeneral+size+(i+ig)%size];
					dz	= (*lstInZ)[iGeneral+i]-(*lstInZ)[iGeneral+size+(i+ig)%size];
					dist= dist + sqrt( dx*dx + dy*dy + dz*dz );
				} // for i size2

				if ( dist<distMin ) 
				{
					iBack	= ig+size;
					distMin	= dist;
				}
				
			} // for ig size

			
printf("EED ManualContourModel_Box::ShiftValues px=%f py=%f pz=%f  iBack=%d/%d  size=%d  distMin=%f \n", (*lstInX)[iGeneral]  , (*lstInY)[iGeneral] , (*lstInZ)[iGeneral], iBack, size2,size, distMin);
			
			if (iBack!=0)
			{
				LstTmpX.clear();
				LstTmpY.clear();
				LstTmpZ.clear();
				for (i=0 ; i<size2 ; i++) 
				{
					ii= (i+iBack)%size2;
					LstTmpX.push_back( (*lstInX)[iGeneral+size+ii] );
					LstTmpY.push_back( (*lstInY)[iGeneral+size+ii] );
					LstTmpZ.push_back( (*lstInZ)[iGeneral+size+ii] );
				} // for i				
				for (i=0 ; i<size2 ; i++) 
				{
					(*lstInX)[iGeneral+size+i] = LstTmpX[i];
					(*lstInY)[iGeneral+size+i] = LstTmpY[i];
					(*lstInZ)[iGeneral+size+i] = LstTmpZ[i];
				} // for i				
			}
			iGeneral=iGeneral+size;
		} // for iLstIndexIn
		
		
	} // if sizeLstIndexIn
	
}






void ManualContourModel_Box::Process()
{
// THE MAIN PROCESSING METHOD BODY
//   Here we simply set the input 'In' value to the output 'Out'
//   And print out the output value
// INPUT/OUTPUT ACCESSORS ARE OF THE FORM :
//    void bbSet{Input|Output}NAME(const TYPE&)
//    const TYPE& bbGet{Input|Output}NAME() const 
//    Where :
//    * NAME is the name of the input/output
//      (the one provided in the attribute 'name' of the tag 'input')
//    * TYPE is the C++ type of the input/output
//      (the one provided in the attribute 'type' of the tag 'input')

//    bbSetOutputOut( bbGetInputIn() );
//    std::cout << "Output value = " <<bbGetOutputOut() << std::endl;

	// First Step  Spline Interpolation
	std::vector<double> lstInX=bbGetInputLstControlPointsX();
	std::vector<double> lstInY=bbGetInputLstControlPointsY();
	std::vector<double> lstInZ=bbGetInputLstControlPointsZ();
	if ( (lstInX.size()!=lstInY.size()) || (lstInY.size()!=lstInZ.size()) ) 
	{ 
		printf("Warnning !!  .. ManualContourModel_Box: The list X Y Z, no have the same number of elements \n");
		return;
	}
	std::vector<int> 	lstIndexsIn=bbGetInputLstIndexsIn();
	std::vector<int> 	lstIndexsOut;
	std::vector<double> lstOutX;
	std::vector<double> lstOutY;
	std::vector<double> lstOutZ;
	if (bbGetInputLstIndexsIn().size()==0)
	{
		lstIndexsIn.push_back( lstInX.size() );
	}
	if (bbGetInputDoubleContour()==1)
	{
		ClockwisePoints( &lstInX , &lstInY , &lstInZ , &lstIndexsIn );
//		ShiftValues( &lstInX , &lstInY , &lstInZ , &lstIndexsIn );
	} // DoubleContour
	int i,size=lstIndexsIn.size();
	int iGeneral=0;
	for (i=0;i<size;i++)
	{
		ProcessBySegment(	bbGetInputType() , 
							iGeneral,  lstIndexsIn[i] ,
						 	&lstInX ,  &lstInY  , &lstInZ,
						 	&lstOutX , &lstOutY , &lstOutZ,
							&lstIndexsOut,bbGetInputOpenClose() );
	} // for

	if (bbGetInputDoubleContour()==0)
	{
		//////////////////// Set Out   DoubleContour = 0
		bbSetOutputLstContourPointsX(lstOutX);	
		bbSetOutputLstContourPointsY(lstOutY);	
		bbSetOutputLstContourPointsZ(lstOutZ);
		bbSetOutputLstIndexsOut(lstIndexsOut);	
	} else {
		RedistributionPoints(&lstOutX,&lstOutY,&lstOutZ,&lstIndexsOut);
		ShiftValues( &lstOutX, &lstOutY, &lstOutZ, &lstIndexsOut );
		///////////// Second Step Transpose the vectors   
		lstInX.clear();
		lstInY.clear();
		lstInZ.clear();
		lstIndexsIn.clear();
		size  = bbGetInputNbPoints();
		int j,size2 = lstIndexsOut.size();
		for (i=0;i<size;i++)
		{
			for (j=0;j<size2;j++)
			{
				lstInX.push_back( lstOutX[ j*lstIndexsOut[j] + i ] );
				lstInY.push_back( lstOutY[ j*lstIndexsOut[j] + i ] );
				lstInZ.push_back( lstOutZ[ j*lstIndexsOut[j] + i ] );
			} // for j
			lstIndexsIn.push_back( size2 );
		} // for i
		lstOutX.clear();
		lstOutY.clear();
		lstOutZ.clear();
		lstIndexsOut.clear();
		///////////////////// Third step Interponation 2
		size=lstIndexsIn.size();
		iGeneral=0;
		for (i=0;i<size;i++)
		{
			ProcessBySegment(	bbGetInputType() , 
								iGeneral, lstIndexsIn[i] ,
							 	&lstInX,&lstInY,&lstInZ,
							 	&lstOutX,&lstOutY,&lstOutZ,
								&lstIndexsOut,bbGetInputOpenClose2());
		} // for
		RedistributionPoints(&lstOutX,&lstOutY,&lstOutZ,&lstIndexsOut);
		//////////////////// Forth step Transpose the vectors   
		lstInX.clear();
		lstInY.clear();
		lstInZ.clear();
		lstIndexsIn.clear();
		size  = bbGetInputNbPoints();
		size2 = lstIndexsOut.size();
		for (i=0;i<size;i++)
		{
			for (j=0;j<size2;j++)
			{
				lstInX.push_back( lstOutX[ j*lstIndexsOut[j] + i ] );
				lstInY.push_back( lstOutY[ j*lstIndexsOut[j] + i ] );
				lstInZ.push_back( lstOutZ[ j*lstIndexsOut[j] + i ] );
			} // for j
			lstIndexsIn.push_back( size2 );
		} // for i
		lstOutX.clear();
		lstOutY.clear();
		lstOutZ.clear();
		lstIndexsOut.clear();
		//////////////////// Set Out   DoubleContour = 1
		bbSetOutputLstContourPointsX(lstInX);	
		bbSetOutputLstContourPointsY(lstInY);	
		bbSetOutputLstContourPointsZ(lstInZ);
		bbSetOutputLstIndexsOut(lstIndexsIn);	

	} // if DoubleContour 

}
//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
void ManualContourModel_Box::bbUserSetDefaultValues()
{

//  SET HERE THE DEFAULT INPUT/OUTPUT VALUES OF YOUR BOX 
//    Here we initialize the input 'In' to 0
   bbSetInputType(1);
   bbSetInputDoubleContour(0);
   bbSetInputOpenClose(false);
   bbSetInputOpenClose2(false);
   bbSetInputNbPoints(100);
  
}
//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
void ManualContourModel_Box::bbUserInitializeProcessing()
{

//  THE INITIALIZATION METHOD BODY :
//    Here does nothing 
//    but this is where you should allocate the internal/output pointers 
//    if any 

  
}
//===== 
// Before editing this file, make sure it's a file of your own (i.e.: it wasn't generated from xml description; if so : your modifications will be lost)
//===== 
void ManualContourModel_Box::bbUserFinalizeProcessing()
{

//  THE FINALIZATION METHOD BODY :
//    Here does nothing 
//    but this is where you should desallocate the internal/output pointers 
//    if any
  
}
}
// EO namespace bbcreaMaracasVisu